ヘボコン2018に参戦したロボットに搭載したシステム(2018.07.28更新)：備忘録として

Arduino ESP8266 ESP32 M5Stack mDNS Wifi WebSocket hebocon ヘボコン

息子がデイリーポータルZ主催のヘボコン2018(2018年6月30日、於東京カルチャーカルチャー)に参戦しました。

息子のロボットにはArduinoベースのリモートコントロール(Wifi)システムを搭載してます。

 

システム構成

  • ロボット側 ESP8266(ESPr Developer)
  • コントローラ ESP32(M5Stack Gray)
  • Wifi親機 iPhoneテザリング(2018.07.28削除)

基本動作

  1. ESP8266, M5StackともWifi親機に接続。ESP8266をWifi親機、M5StackをWifi子機として接続。(2018.07.28更新)
  2. mDNSによりM5StackはESP8266のIPアドレスを取得。
  3. WebSocketによりM5StackからESP8266に指示を伝える。

2018.07.28更新

ロボット搭載システムは、以前クラウド・サービス(Twilio)を使っていたものを流用したためiPhoneにテザリング接続する作りでしたが、ESP8266をWifi親機としてM5Stackが直接接続するように変更しました。

ESP8266が制御するロボットの動き

  1. 熊の生首(ポンポン)
    高輝度LEDを点灯させ、サーボモータにより首を左右に振らせます。
  2. プリズムの回転
    PWMで回転速度を制御します。
    超低速で回転させたかったので、周波数を低くしてます。そのためぎこちない動きとなってしまいました。

おまけの機能

E5Stack Grayは加速度センサー搭載なので、躍度(加加速度)が閾値を超えた時にブザーが鳴る機能を付けました。音が大きいので鳴るとびっくりするという意味不明の機能です。

回路とスケッチ

息子はArduinoスターターキットとGenuinoスターターキットを持っています(ヘボコン・ワールドチャンピオンシップ2016のArduino賞を受賞した時の副賞です)。

モータをPWMでドライブする回路のスイッチング素子として、最初はスターターキットに入っていたIRF520Nを流用したのですが、出力電圧3.3VのESP8266ではゲートターンオンできませんでした。そこで検索してデータシートを調べ、2SK2232というパワーMOS FETを購入し代わりに使いました。

回路図

f:id:tsun226:20180625214330p:plain

ESP8266スケッチ(2018.07.29更新)

#ifdef ESP8266
extern "C" {
#include "user_interface.h"
}
#endif

#include <ESP8266WiFi.h>
#include <ESP8266mDNS.h>
#include <WebSocketsServer.h>
#include <Servo.h>

 

// Wi-Fi parameters
const char* ssid = "{SSID}";
const char* pass = "{PASSWORD}";
const IPAddress ip_address({Wifi Station IP address});  // 2018.07.28 added
const IPAddress ip_subnet({Subnet mask});                   // 2018.07.28 added
const char* hostString = "{HOST NAME}";

 

#define LED1 12
#define LED2 14
#define IO_SERVO 16
#define IO_MOTOR 13

#define BEAR_RESET 0
#define BEAR_START 1
#define BEAR_STOP 2
#define BEAR_LOOP 3
#define BEAR_SWING 50 // bear swinging degree

#define MOTOR_RESET 0
#define MOTOR_UP 1
#define MOTOR_DOWN 2
#define MOTOR_KEEP 3
#define MOTOR_OFF 4
#define MOTOR_LOOP 5
#define MOTOR_MAX 1000 // 0 - 1023?
#define MOTOR_ACCELL 2
#define MOTOR_PWM_FREQ 10

 

// WebSocket Server
void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length);
WebSocketsServer webSocket = WebSocketsServer(80);

 

// Servo
Servo servo;

 

// Setup
void setup() {
  Serial.begin(115200);
  Serial.println("Setup.");
  wifiConnection();
  setupMDNSServer();
  setupWebSocketServer();
  bear(BEAR_RESET);
  motorControl(MOTOR_RESET);
}

 

// Loop
void loop() {
  if(WiFi.status() == WL_CONNECTED || wifiConnection() ||       setupMDNSServer() || setupWebSocketServer()) {
    webSocket.loop();
  } else {
    Serial.println("NO WORK");
    delay(10);
  }
  bear(BEAR_LOOP);
  motorControl(MOTOR_LOOP);
}

 

// WiFi 2018.07.28 modified
boolean wifiConnection() {
  WiFi.mode(WIFI_AP);
  WiFi.softAPConfig(ip_address, ip_address, ip_subnet);
  WiFi.softAP(ssid, pass);
  return(true);
}

 

// mDNS Server
bool setupMDNSServer() {
  if (!MDNS.begin(hostString, WiFi.localIP())) {
    Serial.println("Error setting up MDNS responder!");
    return(false);
  }
  Serial.println("mDNS responder started");
  MDNS.addService("ws", "tcp", 80);
  return(true);
}

 

// WebSocket Server
bool setupWebSocketServer() {
  webSocket.begin();
  webSocket.onEvent(webSocketEvent);
  return(true);
}

 

// WebSocket Event Prosessing
void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length) {
  char buff[4];
  int len;

 

  switch (type) {
  case WStype_DISCONNECTED:
    Serial.println("WS disconnected.");
    break;
  case WStype_CONNECTED:
    Serial.println("WS connected.");
    break;
  case WStype_TEXT:
    len = min(3, (int)length);
    for(int i = 0; i < len; i++) {
      buff[i] = (char)payload[i];
    }
    buff[len] = '\0';
    Serial.println("WS: Text Received: " + String(buff));
    parseRequest(buff);
    break;
  }
}

 

void parseRequest(char *buff) {
  static boolean bear_status = false;
  static boolean motor_status = false;


  if(buff[0] == '1') { // button A pressed
    bear_status = ! bear_status;
    if(bear_status) {
      bear(BEAR_START);
    } else {
      bear(BEAR_STOP);
    }
  }
  if(buff[1] == '1') { // button B pressed
    motor_status = ! motor_status;
    if(motor_status) {
      motorControl(MOTOR_UP);
    } else {
      // motorControl(MOTOR_DOWN);
      motorControl(MOTOR_OFF);
    }
  }
  if(buff[1] == '2' && motor_status) { // button B released
    motorControl(MOTOR_KEEP);
  }
}

 

void bear(int command) {
  static int mode = 0; // 0:off, 1:on
  static int status = 0; // 0:foward -> 1:left -> 2:forward -> 3:right
  const int deg[] = {90, 90 - BEAR_SWING, 90, 90 + BEAR_SWING};
  static unsigned long last = 0L;


  switch(command) {
  case BEAR_RESET:
    Serial.print("Bear RESET: ");
    pinMode(LED1, OUTPUT); pinMode(LED2, OUTPUT);
    digitalWrite(LED1, HIGH); digitalWrite(LED2, HIGH);
    servo.attach(IO_SERVO);
    delay(500);
    // no break
  case BEAR_STOP:
    Serial.println("Bear STOP");
    servo.write(deg[status]);
    digitalWrite(LED1, LOW); digitalWrite(LED2, LOW);
    mode = 0;
    status = 0;
    servo.write(deg[status]);
    break;
  case BEAR_START:
    Serial.println("Bear START");
    mode = 1;
    status = 0;
    last = millis();
    digitalWrite(LED1, HIGH); digitalWrite(LED2, HIGH);
    break;
  case BEAR_LOOP:
    if(! mode || last + 2000L > millis()) {
      break;
    }
    last = millis();
    status = (status + 1) % 4;
    Serial.print("Bear angle "); Serial.println(deg[status]);
    servo.write(deg[status]);
    break;
  }
}


void motorControl(int command) {
  static int status = 0; // 0:off, 1:up, 2:keep, 3:down
  static int speed = 0;
  static unsigned long last = 0L;


  switch(command) {
  case MOTOR_RESET:
    Serial.print("Motor RESET: ");
    pinMode(IO_MOTOR, OUTPUT);
    analogWriteFreq(MOTOR_PWM_FREQ);
    analogWriteRange(MOTOR_MAX);
    // no break
  case MOTOR_OFF:
    status = 0;
    speed = 0;
    analogWrite(IO_MOTOR, speed);
    Serial.println("Motor OFF: ");
    break;
  case MOTOR_UP:
    Serial.println("Motor UP");
    status = 1;
    speed += MOTOR_ACCELL;
    last = millis();
    analogWrite(IO_MOTOR, speed);
    break;
  case MOTOR_DOWN:
    Serial.println("Motor DOWN");
    status = 3;
    speed -= MOTOR_ACCELL;
    last = millis();
    analogWrite(IO_MOTOR, speed);
    break;
  case MOTOR_KEEP:
    Serial.println("Motor KEEP");
    status = 2;
    break;
  case MOTOR_LOOP:
    if(status == 0 || status == 2 || last + 300L > millis() || speed <= 0 || speed >= MOTOR_MAX) {
      break;
    }
    if(status == 1) {
      speed += MOTOR_ACCELL;
    } else if(status == 3) {
      speed -= MOTOR_ACCELL;
    }
    Serial.print("Motor ACCELL "); Serial.println(speed);
    last = millis();
    analogWrite(IO_MOTOR, speed);
    break;
  }
}

 M5Stackのスケッチ

#include <Arduino.h>

#include <M5Stack.h>
#include <WiFi.h>
#include <WiFiMulti.h>
#include <WiFiClientSecure.h>
#include <ESPmDNS.h>
#include <WebSocketsClient.h>
#include "utility/MPU9250.h"
#include "utility/quaternionFilters.h"

 

WiFiMulti WiFiMulti;
WebSocketsClient webSocket;

 

// Wi-Fi parameters
const char* ssid = "{SSID}";
const char* pass = "{PASSWORD}";
const char* wsHost = "{ESP8266 HOST NAME}";
const char* hostString = "{MY HOSTNME}";
IPAddress esp8266Ip;

 

// MPU9250
#define AHRS true // Set to false for basic data read
MPU9250 IMU;

#define JERK_THR 700.0f
#define BEEP_FREQ 880
#define BEEP_TIME 500
#define TSIZE_S 2
#define TSIZE_ST 3
#define TSIZE_L 7

boolean mpu2950_active = true;
boolean network_active = true;
boolean beep_mode = false;

 

// Setup
void setup() {
  Serial.begin(115200);
  Serial.println("Setup.");
  M5.begin();
  M5.Lcd.setTextSize(TSIZE_S);
  M5.Lcd.fillScreen(0); M5.Lcd.setTextColor(WHITE, 0);   M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("Setup.");
  delay(1000);
  if(M5.BtnA.isPressed()) { // Button B -> network disable
    network_active = false;
    M5.Lcd.println("Network disabled.");
  }
  if(M5.BtnB.isPressed()) { // Button A -> mpu2950 disable
    mpu2950_active = false;
    M5.Lcd.println("MPU2950 disabled.");
  }
  if(mpu2950_active) {
    setupMPU9250();
  }
  if(network_active) {
    wifiConnection();
    retrieveEsp8266Ip();
    setupWebSocketClient();
  }
  display_hebocon();
}

 

// Loop
void loop() {
  if(mpu2950_active) {
    checkMPU9250();
  }
  if(network_active) {
    if(WiFiMulti.run() == WL_CONNECTED || wifiConnection() ||           setupWebSocketClient()) {
      webSocket.loop();
      sendStatus();
    } else {
      Serial.println("NO WORK");
      M5.Lcd.fillScreen(RED); M5.Lcd.setTextColor(WHITE, RED);       M5.Lcd.setCursor(0, 0);
      M5.Lcd.println("NO WORK");
    }
  }
}

 

// MPU2950
void setupMPU9250() {
  Wire.begin();
  byte c = IMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
  Serial.print("MPU9250: I AM ");
  Serial.print(c, HEX);
  Serial.print(" : ");
  Serial.println(0x71, HEX);

  // Start by performing self test and reporting values
  IMU.MPU9250SelfTest(IMU.SelfTest);

  // Calibrate gyro and accelerometers, load biases in bias registers
  IMU.calibrateMPU9250(IMU.gyroBias, IMU.accelBias);
  IMU.initMPU9250();
}

 

void checkMPU9250() {
  static float lax = 0.0f, lay = 0.0f, laz = 0.0f;
  float dax, day, daz, accl;

 

  if (IMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
    IMU.readAccelData(IMU.accelCount); // Read the x/y/z adc values
    IMU.getAres();

    // Now we'll calculate the accleration value into actual g's
    // This depends on scale being set
    IMU.ax = (float)IMU.accelCount[0]*IMU.aRes; // - accelBias[0];
    IMU.ay = (float)IMU.accelCount[1]*IMU.aRes; // - accelBias[1];
    IMU.az = (float)IMU.accelCount[2]*IMU.aRes; // - accelBias[2];
  } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)

  // Must be called before updating quaternions!
  IMU.updateTime();

  IMU.delt_t = millis() - IMU.count;
  if (IMU.delt_t > 100) {
    // Print acceleration values in milligs!
    Serial.print("X-acceleration: "); Serial.print(1000*IMU.ax); Serial.print(" (");     Serial.print(1000 * lax);
    Serial.print(" mg ");
    Serial.print("Y-acceleration: "); Serial.print(1000*IMU.ay); Serial.print(" (");     Serial.print(1000 * lay);
    Serial.print(" mg ");
    Serial.print("Z-acceleration: "); Serial.print(1000*IMU.az); Serial.print(" (");     Serial.print(1000 * laz);
    Serial.println(" mg ");
    if(lax != 0.0f && lay != 0.0f && laz != 0.0f) { // not initial
      dax = IMU.ax - lax;
      day = IMU.ay - lay;
      daz = IMU.az - laz;
      accl = sqrt*1 * 1000;
      Serial.print("JERK: "); Serial.println(accl);
      M5.Lcd.setCursor(0, 210);
      M5.Lcd.print("JERK: ");
      M5.Lcd.print(accl);
      if(beep_mode) {
        M5.Lcd.print(" B");
        if(accl > JERK_THR) {
          M5.Lcd.print("EEP ");
          M5.Speaker.tone(BEEP_FREQ, BEEP_TIME);
        }
        M5.Lcd.println(" ");
      } else {
        M5.Lcd.println(" ");
      }
      IMU.count = millis();
    }
    lax = IMU.ax;
    lay = IMU.ay;
    laz = IMU.az;
  }
}


// WiFi
boolean wifiConnection() {
  WiFiMulti.addAP(ssid, pass);
  int count = 0;
  Serial.print("Connecting Wi-Fi");
  M5.Lcd.print("Connecting Wi-Fi");
  while (count < 50) {
    if (WiFiMulti.run() == WL_CONNECTED) {
      String mssg = "Connected.\nC: " + String(hostString) + "\nIP: " +       WiFi.localIP().toString();
      Serial.println();
      Serial.println(mssg);
      M5.Lcd.fillScreen(BLUE); M5.Lcd.setTextColor(WHITE, BLUE);       M5.Lcd.setCursor(0, 0);
      M5.Lcd.println(mssg);
      return(true);
    }
    delay(500);
    Serial.print(".");
    M5.Lcd.print(".");
    count++;
  }
  Serial.println("Timed out.");
  M5.Lcd.fillScreen(RED); M5.Lcd.setTextColor(WHITE, RED);   M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("WiFi connecting timed out.");
  return(false);
}

 

// retrieve ESP8266 IP address by mDNS
void retrieveEsp8266Ip() {
  if (!MDNS.begin(hostString)) {
    Serial.println("Error setting up MDNS responder!");
    M5.Lcd.println("Error setting up MDNS responder!");
  }
  Serial.println("mDNS started.");
  for(int i = 0; i < 10; i++) {
    esp8266Ip = MDNS.queryHost(wsHost);
    Serial.println("mDNS res = " + esp8266Ip.toString());
    if(esp8266Ip[0] != 0) {
      Serial.println("OK");
      M5.Lcd.println("S: " + esp8266Ip.toString());
      return;
    }
    delay(500);
  }
  Serial.println("ESP8266 not found.");
  M5.Lcd.fillScreen(RED); M5.Lcd.setTextColor(WHITE, RED);   M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("ESP8266 not found.");
}


// WebSocket Client
bool setupWebSocketClient() {
  webSocket.begin(esp8266Ip.toString(), 80, "/");
  webSocket.onEvent(webSocketEvent);
  webSocket.setReconnectInterval(5000);
  Serial.println("WebSocket acrivated.");
  M5.Lcd.println("WS activated.");
  return(true);
}


// WebSocket Event Prosessing
void webSocketEvent(WStype_t type, uint8_t * payload, size_t length) {
  switch (type) {
  case WStype_DISCONNECTED:
    Serial.println("WS disconnected.");
    break;
  case WStype_CONNECTED:
    Serial.println("WS connected.");
    break;
  case WStype_TEXT:
    String payload_str = String*2 {
    return;
  }
  last = millis();

  if(M5.BtnA.wasPressed()) { // Button A
    st[0] = '1';
  }
  if(M5.BtnA.wasReleased()) {
    st[0] = '2';
  }
  if(M5.BtnB.wasPressed()) { // Button B
    st[1] = '1';
  }
  if(M5.BtnB.wasReleased()) {
    st[1] = '2';
  }
  if(M5.BtnC.wasPressed()) { // Button C
    st[2] = '1';
    beep_mode = ! beep_mode;
  }
  if(M5.BtnC.wasReleased()) {
    st[2] = '2';
  }
  if(! String(st).equals("000")) {
    webSocket.sendTXT(st);
    Serial.println("Sent: " + String(st));
    M5.Lcd.setCursor(0, 170);
    M5.Lcd.println("Sent: " + String(st));
  }
  M5.update();
}

 

// display HEBOCON LOGO on LCD
void display_hebocon() {
  delay(500);
  M5.Lcd.setBrightness(0);
  M5.Lcd.fillScreen(YELLOW);
  M5.Lcd.setTextSize(TSIZE_L);
  M5.Lcd.setTextColor(BLUE, YELLOW);
  M5.Lcd.setCursor(20, 20);
  M5.Lcd.println("HEBOCON");
  M5.Lcd.println(" 2018");
  M5.Lcd.setTextSize(TSIZE_ST);
  delay(100);
  M5.Lcd.setBrightness(100);
  delay(400);
  M5.Lcd.setBrightness(0);
  delay(400);
  M5.Lcd.setBrightness(100);
  delay(400);
  M5.Lcd.setBrightness(0);
  delay(400);
  M5.Lcd.setBrightness(100);
}

 

*1:dax * dax) + (day * day) + (daz * daz

*2:char*) payload);
    Serial.println("WS: Text Received: " + payload_str);
    M5.Lcd.setCursor(0, 202);
    M5.Lcd.println("Received: " + payload_str);
    parseRequest(payload_str);
    break;
  }
}

 

void parseRequest(String payload_str) {
}

 

void sendStatus() {
  static unsigned long last;
  char st[4] = "000"; // BtnA,BtnB,BtnC

  if(last + 10 > millis(