【二足歩行ロボット入門】初めての3dプリンターとm5atomで制御できた♬

最近、ロボット開発を始めました。
ロボットの基礎技術は、少なくとも以下が必要で本来取り付く島がないです。
①ロボット制作
②制御技術
③電子回路基板
ところが、以下の本に出合いました。
[予算1万円でつくる二足歩行ロボット (I/O BOOKS) Tankobon Hardcover – June 24, 2020
by 中村 俊幸 (著)@amzon](https://www.amazon.co.jp/%E4%BA%88%E7%AE%971%E4%B8%87%E5%86%86%E3%81%A7%E3%81%A4%E3%81%8F%E3%82%8B%E4%BA%8C%E8%B6%B3%E6%AD%A9%E8%A1%8C%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88-I-BOOKS-%E4%B8%AD%E6%9D%91-%E4%BF%8A%E5%B9%B8/dp/4777521095)
![nisokuhokou.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/4d118044-6dec-d0c5-a585-c87b5c4a23e6.jpeg)

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

https://twitter.com/H0meMadeGarbage/status/1406394343053615106

ということで、このレベルには到達していませんが、話を始めようと思います。
###やったこと
・3dプリント
・回路基板について
・制御プログラム
・結果
###・3dプリント
ロボット制作は、本書のリンク先からダウンロードした設計図を3dプリントするだけです。
とはいえ、fusion360のファイルなので、これを3dプリント出力して、tincarcadで処理しました。
処理は、二分割してstlで出力します。
そのファイルをcureで読みんこんで、gcordで出力します。
3dプリンターは、上記のHomemadegarbageさんが使われているものと同じ以下のものを使いました。
※ググるといろいろありますが、同じものを使うのが確実です
[ANYCUBIC MEGA-S 3D Printer@amazon](https://www.amazon.co.jp/-/en/Precision-Operation-Education-Professionals-Beginners/dp/B07J5P3SP9?ref_=Oct_s9_apbd_obs_hd_bw_b2vOZDH&pf_rd_r=KX2S5KDS9APNJ88ZJWDG&pf_rd_p=701b1538-b168-512e-9311-d3ca613872de&pf_rd_s=merchandised-search-10&pf_rd_t=BROWSE&pf_rd_i=2680372051)
（簡単なんだけど）重量があり、どうにか組み立てられました。

![3dprinter.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/e9101548-b52a-1d23-ab3b-9d3c98466609.jpeg)
###・回路基板について
本書を読むと、回路は回路基板設計用のKicadでやるような説明です。
しかし、これはそれなりに時間がかかりますし、発注するのも初体験になるので面倒です。
ところが、実は制御はこれまでやってきた倒立振り子と大差ないものです。
ということで、ここは全部端折って、m5stackの製品でやれそうです。
ちょっと前にm5atomをやっていたので、これを使うことにしました。
※顔イメージならほかの製品を使うのがいいかもしれません
※まあ、本書の解説はPCA9865を別基盤に載せ替え＋esp32を一体型で回路を設計されています。これはこれで使いやすそうなので、そのうち挑戦したいと思います。

ということで、どうしても回路周りは後回しにして、配線は犠牲にしてもとにかく動けばＯＫというスタンスで進めます。
###・制御プログラム
本書には、制御プログラムも書かれていますが、今回は読み飛ばしました。
まず、外部からコントロールするために、以前やったPC3コントローラーで実施します。
今回利用する[m5atomのmacアドレスは、以前の方法と同様に取得して利用します](https://qiita.com/MuAuan/items/6f8f5d0587798b3c6d8b)。

```c
#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を利用すれば同じようなコードで制御できます](https://qiita.com/MuAuan/items/8dacc75b2e94fc644798)。
※m5atomだと6個までは制御できそうですが、ちょっと不足します。

###コード詳細
まだまだ工夫すべきだし、動きは調整が必要ですが、ほぼ以下のコードを改善すればいいと思うレベルになったので、解説していきます。
※予測的な動作をROSでシミュレーションしたいと思いますが、まだできていません
####環境構築
Raspi4上のArduinoIDEで開発しました。
ポイントは、[以下の通り](https://qiita.com/MuAuan/items/e5a8d2a57482b0551bb4)です。
①ArduinoIDEのダウンロード
　[Downloads](https://www.arduino.cc/en/software)から、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](https://github.com/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という左側にある上下で、動作順を繰り上げ、繰り下げするように設定しています。
※紐づけていない動作は、未完成なものです。

```c
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と表示します。

```c
void onConnect(){
    Serial.println("Connected.");
}
```
####初期化
PCA9685のためのI2C初期化が必要です、

```c
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個サーボの書込み関数
それぞれの動作ごとに記載してもいいですが、まとめて書込みする関数を用意しました。

```c
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); 
}
```
動作ごとに関数に数字を送ると動作します。
ちなみに、サーボの基準点が取り付けたときに決まるので、制作段階で合わせておくと設定が楽かもしれません。

```c
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にあった動作をさせるだけです。
震えとスイングが面白いです。
上の関数を組み合わせれば、色々な動作をさせられると思います。

```c
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);
}
```
###・結果
結果は、以下のような動作ができるようになりました。

https://twitter.com/MuAuan/status/1411135282393477122

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

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

<details><summary>コード</summary><div>

```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);
}
```
</div></details>