今回は、「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センサーを利用したいと思う