今回は、「M5Stack Japan Creativity Contest 2021」に参加するべく、M5Stack製品を利用して、大きな四足歩行ロボットを作るとともに、各種センサーを搭載して、リアルタイムで動き回りながら計測結果を表示するローバーを作成してみた。
M5Stack社の製品は、サーボを動かすにも、多様な各種センサーが簡単に利用できることが特徴である。しかも、ここでは中でも最小なAtom liteとAtom matrix(IMU内臓)を利用して簡単に作れることを示す。

###やったこと
・ロボットの設計・組立
・ロボットをM5Atomで動かす
・IoTセンサーを利用した計測
・M5CameraX画像のWebServerリアルタイム表示
・結果
###・ロボットの設計・組立
まず、大きなロボットを作る前に、小さな四本足のロボットを作成した。
これは、参考のようなものである。
【参考】
①【四足歩行ロボット入門】歩いた・走った・後退した！！
参考を読んでもらうと分かるが、簡単に四足歩行させるには、以下の部分が肝心である。

1.周期的かつ引き足な動きをさせる
2.前足同士は、ほぼ人間と同じように交互に動かす
3.前後の足は、位相差をつける
4.前後の足の位相差は最適な位相差がある
5.動作の振幅と位相差は、PS3コントローラーで決定できる

6.後退は前進の足の動作は、前進が①②③④として、後退は④③②そして①に置き換えると対象的な動作として後退が出来る
7.旋回は、右だけ動かせば左に、左だけ動かせば右に旋回する。ただし、若干動く方の足が前方に位置するように基準点つまり出力０点を変える
8.前進・後退においても、それぞれの足の基準点の最適解がありそうであるが、今は前進は後ろ傾斜、後退は上述したような配置にしているが、最適解にたどり着いてはいない。
大まかに動ける位置で動かしている
9.小さいロボットでは、走るを作ったが、大きなロボットではサーボの出力がえられず今回は断念した
####組立
本体部品は、通常の二足歩行ロボットの部品を流用した。
Servoモーターは、DS3218という上記の部品にフィットする180度程度の回転角を有するものをMap関数-90度から90度で利用した。
天板は、１００円ショップの２０x３０cmの木板を利用した。
一方、それ以外に以下のものを利用している。
####センサー類
M5Stack社の製品等から
1.ENV III 温度・湿度・気圧センサー；計測する
2.ToF距離センサー；前方の障害物までの距離を計測する
3.M5CameraX；WebServerのURLに表示させ前方監視とOLEDを読み取る
4.OLED；センサー類の計測結果を表示する
5.Hub２個　センサー類を一つのAtomのGroveでコントロールするために導入
6.Atom liteセンサー計測用；センサーを動かし取得した値をOLEDに表示する
7.Atom matrix　Servo制御用;内臓IMU搭載
（今回は、roll, pitch, yaw表示のみ）

以下、その他の部品
8.PS３コントローラー；ロボットの前進・後退・旋回と歩幅を制御する
9.DC-DCコンバーター２個
10.PCA９８６５ 16chServoコントローラー
11.LIPO１S（約4v）；二個；センサー電源供給(Atom liteのGroveより)及び制御用Atom matrixの電源供給
12.LIPO４S（約16v）2個；DC-DCコンバーターで６v減圧してServo電源供給

####Servo駆動について
結論から書くと、
付け根側4個のServoは、PCA9865で制御し、残り4個はAtomのPWM出力で直接制御している
これは、Servo制御にパワーが必要であり、一個のPCA9865で供給しようとしたが、トルクが必要な部分で、制御不能が頻発するために、上記の選択をした。
※将来的にはM5Stack Coreで、LIFE9.9V供給、Servo2を２個利用することを考えている
###・ロボットをM5Atomで動かす
コードは以下のとおりです.
必要なコメントは、コード内に記載

制御系コード

#include <M5Atom.h>
#include <Wire.h> 
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

//Servoのマップ関数の下限と上限の角度に相当するパルス幅相当digitの値
#define SERVOMIN 143 
#define SERVOMAX 471

//時間の経過を示す変数
int sk =0;
//paramは、動作に紐ついた変数、phaseは前後足の位相差、power0;歩幅
int param,phase=-3,power0=4;

#include <Ps3Controller.h>

//直接制御Servo
int PIN0 = 33;
int PIN1 = 23;
int PWMCH = 0;
int PWMCH1 = 1;
int PIN2 = 19;
int PIN3 = 22;
int PWMCH2 = 2;
int PWMCH3 = 3;

float pitch = 0.0F;
float roll  = 0.0F;
float yaw   = 0.0F;
float pitch0 = 0.0F;
float roll0  = 0.0F;
float yaw0   = 0.0F;

float Ppitch=0.0F;
float Proll=0.0F;

float p0, p1, p6, p5, p8, p9, p12, p13;
//Servo基準値
float p0_0=0, p1_0=0,p6_0=0, p5_0=10, p8_0=0, p9_0=0, p12_0=0, p13_0=-10;


void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.triangle ) {
      param = 0;
      Serial.println(param);
      Serial.println("Change2stop");
    }
    if( Ps3.event.button_down.square ) {
      param = 1;
      Serial.println(param);
      Serial.println("Change2walk");
    }    
    if( Ps3.event.button_down.cross ) {
      param = 2;
      Serial.println(param);
      Serial.println("Change2back");
    }
    if( Ps3.event.button_down.circle ) {
      param = 3;
      Serial.println(param);
      Serial.println("Change2rot");
    }        
    //--------------- Digital D-pad button events --------------
    
    if( Ps3.event.button_down.up ){
      power0 += 1;
      Serial.println(power0); Serial.println("\n");
      Serial.println("Change param0 up");
    }
    if( Ps3.event.button_down.down ){
      power0 += -1;
      Serial.println(power0); Serial.println("\n");
      Serial.println("Change param0 down");
    }
    if( Ps3.event.button_down.right ){
      phase += 1;
      Serial.println(phase);Serial.println("\n");
      Serial.println("Change param1 up");
    }
    if( Ps3.event.button_down.left ){
      phase += -1;
      Serial.println(phase);Serial.println("\n");
      Serial.println("Change param1 down");
    }
    
}

void onConnect(){
    Serial.println("Connected.");
}

void setup() {
  M5.begin(true, true, true);
  M5.IMU.Init(); //IMU利用姿勢計測のため
  M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS); //250,,500,1000,2000
  M5.IMU.SetAccelFsr(M5.IMU.AFS_2G); //2,4,8,16
  M5.IMU.getAhrsData(&pitch0,&roll0,&yaw0);
  
  Serial.begin(115200);     //シリアル通信を115200bpsに設定
  
  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.begin("94:b9:7e:92:4a:da"); //PS3コントロール　Atom　Mac
  
  pinMode(PIN0, OUTPUT);
  pinMode(PIN1, OUTPUT);
  ledcSetup(PWMCH, 50, 12); //12bit
  ledcAttachPin(PIN0, PWMCH);
  ledcSetup(PWMCH1, 50, 12); 
  ledcAttachPin(PIN1, PWMCH1);
  pinMode(PIN2, OUTPUT);
  pinMode(PIN3, OUTPUT);
  ledcSetup(PWMCH2, 50, 12); //12bit
  ledcAttachPin(PIN2, PWMCH2);
  ledcSetup(PWMCH3, 50, 12); 
  ledcAttachPin(PIN3, PWMCH3);
  
  param = 0; 
  Wire.begin(26,32); //i2cのpin宣言が重要 SDA, SCL
  pwm.begin();
  pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）
}

float func_sin(float skf){
  return sin(3.1415926535*skf/180);
}

void servo_write_func(float p0,float p1,float  p6,float  p5,float  p8,float  p9,float  p12, float p13){
  servo_write(0, constrain(p0, -50, 50));
  servo_write_ledc(0, constrain(p1, -50, 50)); 
  servo_write(6, constrain(p6, -50, 50));
  servo_write_ledc(1, constrain(p5, -50, 50));                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         
  servo_write(8, constrain(p8, -50, 50));
  servo_write_ledc(2, constrain(p9, -50, 50));
  servo_write(12, constrain(p12, -50, 50));
  servo_write_ledc(3, constrain(p13, -50, 50));
}

void stop(){
  p0 = p0_0;
  p1 = p1_0;
  p6 = p6_0;
  p5 = p5_0;
  p8 = p8_0;
  p9 = p9_0;
  p12 = p12_0;
  p13 = p13_0;
  servo_write_func(p0,p1,p6,p5,p8,p9,p12,p13);
}

void walk(){
  float power1=20;
  p0 = -10+power0*(6*(sk%30)-90)/18+p0_0;  //1
  p1 = 10+p1_0; 
  p6 = -10-power0*(6*((sk+12)%30)-90)/18+p6_0;  //2
  p5 = 10+p5_0; 
  p8 = power0*(6*((sk-20)%30)-90)/18+p8_0; //3
  p9 = power1+p9_0;
  p12 = -power0*(6*((sk+12-20)%30)-90)/18+p12_0;  //4
  p13 = -power1+p13_0;
  servo_write_func(p0,p1,p6,p5,p8,p9,p12,p13);
}

void back(){
  float power1=0;
  p0 = -power0*(6*((sk+12-20)%30)-90)/18+p0_0; //4
  p1 = power1-10+p1_0; 
  p6 = -10+power0*(6*((sk-20)%30)-90)/18+p6_0; //3
  p5 = -power1+0+p5_0; 
  p8 = -power0*(6*((sk+12)%30)-90)/18+p8_0;  //2
  p9 = power1+p9_0;
  p12 = power0*(6*(sk%30)-90)/18+p12_0; //1
  p13 = -power1-10+p13_0; 
  servo_write_func(p0,p1,p6,p5,p8,p9,p12,p13);
}

void rot(){
  p0 = phase+power0*(6*(sk%30)-90)/18+p0_0;  //1
  p1 = -10+p1_0; 
  p6 = -10+p6_0; //2
  p5 = p5_0; //*(6*(sk%30)-90)/18+p5_0;
  p8 = phase+power0*(6*((sk-20)%30)-90)/18+p8_0; //3  20
  p9 = p9_0;
  p12 = p12_0; //4
  p13 = -20+p13_0; 
  servo_write_func(p0,p1,p6,p5,p8,p9,p12,p13);
}

void loop() {
  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
  //Serial.printf("%.2f,%.2f,%.2f,\n", pitch-pitch0, roll-roll0, yaw-yaw0);
  
  param = constrain(param,0,7);
  if(param ==0){
    stop();
    Serial.printf("%d \n", phase);
    power0=0;
    delay(5);
  }
  if(param ==1){
    walk();
    delay(5);
  }
  if(param ==2){
    back();
    delay(5);
  } 
  if(param ==3){
    rot();
    delay(5);
  }    
  delay(0); //furue 0, swing others 5
  sk += 1;
  M5.update();
}

//PCA9865利用の出力
void servo_write(int ch, int ang){
  ang = map(ang, -90, 90, SERVOMIN, SERVOMAX); 
  pwm.setPWM(ch, 0, ang);
}

//Atomのledピンを直接利用
void servo_write_ledc(int ch, int ang){ 
  ang = map(ang, -90, 90, SERVOMIN, SERVOMAX);
  ledcWrite(ch, ang);
}

###・IoTセンサーを利用した計測
ここで工夫した点は、Hubを2個利用していわゆるタコ足配線で5個のGrove製品を利用できるようにしてみたことである。
そして、制御系とは別にもう一個のAtomを使って計測し、表示し、それをWebServerにM5CameraXを利用して転送する。

コードは以下のとおり、シンプルである.
それぞれのセンサー個別の利用方法は、以下の参考にまとめている。
【参考】
【M5Stack_IoT入門】いろいろ遊んでみた♪

計測系コード

#include <Wire.h>
#include <VL53L0X.h> // by Pololu http://librarymanager/All#vl53l0x-arduino https://github.com/pololu/vl53l0x-arduino
#include "UNIT_ENV.h"
#include <M5UnitOLED.h>

M5UnitOLED display;

M5Canvas canvas(&display);

int textpos = 0;
int scrollstep = 2;

SHT3X sht30;
QMP6988 qmp6988;

char draw_string[1024];
VL53L0X sensor;

void setup()
{
  display.init();
  display.setRotation(1); //2);
  canvas.setColorDepth(1); // mono color
  canvas.setFont(&fonts::lgfxJapanMinchoP_32);
  canvas.setTextWrap(false);
  canvas.setTextSize(2);
  canvas.createSprite(display.width() + 64, 72);
    
  Serial.begin(115200);
  Wire.begin(26,32); //stamp (26,32);  //Atom  (32, 33); // M5StickC
  qmp6988.init();
  
  sensor.setAddress(0x29);
  sensor.setTimeout(500);
  if (!sensor.init()) {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {}
  }
  sensor.startContinuous();
}
void loop()
{
  if (sht30.get() != 0) {
    return;
  }
  float Temp=sht30.cTemp;
  float Hum = sht30.humidity;
  float Pr = qmp6988.calcPressure();
  Serial.printf("Temperatura: %2.2f*C  Humedad: %0.2f%%  Pressure: %0.2fPa\r\n", Temp, Hum, Pr);
  
  Serial.print(sensor.readRangeContinuousMillimeters());
  if (sensor.timeoutOccurred()) {
    Serial.print(" TIMEOUT");
  }
  Serial.println();

  display.printf("\n");
  display.setCursor(16, 0);
  display.printf("Temp: %2.2f*C \n",  Temp);
  display.setCursor(16, 16);
  display.printf("Humedad: %0.2f%% \n", Hum);
  display.setCursor(16, 32);
  display.printf("Pres: %0.0fPa \n", Pr);
  display.setCursor(16, 48);
  display.printf("  Sensor: %d mm \n",sensor.readRangeContinuousMillimeters());
}

一方、、M5CameraXのコードは以下の参考サイトのとおりです.
M5Cameraを動かしてみる
###・結果
ということで、以下のような動きができる大きな四足歩行ローバーが出来ました.

###まとめ
・大きな四足歩行ローバーを作ってみた
・いろいろなセンサーで温度・湿度・気圧・距離を測定し、その情報をOLEDにリアルタイムに表示、その画像をカメラで撮影し、情報を画像としてWebに表示してみた
・画像をWeb（PC、VRやスマホなど）で見ながら、PS3コントローラーで制御できる

・大きな四足歩行ロボットでは、小さなロボットに比較して旋回がしずらく、さらなる改善が必要である
・センサー類は、少なくともあと１個は余裕あるので、CO2などのgasセンサーを利用したいと思う