先日、二足歩行ロボットの途中経過までをまとめたが、今回はそれよりは簡単だろうということで、四足歩行ロボットを作成して動かしてみた。
思っていたよりは難しく、それでも基本動作はできるようになったので、まとめておく。

###やったこと
・組み立て
・環境
・動かしてみる
###・組み立て
製作はいたって簡単化してみた。
通常だと、足関節は最小２個のようだが、今回は最も簡単に一本ずつサーボで制御することとした。
ちょっと考察すればわかるが、これはむしろ足の抵抗をコントロールできないので、歩かせるのは本来難しい。
しかし、通常の四輪車が動くのだから、できなくはないだろうと思ってやってみることにした。
###・環境
制御するためのマイコンは、今回はM5Stack社のM5StickCを利用することとした。
まあ、何を利用しても同じようにできると思うが、比較的小ぶりで以前追跡カメラや倒立振り子で利用したのでPin配列も扱いやすい。

M5StickC	Servo	備考
Pin32	S1 yellow	ー
Pin33	S2 yellow	ー
Pin0	S3 yellow	ー
Pin26	S4 yellow	ー
GND	S1,S2,S3,S4 GND	LIPO GND共通
LiPO1S+	S1,S2,S3,S4 red
LiPO1S-	S1,S2,S3,S4 brown
上記表以外に、Bluetooth接続のPS3コントローラーを利用しました。
マイコン毎のMacアドレスを取得し、プログラム内で設定する必要がありますが、これについては以下の参考を見てください。
【参考】
【PS3コントローラー入門】外部制御で倒立振り子のPID制御パラメータを決定する♪

###・動かしてみる
まず、比較的簡単な例をしめす。
利用するPinを以下のように定義する。
ここで、３２、３３が前足、０、２６が後ろ足。
あわせて、PWMCHを定義している。

#include <M5StickC.h>
#define PI 3.141592653538

int PIN0 = 32;
int PIN1 = 33;
int PWMCH = 0;
int PWMCH1 = 1;
int PIN2 = 0;
int PIN3 = 26;
int PWMCH2 = 2;
int PWMCH3 = 3;

サーボの初期値を定義する。
これは、サーボの初期値がどうしても工作したときずれるので、ここで校正している。
また、param, power0は歩行の種類を表すパラメータと歩行の振幅を決める係数である。
roll, pitchなどは、今回は利用しないが、姿勢制御用の変数である。

float p0_0 = 0;            // サーボの出力基準
float p1_0 = 0;
float p2_0 = 10;
float p3_0 = 10;
float p0 = 0;            // サーボの出力
float p1 = 0;
float p2 = 0;
float p3 = 0;

int param=0;
float power0=10;

float roll, pitch, yaw;

以下が、PS3コントローラーの部分である。
notify()の中でコントローラーのインプットに対する制御を記述している。

#include <Ps3Controller.h>
int player = 0;

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.triangle ) {
      param = 0;
      Serial.println("Change 2 stop");
    }
    if( Ps3.event.button_down.square ){
      param = 1;
      Serial.println("Change 2 walk");
    }
}

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

以下初期化関数です。StickCに固有なMacアドレスでPS3を開始しています。
pinMode()でそのピンが出力用だと示しています。
その後、表示します。

int sk=0;

void setup() {
  M5.begin();
  Serial.begin(115200);
  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.begin("94:b9:7e:a2:7c:22"); //StickC

  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);
  
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(40, 0, 2);
  M5.Lcd.println("MPU6886 TEST");
  M5.MPU6886.Init();
  
}

4つのサーボへの出力をまとめて行う関数を以下のように定義します。
これを利用する関数として、基準点を決めるためのstop()と歩く動作を実現するwalk()を示しています。

void servo_write_func(float p0,float  p1,float  p2,float  p3){
  servo_write(0, constrain(p0, -50, 50));
  servo_write(1, constrain(p1, -50, 50));
  servo_write(2, constrain(p2, -50, 50));
  servo_write(3, constrain(p3, -50, 50));
}

void stop(){
  p0 = p0_0;
  p1 = p1_0;
  p2 = p2_0;
  p3 = p3_0;
  servo_write_func(p0,p1,p2,p3);
}

void walk(){
  p0 = p0_0+power0*sin(5*sk*PI/180);
  p1 = p1_0+power0*cos(5*sk*PI/180);
  p2 = p2_0+power0*sin(5*sk*PI/180+30/18);
  p3 = p3_0+power0*cos(5*sk*PI/180+30/18);
  servo_write_func(p0,p1,p2,p3);
}

以下が繰り返し実行されるシーケンスです。
param=0のときは、stop()で停止しています。
そして、Ps3コントーローラーでparam=1を選ぶと、walk()動作を行います。

void loop() {
  M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
  if(param==0){
    stop();
  }
  if(param==1){
    walk();
  }
  M5.Lcd.setCursor(0, 15);
  M5.Lcd.printf("roll %5.1f  pitch %5.1f\n",roll,pitch); M5.Lcd.print("\n");
  M5.Lcd.setCursor(0, 45);
  M5.Lcd.printf("p0 %5.1f  p1 %5.1f  \n",p0,p1);
  M5.Lcd.printf("p2 %5.1f  p3 %5.1f\n",p2,p3); M5.Lcd.print("\n");
  delay(param1);
  sk +=1;
}

最後にサーボの角度をPWMに変換して出力します。

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, -90, 90, int(0.7*4096/20), int(2.3*4096/20)); //角度（-90～90）をPWMのパルス幅（700～2300）に変換
  ledcWrite(ch, ang);
}

###ちょっとした工夫
####動作関数
これは、動物がどう歩いているか考えると、少なくとも周期的に動かしている。
そして、車を考えると円運動が設置面を変えて移動している。だから、足の運動も三角関数で表せるんじゃないかと考えつく。そして、ねこなんかの足の動きを見ると若干前足と後ろ足が位相を持って、つまり遅れ気味に動いている。
これを以下のような関数で表現した。
p0, p1が前足でこれはSin,Cosであらわす。
いっぽう、後ろ足は前足からpower0＿/18だけ位相がずれている。
この位相のずれを直接歩行させて、PS3コントローラーを利用して変更しつつ決定した。
これで、一応歩く。しかし、スムースな感じではなく進みも悪い。

void mode(){
  p0 = p0_0+power0*sin(5*sk*PI/180);
  p1 = p1_0+power0*cos(5*sk*PI/180);
  p2 = p2_0+power0*sin(5*sk*PI/180-power0_/18); //power0_=-30;foward 
  p3 = p3_0+power0*cos(5*sk*PI/180-power0_/18); //+power0_=36; backward
  servo_write_func(p0,p1,p2,p3);
}

####改善1引き足
よく考えれば足の関節があれば、足を設置して、前に出すときは上に引き上げて下ろす。しかも出す方が歩くときよりすばやい。この効果を取り入れたい。ということで、以下の関数でやってみた。
以下の関数では、sk=0-29まで徐々に数値が増大し、30になると一度に最小値になるものである。
その関数をつかって、上記と同じように前後の足で位相差をつけて動かすこととした。
この関数を使うと、進み方は格段によくなる。それでも、歩幅を変えると引っかかったり、何より前進がよいかと思うと後退の動きが悪いという現象があらわれだした。

void mode2(){
  p0 = p0_0+power0*((6*(sk%30)-90)/20);
  p1 = p1_0-power0*((6*((sk+12)%30)-90)/20);
  p2 = p2_0+power0*((6*((sk+power0_)%30)-90)/20); //power0_=-20; power 4; param1=18 ;foward 
  p3 = p3_0-power0*((6*((sk+12+power0_)%30)-90)/20); //+power0_=6; backward
  servo_write_func(p0,p1,p2,p3);
}

####改善２足の初期値
よく考えると、前進と後退で足のゼロ点が同じ必要はない。人でも、前進と後退は歩く姿勢も足の角度も異なる。
ということで、前進と後退時のp0-p3を変えることにした。
これが大当たりでした。まあ、当たり前な気もします。
以下のような関数になりました。
つまり、後退時は前進に比べて進行方向に足を傾けています。
これで引っかかることもなく前進後退が出来るようになりました。

void walk(){
  p0 = p0_0+power0*((6*(sk%30)-90)/20);
  p1 = p1_0-power0*((6*((sk+12+power0_)%30)-90)/20);
  p2 = p2_0+power0*((6*((sk-20)%30)-90)/20); //power0_=-20; power 4; param1=18 ;forward 
  p3 = p3_0-power0*((6*((sk+12-20+power0_)%30)-90)/20); 
  servo_write_func(p0,p1,p2,p3);
  delay(18);
}

void back(){
  p0 = p0_0+20-power0*((6*(sk%30)-90)/20);
  p1 = p1_0+power0*((6*((sk+12)%30)-90)/20);
  p2 = p2_0+10-power0*((6*((sk-16)%30)-90)/20); //power0_=-16; power0;4; param1=10 ;forward 
  p3 = p3_0+15+power0*((6*((sk+12-16)%30)-90)/20); 
  servo_write_func(p0,p1,p2,p3);
  delay(10);
}

####走れるか
四足動物の特徴として、走るというのがあります。
これが出来るかどうかは興味がわきます。ということで、以下の関数でやってみました。
特徴は後ろ足がそろっていて、前足は少し位相差があり、後ろ足とは逆位相です。
以下の関数で雰囲気は出せました。

void run(){
  p0 = p0_0-power0*((6*(sk%30)-90)/20);
  p1 = p1_0+power0*((6*((sk+2)%30)-90)/20);
  p2 = p2_0+power0*((6*((sk-7)%30)-90)/20); //power0=12; power0_=-7; param1=18 ;foward 
  p3 = p3_0-power0*((6*((sk-7)%30)-90)/20); 
  servo_write_func(p0,p1,p2,p3);
  delay(18);
}

###結果
####前進後退の例

#あらぶるちゃん
らしい‼️

名前に恥じない振舞い🎵
小さな歩幅から大きな歩幅まで安定してきた#M5Stack #M5StickC pic.twitter.com/Ca0YDE1Cia
— ウワン (@MuAuan) August 12, 2021

####走るの例（この動画の前進後退はゼロを変更する前）

#あらぶるちゃん
今朝のお散歩‼️#M5Stack #M5StickC pic.twitter.com/SbaRjdDj3J
— ウワン (@MuAuan) August 12, 2021

###まとめ
・四速歩行ロット作ってみた
・前進後退そして、走るをやってみた

・センサー類を装備して自律走行をやってみようと思う
###完成版

#include <M5StickC.h>
#define PI 3.141592653538

int PIN0 = 32;
int PIN1 = 33;
int PWMCH = 0;
int PWMCH1 = 1;
int PIN2 = 0;
int PIN3 = 26;
int PWMCH2 = 2;
int PWMCH3 = 3;

float p0_0 = -5;            // サーボの出力基準
float p1_0 = 0;
float p2_0 = 10;
float p3_0 = 10;
float p0 = 0;            // サーボの出力
float p1 = 0;
float p2 = 0;
float p3 = 0;

int param=0;
float power0=0;
int power0_=0;

float roll, pitch, yaw;
#include <Ps3Controller.h>
int player = 0;

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.triangle ) {
      param = 0;
      Serial.println("Change 2 stop");
    }
    if( Ps3.event.button_down.square ){
      param = 1;
      Serial.println("Change 2 walk");
    }
    if( Ps3.event.button_down.cross ){
      param = 2;
      Serial.println("Change 2 back");
    }
    if( Ps3.event.button_down.circle ){
      param = 3;
      Serial.println("Change 2 run");
    }
    

    //--------------- Digital D-pad button events --------------
    if( Ps3.event.button_down.up ){
      power0 +=1;
      Serial.println(power0);
      Serial.println("Change power0 up");
    }

    if( Ps3.event.button_down.right ){
      power0_ += 1;
      Serial.println(power0_);
      Serial.println("Change power0_ up");
    }

    if( Ps3.event.button_down.down ){
      power0 += -1;
      Serial.println(power0);
      Serial.println("Change power0 down");
    }

    if( Ps3.event.button_down.left ){
      power0_ += -1;
      Serial.println(power0_);
      Serial.println("Change power0_ down");
    }
}

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

int sk=0;

void setup() {
  M5.begin();
  Serial.begin(115200);
  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.begin("94:b9:7e:a2:7c:22"); //StickC

  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);
  
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(BLACK);

  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(40, 0, 2);
  M5.Lcd.println("MPU6886 TEST");
  M5.MPU6886.Init();
}

void servo_write_func(float p0,float  p1,float  p2,float  p3){
  servo_write(0, constrain(p0, -50, 50));
  servo_write(1, constrain(p1, -50, 50));
  servo_write(2, constrain(p2, -50, 50));
  servo_write(3, constrain(p3, -50, 50));
}

void stop(){
  p0 = p0_0;
  p1 = p1_0;
  p2 = p2_0;
  p3 = p3_0;
  servo_write_func(p0,p1,p2,p3);
}

void walk(){
  p0 = p0_0+power0*((6*(sk%30)-90)/20);
  p1 = p1_0-power0*((6*((sk+12)%30)-90)/20);
  p2 = p2_0+power0*((6*((sk-20)%30)-90)/20); //power0_=-20; power 4; param1=18 ;foward 
  p3 = p3_0-power0*((6*((sk+12-20)%30)-90)/20); 
  servo_write_func(p0,p1,p2,p3);
  delay(18);
}

void back(){
  p0 = p0_0-power0*((6*(sk%30)-90)/20);
  p1 = p1_0+power0*((6*((sk+12)%30)-90)/20);
  p2 = p2_0-power0*((6*((sk-16)%30)-90)/20); //power0_=-16; power0;4; param1=10 ;foward 
  p3 = p3_0+power0*((6*((sk+12-16)%30)-90)/20); 
  servo_write_func(p0,p1,p2,p3);
  delay(10);
}

void run(){
  p0 = p0_0-power0*((6*(sk%30)-90)/20);
  p1 = p1_0+power0*((6*((sk+2)%30)-90)/20);
  p2 = p2_0+power0*((6*((sk-7)%30)-90)/20); //power0=12; power0_=-7; param1=18 ;foward 
  p3 = p3_0-power0*((6*((sk-7)%30)-90)/20); 
  servo_write_func(p0,p1,p2,p3);
  delay(18);
}

void loop() {
  M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
  if(param==0){
    stop();
  }
  if(param==1){
    walk();
  }
  if(param==2){
    back();
  }
  if(param==3){
    run();
  }

  M5.Lcd.setCursor(0, 15);
  M5.Lcd.printf("roll %5.1f  pitch %5.1f\n",roll,pitch); M5.Lcd.print("\n");
  M5.Lcd.setCursor(0, 45);
  M5.Lcd.printf("p0 %5.1f  p1 %5.1f  \n",p0,p1);
  M5.Lcd.printf("p2 %5.1f  p3 %5.1f\n",p2,p3); M5.Lcd.print("\n");
  delay(0);
  sk +=1;
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, -90, 90, int(0.7*4096/20), int(2.3*4096/20)); //角度（-90～90）をPWMのパルス幅（700～2300）に変換
  ledcWrite(ch, ang);
}