最近、ロボット開発を始めました。
ロボットの基礎技術は、少なくとも以下が必要で本来取り付く島がないです。
①ロボット制作
②制御技術
③電子回路基板
ところが、以下の本に出合いました。
予算1万円でつくる二足歩行ロボット (I/O BOOKS) Tankobon Hardcover – June 24, 2020
by 中村俊幸 (著)@amzon

これを購入したのが5/25
そして、決定的になったのは、以下のTweetを見たときでした。
丁度、上記の本を眺めているときだったのでちょっと衝撃です。
これは、3dプリンターでやらなきゃということで、すぐ開始しました。
※3dプリンターは6/8に別のフライホイールの倒立振り子作成のために使い始めていました

ということで、このレベルには到達していませんが、話を始めようと思います。

やったこと

・3dプリント
・回路基板について
・制御プログラム
・結果

・3dプリント

ロボット制作は、本書のリンク先からダウンロードした設計図を3dプリントするだけです。
とはいえ、fusion360のファイルなので、これを3dプリント出力して、tincarcadで処理しました。
処理は、二分割してstlで出力します。
そのファイルをcureで読みんこんで、gcordで出力します。
3dプリンターは、上記のHomemadegarbageさんが使われているものと同じ以下のものを使いました。
※ググるといろいろありますが、同じものを使うのが確実です
ANYCUBIC MEGA-S 3D Printer@amazon
（簡単なんだけど）重量があり、どうにか組み立てられました。

・回路基板について

本書を読むと、回路は回路基板設計用のKicadでやるような説明です。
しかし、これはそれなりに時間がかかりますし、発注するのも初体験になるので面倒です。
ところが、実は制御はこれまでやってきた倒立振り子と大差ないものです。
ということで、ここは全部端折って、m5stackの製品でやれそうです。
ちょっと前にm5atomをやっていたので、これを使うことにしました。
※顔イメージならほかの製品を使うのがいいかもしれません
※まあ、本書の解説はPCA9865を別基盤に載せ替え＋esp32を一体型で回路を設計されています。これはこれで使いやすそうなので、そのうち挑戦したいと思います。

ということで、どうしても回路周りは後回しにして、配線は犠牲にしてもとにかく動けばＯＫというスタンスで進めます。

・制御プログラム

本書には、制御プログラムも書かれていますが、今回は読み飛ばしました。
まず、外部からコントロールするために、以前やったPC3コントローラーで実施します。
今回利用するm5atomのmacアドレスは、以前の方法と同様に取得して利用します。

# include <Ps3Controller.h>

void setup()
{
    Serial.begin(115200);
    Ps3.begin();

    String address = Ps3.getAddress();

    Serial.print("The ESP32's Bluetooth MAC address is: ");
    Serial.println(address);
}

void loop()
{
    String address = Ps3.getAddress();
    Serial.print("The ESP32's Bluetooth MAC address is: ");
    Serial.println(address);
    // The ESP32's Bluetooth MAC address is: 94:b9:7e:92:4a:da
}

また、servoは全部で12個使いますが、この制御はこれもm5atomで倒立振り子をやった時の記事と同様に、PCA9685を利用すれば同じようなコードで制御できます。
※m5atomだと6個までは制御できそうですが、ちょっと不足します。

コード詳細

まだまだ工夫すべきだし、動きは調整が必要ですが、ほぼ以下のコードを改善すればいいと思うレベルになったので、解説していきます。
※予測的な動作をROSでシミュレーションしたいと思いますが、まだできていません

環境構築

Raspi4上のArduinoIDEで開発しました。
ポイントは、以下の通りです。
①ArduinoIDEのダウンロード
　Downloadsから、ARM 32 bitsをダウンロードします。
解凍して、ファイルのinstall.shをダブルクリックでraspiのプログラムにインストール（掲載）され、デスクトップにも起動リンクが表れます。
ここでポイントは実は同じようにRaspiにArduinoをインストールできる手順がありますが、それはunoなどは使えますが、設定ファイルが異なりボードが追加できません
そして、一度インストールしてしまうと、再インストールしても元にもどせず、RaspiのSDからやり直しが発生しました。
原因は、よくわかりませんが一度インストール後、新たに出来るArduinoとdirにものを入れるとその後修復が出来なくなってしまいました。
ということで正しい手順で一発でインストールするのが簡単です。
その後は、以前と同様な手順でboard(esp32)インストールやプログラム管理を起動して、m5atom core2, stickcなどのスケッチをインストールします。
また、今回開発中のアプリを別のraspiから移設しましたが、普通にSDに焼いて、そのまま開いて、それをコピーしてやると先ほどのArduinoのdir配下に保存出来ました。
もちろん、SDのままでも継続して開発できます。

利用Libraryと準備

Libraryのうち、Adafruitは、以下のサイトからzipダウンロードして、zipでインストールしました。
adafruit/Adafruit-PWM-Servo-Driver-Library
また、サーボはsg90なので以前と同じ最大値、最小値を利用します。
skは、時間と考えてください
paramはarm() leg() walk()などの動作に紐づけられた変数です。
Ps3Controllerは、プログラム管理からインストールします。

robot_base_ino.c

# include <M5Atom.h>
# include <Wire.h> 
# include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

# define SERVOMIN 143 
     // 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最小パルス幅　：4096/20×0.7＝102.4　（0.5ms：SG90、0°の時のパルス幅）
# define SERVOMAX 471
     // 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最大パルス幅　：4096/20×2.3＝491.5　（2.4ms：SG90、180°の時のパルス幅）
int sk =0;
int param;
# include <Ps3Controller.h>

notify

Ps3コントローラーの命令と紐づけます。
cross/square/triangle/circle buttonは直接動作と紐づけました。
一方、D-pad buttonという左側にある上下で、動作順を繰り上げ、繰り下げするように設定しています。
※紐づけていない動作は、未完成なものです。

int player = 0;

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.triangle ) {
      param = 0; //arm()
      Serial.println(param);
      Serial.println("Change2Arm");
    }
    if( Ps3.event.button_down.square ) {
      param = 5;  //Furue()
      Serial.println(param);
      Serial.println("Change2Furue");
    }    
    if( Ps3.event.button_down.cross ) {
      param = 6; //Swing()
      Serial.println(param);
      Serial.println("Change2Swing");
    }
    if( Ps3.event.button_down.circle ) {
      param = 3; //side_step()
      Serial.println(param);
      Serial.println("Change2Step_L");
    }        
    //--------------- Digital D-pad button events --------------
    
    if( Ps3.event.button_down.up ){
      param += 1;
      Serial.println(param);
      Serial.println("Change param up");
    }
    if( Ps3.event.button_down.down ){
      param += -1;
      Serial.println(param);
      Serial.println("Change param down");
    }
    
}

Bleコネクション

出来たら標準出力にConnectedと表示します。

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

初期化

PCA9685のためのI2C初期化が必要です、

void setup() {
 Serial.begin(115200);     //シリアル通信を115200bpsに設定

 Ps3.attach(notify);
 Ps3.attachOnConnect(onConnect);
 Ps3.begin("94:b9:7e:92:4a:da");
 
 param = 0; //2; //1; //0; //3; //4; //5; //6;
 M5.begin(true, false, true);
 Wire.begin(26,32); //i2cのpin宣言が重要 SDA, SCL
 pwm.begin();
 pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）
}

12個サーボの書込み関数

それぞれの動作ごとに記載してもいいですが、まとめて書込みする関数を用意しました。

void servo_write_func(float p0,float p15,float p1,float p14,float p2,float p13,float p3,float p12,float p5,float p10,float p7,float p8){
  servo_write(0, p0);
  servo_write(15, p15);
  servo_write(1, p1);
  servo_write(14, p14);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         
  servo_write(2, p2);
  servo_write(13, p13);
  servo_write(3, p3);
  servo_write(12, p12);
  servo_write(5, p5);
  servo_write(10, p10); 
  servo_write(7, p7);
  servo_write(8, p8); 
}

動作ごとに関数に数字を送ると動作します。
ちなみに、サーボの基準点が取り付けたときに決まるので、制作段階で合わせておくと設定が楽かもしれません。

void arm(){
  servo_write_func(1*sk%180-90,1*sk%180-90,50,-50,60,0,30,-10,-20,20,10,0);
}

void leg(){
  servo_write_func(0,0,50,-50,60,0,0.1*(1*sk%180-90)+30,0.1*(1*sk%180-90)-10,0.05*(1*sk%180-90)-20,-0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

void walk(){
  servo_write_func(0.5*(1*sk%180-90),0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,60,0,0.1*(1*sk%180-90)+30,0.1*(1*sk%180-90)-10,0.05*(1*sk%180-90)-20,-0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

void back(){
  servo_write_func(-0.5*(1*sk%180-90),-0.5*(1*sk%180-90),-0.1*(1*sk%180-90)+50,0.1*(1*sk%180-90)-50,60,0,-0.1*(1*sk%180-90)+30,-0.1*(1*sk%180-90)-10,-0.05*(1*sk%180-90)-20,0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

void side_step_R(){
  servo_write_func(0.5*(1*sk%180-90), 0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,0.2*(1*sk%180-90)+60,0.2*(1*sk%180-90),30,-10,-20,20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

void side_step_L(){
  servo_write_func(0.5*(1*sk%180-90), 0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,-0.2*(1*sk%180-90)+60,-0.2*(1*sk%180-90),30,-10,-20,20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

loop()関数

あとは、paramにあった動作をさせるだけです。
震えとスイングが面白いです。
上の関数を組み合わせれば、色々な動作をさせられると思います。

void loop() {
  param = constrain(param,0,6);
  if(param ==0){
    arm();
    delay(5);
  }
  if(param ==1){
    walk();
    delay(5);
  }
  if(param ==2){
    side_step_L();
    delay(5);
  }
  if(param ==3){
    side_step_R();
  }
  if(param ==4){
    leg(); //back();
    delay(5);
  }
  
  if(param ==5){
  // furue
    if(sk%10>5){ //Furue; 10,5 swing; 360,180
      side_step_L();
    }
    if(sk%10<=5){
      side_step_R();
    }
    delay(0);   
  }
  
  if(param ==6){
  // swing
    if(sk%360>180){ //Furue; 10,5 swing; 360,180
    side_step_L();
    }
    if(sk%360<=180){
      side_step_R();
    }
    delay(5);  
  }
 
  delay(0);
  sk += 1;
  M5.update();
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, -90, 90, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
  pwm.setPWM(ch, 0, ang);
}

・結果

結果は、以下のような動作ができるようになりました。

まとめ

・初めての3dプリンターで二足歩行ロボットを作ってみた
・m5atomで歩行させてみた
・動作を整理して、それぞれの動作を調整すればRosに頼らなくても一定の動作はできた

・Rosシミュレーションやりたいな
・ロボットが身近になり、いろいろなロボットを作れるような気がする

コード全体

コード

robot_base_ino.c

# include <M5Atom.h>
# include <Wire.h> 
# include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

# define SERVOMIN 143 
     // 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最小パルス幅　：4096/20×0.7＝102.4　（0.5ms：SG90、0°の時のパルス幅）
# define SERVOMAX 471
     // 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最大パルス幅　：4096/20×2.3＝491.5　（2.4ms：SG90、180°の時のパルス幅）
int sk =0;
int param;
# include <Ps3Controller.h>

int player = 0;

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.triangle ) {
      param = 0; //arm()
      Serial.println(param);
      Serial.println("Change2Arm");
    }
    if( Ps3.event.button_down.square ) {
      param = 5;  //Furue()
      Serial.println(param);
      Serial.println("Change2Furue");
    }    
    if( Ps3.event.button_down.cross ) {
      param = 6; //Swing()
      Serial.println(param);
      Serial.println("Change2Swing");
    }
    if( Ps3.event.button_down.circle ) {
      param = 3; //side_step()
      Serial.println(param);
      Serial.println("Change2Step_L");
    }        
    //--------------- Digital D-pad button events --------------
    
    if( Ps3.event.button_down.up ){
      param += 1;
      Serial.println(param);
      Serial.println("Change param up");
    }
    if( Ps3.event.button_down.down ){
      param += -1;
      Serial.println(param);
      Serial.println("Change param down");
    }
    
}

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

void setup() {
 Serial.begin(115200);     //シリアル通信を115200bpsに設定

 Ps3.attach(notify);
 Ps3.attachOnConnect(onConnect);
 Ps3.begin("94:b9:7e:92:4a:da");
 
 param = 0; //2; //1; //0; //3; //4; //5; //6;
 M5.begin(true, false, true);
 Wire.begin(26,32); //i2cのpin宣言が重要 SDA, SCL
 pwm.begin();
 pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）
}

void servo_write_func(float p0,float p15,float p1,float p14,float p2,float p13,float p3,float p12,float p5,float p10,float p7,float p8){
  servo_write(0, p0);
  servo_write(15, p15);
  servo_write(1, p1);
  servo_write(14, p14);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         
  servo_write(2, p2);
  servo_write(13, p13);
  servo_write(3, p3);
  servo_write(12, p12);
  servo_write(5, p5);
  servo_write(10, p10); 
  servo_write(7, p7);
  servo_write(8, p8); 
}

void arm(){
  servo_write_func(1*sk%180-90,1*sk%180-90,50,-50,60,0,30,-10,-20,20,10,0);
}

void leg(){
  servo_write_func(0,0,50,-50,60,0,0.1*(1*sk%180-90)+30,0.1*(1*sk%180-90)-10,0.05*(1*sk%180-90)-20,-0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

void walk(){
  servo_write_func(0.5*(1*sk%180-90),0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,60,0,0.1*(1*sk%180-90)+30,0.1*(1*sk%180-90)-10,0.05*(1*sk%180-90)-20,-0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

void back(){
  servo_write_func(-0.5*(1*sk%180-90),-0.5*(1*sk%180-90),-0.1*(1*sk%180-90)+50,0.1*(1*sk%180-90)-50,60,0,-0.1*(1*sk%180-90)+30,-0.1*(1*sk%180-90)-10,-0.05*(1*sk%180-90)-20,0.05*(1*sk%180-90)+20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

void side_step_R(){
  servo_write_func(0.5*(1*sk%180-90), 0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,0.2*(1*sk%180-90)+60,0.2*(1*sk%180-90),30,-10,-20,20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

void side_step_L(){
  servo_write_func(0.5*(1*sk%180-90), 0.5*(1*sk%180-90),0.1*(1*sk%180-90)+50,-0.1*(1*sk%180-90)-50,-0.2*(1*sk%180-90)+60,-0.2*(1*sk%180-90),30,-10,-20,20,0.1*(1*sk%180-90)+10,0.1*(1*sk%180-90));
}

void loop() {
  param = constrain(param,0,6);
  if(param ==0){
    arm();
    delay(5);
  }
  if(param ==1){
    walk();
    delay(5);
  }
  if(param ==2){
    side_step_L();
    delay(5);
  }
  if(param ==3){
    side_step_R();
  }
  if(param ==4){
    leg(); //back();
    delay(5);
  }
  
  if(param ==5){
  // furue
    if(sk%10>5){ //Furue; 10,5 swing; 360,180
      side_step_L();
    }
    if(sk%10<=5){
      side_step_R();
    }
    delay(0);   
  }
  
  if(param ==6){
  // swing
    if(sk%360>180){ //Furue; 10,5 swing; 360,180
    side_step_L();
    }
    if(sk%360<=180){
      side_step_R();
    }
    delay(5);  
  }
 
  delay(0);
  sk += 1;
  M5.update();
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, -90, 90, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
  pwm.setPWM(ch, 0, ang);
}