OGIMOノート~家族のためのモノづくり~

OGIMOノート ~家族のためのモノづくり~

9歳の娘と、7歳の息子(脳性麻痺)を持った父親エンジニアの備忘録。自作の電子工作おもちゃ/リハビリ器具/ロボット関係の製作記録、勉強記録を残していきます

二輪走行ロボット「mBot」をWifiコントローラ(息子リハビリ用)で操作してみた

はじめに

前回は息子用モビリティを作成しましたが、そちらのブラッシュアップに現在取り組み中。特に、姿勢保持の課題に現在トライ中。

ogimotokin.hatenablog.com
ogimotokin.hatenablog.com

その一方で、ジョイスティックの操作感に不慣れ」という課題に対応するため、以下にしてジョイスティックの操作するモチベーションを上げる取り組みの必要性を感じました。
その操作練習のためにモビリティを使う手もありましたが、
 ・広くない我が家にて、操作練習のために常時モビリティを置いておくのが困難ために、同じ操作コントローラで動く小型二輪走行ロボットを作ってみました!

f:id:motokiinfinity8:20181220005017j:plain
f:id:motokiinfinity8:20181220005153j:plain



小型二輪走行ロボットはいくつか試してみたのですが、個人的に好きなのは
MakeBlock社のmBot です。

mBot robot kit - educational programmable robot | Makeblock®

子供のプログラミング教育用のロボットです。専用アプリでScratchベースの開発が出来る事に加えて、搭載されているマイコンArduinoベースのため、普通にArduinoベースで開発できる!
これを選んだ理由としては、
 ・二輪駆動が滑らかかつコンパクト
 ・筐体が頑丈で、多少の乱暴な扱いにも耐えられる
 ・見た目が可愛い (超音波センサーを目に見立てたデザインが子供ウケしそう)

といったとこ。

しかし、今回の私の作成した操作コントローラとは直接接続できない。
理由は、「無線モジュールが搭載されていない」点。
mBotの標準オプションとして、Blutooth搭載版はあるが、Classicのみ対応(BLE非対応)である。
一方で、今回私が使用しているジョイスティックはESP32モジュールのためBlutoothは対応しているのだが、現状世の中で普及しているソフトライブラリはBLEのため、そのままでは使うのは難しい。
BLE対応モジュールをmBotに搭載する事も検討したが、
「そもそもESP32間でどうやってペアリングするんだろう」
と悩みはじめたので、一旦これも保留。

結局 mBotに 無線対応モジュールとして最も小型化可能な ESP8266 を搭載する事にしました。

完成品


動画の通り、小型ロボットにした事で息子自身は自分の目線で動く事で親近感が沸いたらしく、それ以降はかなり積極的にジョイスティックを操作する様になりました!!
毎日使い続けようという工夫、だいじ!

しかし、前後の突起物は寝転んで遊ぶ息子には危なそうだなぁ。。
なので、あそこにはカバーが必要そう!という事で、早速3Dプリンターでカバーを設計。
既製品をベースにして、足りない部分は個々でカスタマイズ!

ただのカバーではなく、ちょっとした『遊びゴコロ』も忘れずに!
前側はカワイイお手て、そして後ろ側は……そりゃあカワイイおしりでしょ!!
という事でと無駄にモデリング頑張って、3Dプリンタで出力。

結果、なかなか愛嬌溢れた子になっちゃいました(笑)



f:id:motokiinfinity8:20181220005017j:plain



f:id:motokiinfinity8:20181220004951j:plain





その愛嬌ある見た目と、持ち運んでも壊れない頑丈さのおかげで、息子の通う療育園でも使ってもらえる様になりました!

息子の療育園お友達、普段硬直が激しくモノを触ることが出来ないのですが、ロボットの操作を行った時は硬直なく自分の意思で動かせたらしいです!
お母さんが涙流して喜んでくれたり、「うちにも欲しい」という話も頂いたりで、とても胸が熱くなりました!


ハードウェア構成

ハードウェア構成は以下。

f:id:motokiinfinity8:20181220005845j:plain

mBotに無線対応マイコンESP8266をUARTで接続するだけの構成。
ただし、mBotは5V系マイコン、ESP8266は3.3V系マイコンのため、両者間にレベル変換ICをはさむ必要があります。
レベル変換ICは以下のものを使用。
NTB0104 レベルシフタ ピッチ変換済みモジュール - スイッチサイエンス

そして、ESP8266を使ったもう一つの理由、
それは……頑張ってmBotマイコン筐体内に収める事
これにより子供のおもちゃとしての堅牢性を確保します!(大事)
そのため、具体的な回路図/レイアウトを検討してみた。

まず、接続方法から。

github.com

上記サイトからmBot側の回路図を見てみる。当初は、UART信号としてmBot側はDO端子(RXD)/D1端子(TXD)を使う予定だったが、これだとデバッグのために使うSerial(USB)とバッティングしてしまい開発上面倒くさい事になる。
私の開発スタイルは基本的に速度重視なので、Software SerialでUART信号をソフト対応する事にするため、任意のデジタル端子2系統に変更する事にした。

レイアウトを見ていると、mBot中央部にESP8266モジュールサイズ程度なら入りそうなスペースと、その隣接に5V+GND+デジタル2Pinが出力されていてピンヘッダを手付けすれば使えそうな端子があるので、ここに収まる基板サイズを作ればよさそうです。

f:id:motokiinfinity8:20181220234606j:plain
f:id:motokiinfinity8:20181220005328j:plainf:id:motokiinfinity8:20181220005344j:plain

作ったらこんな感じです。結構コンパクトに収める事ができました!
二つの基板間はピンヘッダメスで直接基板を刺す事で、配線をなくし、堅牢性を上げました。

外装は自由にカスタマイズで!

私はmBotの前後突起物が息子の目に刺さらないためのカバーに遊び心を加えたものをfusion360モデリング3Dプリンタで作りました

f:id:motokiinfinity8:20181220235322p:plain
f:id:motokiinfinity8:20181220071058j:plain


ソフトウェア構成

ソフトウェアに関して、ジョイスティックからの操作命令は先日に記載したモビリティと同じ仕様にしています。
ogimotokin.hatenablog.com

また、mBotのライブラリについては公式で用意されているので、以下を参考にしました。
github.com

一つ方針に悩んだのは、mBot / ESP8266 のどちらを本体頭脳にするのか、です。
ESP8266の方が処理性能は高いのですが、mBotの方が他のデバイス(ブザー/LED等)が接続されている事、今後の拡張性、メンテナンス性を考えると、mBotを本体頭脳に設定しました。(こうすれば別のロボットを作った際でも、ESP8266側のソフトはそのまま使いまわせるメリットを選択しました)

なので、ESP8266は、コントローラから無線経由で送信されたコマンド情報をUARTに変換するだけのシンプルな作りにしました。

また、mBot側は、今回
 ・ジョイスティック方向に応じてモータ制御を実施 (MeDCMotor.run関数)
 ・ブザー音で行動予告 (MeBuzzer.tone関数)
 ・カラーLEDで方向と色を紐づけ (MeRGBLed.setColor関数)

の機能も加えています。

全ソースは下記GitHubにも置いています。
(コード自体は汚いですが、あくまでラピッド開発的に開発しているので、とにかく自分での分かりやすさと使いまわしやすさを優先している感じです)
github.com

■mBot側(mBot_arduino.ino)

#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <MeMCore.h>

#define M_SPEED_L 6
#define M_DIR_L 7
#define M_SPEED_R 5
#define M_DIR_R 4
#define SW_UART_TX 11
#define SW_UART_RX 12

#define CMD_SIZE  12
#define STEP_PWM_1MS   80  //1秒ごとに増加するPWM値

SoftwareSerial softSerial(SW_UART_RX, SW_UART_TX);

double angle_rad = PI/180.0;
double angle_deg = 180.0/PI;

String recv_buffer;//
int i = 0;        // 文字数のカウンタ
bool inst_flg = 0;

String cmd_msg, vel_msg;
int vel_linear=0, vel_argular=0;
int motor_inst, motor_state ;
int running_mode; //0: 事前通知モード、1:事前予告なし(通常)モード
int cmd_start_time = 0;


MeBuzzer buzzer;
MeRGBLed rgbled_7(7, 7==7?2:4);
MeDCMotor motor_9(9);
MeDCMotor motor_10(10);
int Speed = 0;

void setup(){
  Serial.begin(19200);
  softSerial.begin(9600);
  motor_inst = 0;
  motor_state = -3;
  running_mode = 0;
  recv_buffer = "";
}

void loop(){
loop_start: 
  String cmdmsg = "";

  // Serial受信(内部バッファへのコピー)
  rcv_buf_update();
  
  // Serialバッファからコマンド取得
  cmdmsg = getCmdmsg();
  if(cmdmsg != ""){
    //Serial.print(cmdmsg);
    cmd_msg = (cmdmsg.substring(0,3));
    vel_msg = (cmdmsg.substring(3.7));
    running_mode = (cmdmsg.substring(7,8)).toInt();
  
    // MDC (Mobility Direction Control)の場合
    int motor_inst_tmp = motor_inst;
          
    if(cmd_msg.equals("MDC")){
        if((vel_msg.substring(0,2)).equals("-9")) motor_inst = -4;
        if((vel_msg.substring(0,2)).equals("+0")) motor_inst =  0;
        if((vel_msg.substring(0,2)).equals("+9")) motor_inst =  4;
        if((vel_msg.substring(2,4)).equals("-9")) motor_inst -= 1;
        if((vel_msg.substring(2,4)).equals("+9")) motor_inst += 1;
    }

    // レバー操作のチャタ防止対策 2回連続で同じコマンドの場合のみ有効にする
    if(motor_inst_tmp != motor_inst && motor_inst != 0 && running_mode == 0 ){
         delay(100);
         goto loop_start;
    }

    Serial.print("Motor Instructiont:"); 
    Serial.println(motor_inst);   
  }

  //モータ制御
  if(Time_Mesure(cmd_start_time) < 1000){
    Speed = 96;
  }else{
    Speed = (int)(96 + STEP_PWM_1MS * (float)(Time_Mesure(cmd_start_time)-1000) / 1000);
  }

  if(running_mode ==2){
    Speed = 96;
  }

  if(Speed > 255) Speed = 255;

  Serial.print("  ");
  Serial.println(Time_Mesure(cmd_start_time));
  Serial.print("  ");
  Serial.println(Speed);

  // Motor制御命令に変更があった場合に停止 & 音源再生
  if((motor_inst != motor_state) && motor_inst != 0 && running_mode == 0){
    Serial.println("Motor Instruction input!"); 
    // Motor停止
    motor_9.run(0);
    motor_10.run(0);
     switch(motor_inst){
          case  -5: //右後ろ
            (省略)
          case -4:  //後ろ
            (省略)
          case  -3: //左後ろ
            (省略)
          case  -1: //右 
           rgbled_7.setColor(1,0,255,255); //水色
           rgbled_7.setColor(2,0,0,0); //無色
           rgbled_7.show();
           buzzer.tone(659, 500);   //E5音階を500ms 
           break;
          case  1:  //左
           rgbled_7.setColor(1,0,0,0); //無色
           rgbled_7.setColor(2,0,255,0); //緑色
           rgbled_7.show();  
           buzzer.tone(440, 500);   //A4音階を500ms  
           break;
          case  3:  //右前 
            (省略)
          case  4:  //前
           rgbled_7.setColor(0,255,255,0); //黄色
           rgbled_7.show();  
           buzzer.tone(523, 500);   //C5音階を500ms 
           break;
          case  5:  //左前
            (省略)
        }  
  }
  
  int leftSpeed, rightSpeed;
  switch(motor_inst){
      case  -5: //右後ろ
        (省略)
      case -4:  //後ろ
        (省略)
      case  -3: //左後ろ 
        (省略)
      case  -1: //右
        leftSpeed = Speed;
        rightSpeed = -1*Speed;
        rgbled_7.setColor(1,0,255,255); //水色
        rgbled_7.setColor(2,0,0,0); //無色
        rgbled_7.show();  
        break;
      case  1:  //左
        leftSpeed = -1*Speed;
        rightSpeed = Speed;
        rgbled_7.setColor(1,0,0,0); //無色
        rgbled_7.setColor(2,0,255,0); //緑色
        rgbled_7.show();  
       break;
      case  3:  //右前
        (省略)
     case  4:  //前
        leftSpeed = Speed;
        rightSpeed = Speed;
        rgbled_7.setColor(0,255,255,0); //黄色
        rgbled_7.show();  
        break;
      case  5:  //左前
        (省略)
      case  0:  //なし
        leftSpeed = 0;
        rightSpeed = 0;
        rgbled_7.setColor(0,0,0,0); //無色
        rgbled_7.show();  
        break;
    }
    motor_9.run((9)==M1?-(leftSpeed):(leftSpeed));
    motor_10.run((10)==M1?-(rightSpeed):(rightSpeed));



  //情報更新
  delay(10);
  if(running_mode != 1){
    if(motor_inst != motor_state || motor_inst ==0 ) cmd_start_time = millis();  // 命令情報が変わった場合、時間リセット
  }else{
    if(abs(motor_inst-motor_state) >1 || motor_inst ==0 ) cmd_start_time = millis();  // 命令情報が前後で変わった場合、時間リセット
  }
  motor_state = motor_inst; //状態更新    
}


//受信用内部バッファの更新
void rcv_buf_update(){
  int size = softSerial.available();
  char c;
  int i;

  for(i=0; i<size; i++){
    c = softSerial.read();
    recv_buffer += c;
  }
}

//内部バッファからコマンド命令の検索・抽出
String getCmdmsg(){
  String result = "";
  bool incmd = false;
  int start_pos, end_pos = 0;
  int i;
  
  for( i=0; i<recv_buffer.length(); i++){
    if(incmd == false){
      // STX捜索
      if(recv_buffer.charAt(i) == 0x2){
        incmd = true;
        start_pos = i;
      }
    }else{
      // ETX捜索
      if(recv_buffer.charAt(i) == 0x3){
        incmd = false;
        end_pos = i;
        // メッセージ抽出 & メッセージより前のフレーム破棄
        result = recv_buffer.substring( start_pos+1, end_pos);
        recv_buffer = recv_buffer.substring(end_pos+1, recv_buffer.length());
        break;
      }
    }
  }
  return result;
}


■ESP8266側(mBot_esp8266.ino)

#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include <SoftwareSerial.h>

#define SerialPort Serial
#define SW_UART_TX 5
#define SW_UART_RX 4

#define CMD_SIZE  12

SoftwareSerial softSerial(SW_UART_RX, SW_UART_TX);


const char ssid[] = "ESP32_MOBILITY"; // SSID
const char password[] = "esp32_con";  // password
const int WifiPort = 8000;      // ポート番号

const IPAddress HostIP(192, 168, 11, 1);       // IPアドレス
const IPAddress ClientIP(192, 168, 11, 2);       // IPアドレス
const IPAddress subnet(255, 255, 255, 0); // サブネットマスク
const IPAddress gateway(192,168, 11, 0);
const IPAddress dns(192, 168, 11, 0);
bool wifi_connect;

WiFiUDP Udp;
uint8_t WiFibuff[CMD_SIZE];
int motor_inst, motor_state ;
String cmd_msg, vel_msg;
int vel_linear=0, vel_argular=0;
int running_mode; //0: 事前通知モード、1:事前予告なし(通常)モード


void setup() 
{
  //PIN設定
  Serial.begin(9600);
  softSerial.begin(9600);
  motor_inst = 0;
  running_mode = 0;
  
  // Wifi設定 (SSID&パス設定)
  wifi_connect = false;
  WiFi.mode(WIFI_AP);
  WiFi.softAP(ssid, password);
  delay(100);
  WiFi.softAPConfig(HostIP, HostIP, subnet); // IPアドレス設定

  Serial.print("AP IP address: ");
  IPAddress myAddress = WiFi.softAPIP();
  Serial.println(myAddress);

  Udp.begin(WifiPort);  // UDP通信開始
  Serial.println("Starting UDP");
  
  Serial.print("Local port: ");
  Serial.println(WifiPort);

}

void loop() {

loop_start:

  int packet_size = Udp.parsePacket();

  if (packet_size > 0) {
    //Serial.println("Serial Received");   
    rcvWiFi();    
    if(WiFibuff[0] == 0x2){
          String s_buf = String((char*)WiFibuff);
          Serial.write(WiFibuff, CMD_SIZE);
          softSerial.write(WiFibuff, CMD_SIZE);   
      }
  }

void rcvWiFi() {
  Udp.read(WiFibuff, CMD_SIZE);
  //Serial.print(WiFibuff);
  Udp.flush();
}

おわりに

今回頑張って mBotの筐体内に無線モジュールを入れ込んだおかげで、頑丈になっており、息子の療育園への運搬などで壊れる心配もなく
かなり安心して使ってもらえる様になりました! 子供って、本当に色々な使い方をするので、出来る限り既製品の外装部をうまく利用していくやり方がよいのかなぁ、と感じてます!

モビリティMottoyと同じ通信IFにしてありますので、これから新しいコントローラを作ったら「モビリティ」も「小型ロボット」もどちらも動かす事ができるので、色々と応用が広がりそうです!

療育園のお母さんからも「是非欲しい!」というお声を頂いたのですが、残念ながら現状だとなかなか高いお値段になってしまいます。(mBotだけで1万円以上しますので…)
ただ、mBotだと今回の用途にはオーバースペックなので、療育園リハビリ用にはもっと簡単に&安く作れる手段を検討していきたいと思います。