【MiniPupper入門】MPU6050とAtom_liteで歩行中姿勢制御してみた♪

今回は、動きのバリエーションを増やすと共に、MPU6050を利用してバランスをとりつつ歩行させてみた。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">そして、IMUでバランス取りながらの前進🎵<br><br>停止してから、モード変えると修正分が...<a href="https://twitter.com/hashtag/mini_pupper?src=hash&amp;ref_src=twsrc%5Etfw">#mini_pupper</a> <a href="https://t.co/hsH0Res0TX">pic.twitter.com/hsH0Res0TX</a></p>&mdash; ウワン (@MuAuan) <a href="https://twitter.com/MuAuan/status/1502914701621022720?ref_src=twsrc%5Etfw">March 13, 2022</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# 準備
PS3コントローラーの左スティックと右スティックを動かすと、ひねり動作や前後動作するようにした。
#### ひねり動作
基本

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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

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

//時間の経過を示す変数
int sk =0;
float kx=0,ky=0, lx=0, ly=0;
float pk=1.; //pk=1 ; 歩幅初期値
int st=36; //st=36　；繰り返し初期値
float st2=st*0.5; //繰り返しの折り返し
float st4=st*0.25; //位相初期値
float D10=50; //delay

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

void servo_write_func(float p0,float p1,float p2,float p3,float p4,float p5,float p6,float p7,float p8,float p9,float p10,float p11){
  servo_write(0, p0);
  servo_write(1, p1);
  servo_write(2, p2);
  servo_write(3, p3);
  servo_write(4, p4);
  servo_write(5, p5);
  servo_write(6, p6);
  servo_write(7, p7);
  servo_write(8, p8);
  servo_write(9, p9);
  servo_write(10, p10);
  servo_write(11, p11);
}

float p0_=90,p1_=90, p2_=70;
float p3_=95,p4_=90, p5_=60;
float p6_=85, p7_=100, p8_=110;
float p9_=86, p10_=100, p11_=110;
```
以下のnotify()でパラメーターを変更する。

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

    //--------------- Digital D-pad button events --------------
    if( Ps3.event.button_down.up ){
      param += 1;
      Serial.println(param); Serial.println("\n");
      Serial.println("Change param up");
    }
    if( Ps3.event.button_down.down ){
      param += -1;
      Serial.println(param); Serial.println("\n");
      Serial.println("Change param down");
    }
    if( Ps3.event.button_down.right ){
      D10 *= 2.;
      Serial.println(D10);Serial.println("\n");
      Serial.println("Change Delay up");
    }
    if( Ps3.event.button_down.left ){
      D10 *= 0.5;
      Serial.println(D10);Serial.println("\n");
      Serial.println("Change Delay down");
    }
    
     //---------- Analog shoulder/trigger button events ----------
     if( abs(Ps3.event.analog_changed.button.l1)>2){
      pk *= 0.9;
      delay(1000);
      Serial.print(pk);
      Serial.print("\n");
      //Serial.print("Pressing the left shoulder button: ");
     }

     //left_stick
     //---------------- Analog stick value events ---------------
     if( abs(Ps3.event.analog_changed.stick.lx) + abs(Ps3.event.analog_changed.stick.ly) > 2 ){
         Serial.print("Moved the left stick:");Serial.print("\n");
         kx = Ps3.data.analog.stick.lx; 
         ky = Ps3.data.analog.stick.ly;
         Serial.print(" kx="); Serial.print(kx);Serial.print("\n");
         Serial.print(" ky="); Serial.print(ky);Serial.print("\n");
         delay(10);
     }

     //right_stick
     if( abs(Ps3.event.analog_changed.stick.rx) + abs(Ps3.event.analog_changed.stick.ry) > 2 ){
         Serial.print("Moved the right stick:");Serial.print("\n");
         lx = Ps3.data.analog.stick.rx;
         ly = Ps3.data.analog.stick.ry;
         Serial.print(" lx="); Serial.print(lx);Serial.print("\n");
         Serial.print(" ly="); Serial.print(ly);Serial.print("\n");
         delay(10);
     }
}

void onConnect(){
    Serial.println("Connected.");
}
```
setup関数はi2c宣言以外は前回と同じ

```srtup().c
void setup() {
  Wire.begin(26,32); //i2cのpin宣言が重要
  Serial.begin(115200);
  
  pwm.begin();
  pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）  

  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.begin("24:a1:60:54:2a:8a"); //PS3コントロール　Atom　Mac
  Serial.println("Ready.");
  param = 0; 
}
```
以下の関数で正弦波的な動作をさせる。

```func_sin().c
float func_sin(float skf){
  return sin(3.1415926535*skf/180);
}
```
#### 前足のswing動作, 後ろ足のswing動作, 前後足のswing動作

```swing().c
void swing(){
  servo_write_func(
    p0_+10*pk*func_sin(5*sk),p1_+10*pk*func_sin(5*sk), p2_,
    p3_+10*pk*func_sin(5*sk),p4_+10*pk*func_sin(5*sk), p5_, 
    p6_, p7_, p8_, 
    p9_, p10_, p11_);
  delay(D10);
}

void swing_back(){
  servo_write_func(
    p0_,p1_, p2_,
    p3_,p4_, p5_, 
    p6_+10*pk*func_sin(5*sk), p7_+10*pk*func_sin(5*sk), p8_, 
    p9_+10*pk*func_sin(5*sk), p10_+10*pk*func_sin(5*sk), p11_);
  delay(D10);
}

void swing_all(){
  servo_write_func(
    p0_+10*pk*func_sin(5*sk),p1_+10*pk*func_sin(5*sk), p2_,
    p3_+10*pk*func_sin(5*sk),p4_+10*pk*func_sin(5*sk), p5_,
    p6_+10*pk*func_sin(5*sk), p7_+10*pk*func_sin(5*sk), p8_, 
    p9_+10*pk*func_sin(5*sk), p10_+10*pk*func_sin(5*sk), p11_);
  delay(D10);
}
```
#### left_stickの読取値kx, kyで動かす
上の時間的増加する変数skをkx, kyに置き換えて動かす

```swing_all().c
void swing_all_(){
  servo_write_func(
    p0_+10*pk*func_sin(1*kx),p1_+10*pk*func_sin(1*kx), p2_,
    p3_+10*pk*func_sin(1*kx),p4_+10*pk*func_sin(1*kx), p5_,
    p6_+10*pk*func_sin(1*ky), p7_+10*pk*func_sin(1*ky), p8_, 
    p9_+10*pk*func_sin(1*ky), p10_+10*pk*func_sin(1*ky), p11_);
  delay(D10);
}
```
#### right_stickの読取値lx, lyで動かす
前後・左右に動くようにする。

```swing_forback().c
void swing_forback(){
  servo_write_func(
    p0_,p1_-10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx)), p2_+10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx)),
    p3_,p4_+10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx)), p5_-10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx)),
    p6_, p7_-10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx)), p8_+10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx)), 
    p9_, p10_+10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx)), p11_-10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx)));
  delay(10);
}
```
#### 動作モード変更
PS3コントローラーのボタンに合わせてparamを変更し、動作モードを変更する。
`calibrate();l_hineri();r_hineri();`の関数は、上記では割愛した。

```loop().c
void loop() {
  if(!Ps3.isConnected())
    return;
  Ps3.setPlayer(player);

  param = constrain(param,0,9);
  if(param ==0){
    calibrate();
    delay(0);
  }
  if(param ==1){
    l_hineri();
    delay(0);
  }
  if(param ==2){
    r_hineri();
    delay(0);
  }
  if(param ==3){
    swing(); 
    delay(0);
  }
  if(param ==4){
    swing_back();
    delay(0);
  }
  if(param ==5){
    swing_all(); 
    delay(0);
  }
  if(param ==6){
    swing_all_(); 
    delay(0);
  }
  if(param ==7){
    swing_forback();
    delay(0);
  }

  sk += 1;
}

//MangDang社サーボ利用の出力
void servo_write(int ch, int ang){
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); 
  pwm.setPWM(ch, 0, ang);
}
```
# MPU6050を使う
【参考】
・[【arduino入門】esp8266でServoとMPU9250センサーで追跡カメラを制御する♪〜inoも関数化とclass化をする](https://qiita.com/MuAuan/items/e5a8d2a57482b0551bb4)
のmpu9250_esp8266のコードより必要なコードを抜き出すと以下の通り。
**温度を削除。また地磁気を除くとMPU9250とMPU6050は同一の関数で動作する。**

```c
...
//MPU params
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw; //, Temperature;
float acc_x0=0, acc_y0=0, acc_z0=0;
float acc_x=0, acc_y=0, acc_z=0;
float gyro_x=0, gyro_y=0, gyro_z=0;
float rawX, rawY, rawZ;
float pitch, roll;
...

void setup() {
  Wire.begin(26,32); //i2cのpin宣言が重要
  Serial.begin(115200);
  setupMPU9250();
  readData();
  acc_x0=acc_x;
  acc_y0=acc_y;
  acc_z0=acc_z-1;
  ...  
}

void get_angle(){
  rawX = acc_x-acc_x0;
  rawY = acc_y-acc_y0;
  rawZ = acc_z-acc_z0;
  pitch = atan2(rawX, sqrt(rawY*rawY+rawZ*rawZ))* 180.0 / PI;
  roll = atan2(rawY, rawZ)* 180.0 / PI;
}

int16_t u2s(int16_t unsigneddata){
  int16_t usd = unsigneddata;
  if (usd & (0x01 << 15)){ 
      usd = -1 * ((unsigneddata ^ 0xffff) + 1);
  }
  return usd;
}

void readData() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 14, true);
  while (Wire.available() < 14);

  axRaw = u2s(Wire.read() << 8 | Wire.read());
  ayRaw = u2s(Wire.read() << 8 | Wire.read());
  azRaw = u2s(Wire.read() << 8 | Wire.read());
  //Temperature = u2s(Wire.read() << 8 | Wire.read());
  gxRaw = u2s(Wire.read() << 8 | Wire.read());
  gyRaw = u2s(Wire.read() << 8 | Wire.read());
  gzRaw = u2s(Wire.read() << 8 | Wire.read());

  // 加速度値を分解能で割って加速度(G)に変換する
  acc_x = (2. / 32768.)*axRaw ;  //FS_SEL_0 16,384 LSB / g
  acc_y = (2. / 32768.)*ayRaw ;
  acc_z = (2. / 32768.)*azRaw ;

  // 角速度値を分解能で割って角速度(degrees per sec)に変換する
  gyro_x = (250. / 32768.)*gxRaw ;  // (度/s)
  gyro_y = (250. / 32768.)*gyRaw ;
  gyro_z = (250. / 32768.)*gzRaw ;
  //temperature
  //temp= (Temperature-4040)/340. + 29.0; //室温29.0Cで校正
}

void setupMPU9250() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);  // 0; 内部クロックを設定
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);  // 
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x37);
  Wire.write(0x02);
  Wire.endTransmission();

  Wire.beginTransmission(0x0C);
  Wire.write(0x0A);
  Wire.write(0x16);
  Wire.endTransmission();
  delay(500);
}
```
# MPU6050で姿勢制御
以下のコードで二軸のPID制御ができる。
それぞれの方向のKp, Ki, Kdの値は、ほぼジーグラー・ニコルスのように
・Kpで持続振動させて、周期計測
・だいたい決まったら、好みの反応になるようにKi, Kdを動かす
という手法で決められました。
なお、pitchの符号が-だったので、演算で修正しています。
そして、最大最小値をlx, lyの実測値から120としています。
また、**sin関数で反応を修正していますが、これが最適かどうかはさらに調整が必要だと考えています。**
同様に、最大角度もpk=2と上の範囲(pk=1)より大きくしています。
【参考】
・[【PID入門】制御して遊んでみた♬](https://qiita.com/MuAuan/items/ee8ee095dfe01fcece7b)

```swing_mpu().c
void swing_mpu(){
  pk=2;  //反応角度の調整
  dt = (micros()-preTime)/1000000. ;
  preTime = micros();

  P_r  = (target_r - roll) ; /// 90;      // 規格化無し
  I_r += P_r * dt;                    // 偏差を積分する
  D_r  = (P_r - preP_r) / dt;           // 偏差を微分する
  preP_r = P_r;                       // 偏差を記録する
  if (abs(Ki*I_r)>120) I_r=0;

  P_p  = -(target_p - pitch); // / 90;      // 規格化無し
  I_p += P_p * dt;                    // 偏差を積分する
  D_p  = (P_p - preP_p) / dt;           // 偏差を微分する
  preP_p = P_p;                       // 偏差を記録する
  if (abs(Ki*I_p)>120) I_p=0;

  float lx_, ly_;

  lx_ = Kp * P_r + Ki * I_r + Kd * D_r;
  lx_ = constrain(lx_, -120, 120);  // ±120に制限
  ly_ = (Kp_ * P_p + Ki_ * I_p + Kd_ * D_p);
  ly_ = constrain(ly_, -120, 120);  // ±120に制限
  
  Serial.print(" roll="); Serial.print(roll);
  Serial.print(" pitch="); Serial.print(pitch);Serial.print("\n");
  Serial.print(" lx_="); Serial.print(lx_);
  Serial.print(" ly_="); Serial.print(ly_);Serial.print("\n");

  servo_write_func(
    p0_,p1_-10*pk*func_sin(1*(ly_))-10*pk*func_sin(1*(lx_)), p2_+10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx_)),
    p3_,p4_+10*pk*func_sin(1*(ly_))-10*pk*func_sin(1*(lx_)), p5_-10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx_)),
    p6_, p7_-10*pk*func_sin(1*(ly_))+10*pk*func_sin(1*(lx_)), p8_+10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx_)), 
    p9_, p10_+10*pk*func_sin(1*(ly_))+10*pk*func_sin(1*(lx_)), p11_-10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx_)));
  delay(10*D10);
}
```
#### MPU6050で姿勢制御しつつ歩行させる
前進の関数は、前回から少し改善して、現在は以下の通りです。

```walk().c
void walk(){
  st = 32;
  st2 =st*0.5;
  st4 = st*0.05;
  //pk=1;
  servo_write_func(
    p0_-10-0.5*pk*(st2-int(sk-st2)%st),2*pk*(st2-int(sk-st2)%st)+p1_, 1*pk*(st2-int(sk-st2)%st)+p2_,
    p3_+10-0.5*pk*(st2-int(sk-st2)%st),2*pk*(sk%st-st2)+p4_, 1*pk*(sk%st-st2)+p5_,
    p6_,-2*pk*(int(sk-st4)%st-st2)+p7_, -1*pk*(int(sk-st4)%st-st2)+p8_,
    p9_,-2*pk*(st2-int(sk-st2-st4)%st)+p10_, -1*pk*(st2-int(sk-st2-st4)%st)+p11_);
  delay(D10);
}
```
この歩行の時時刻刻変化するp0_等を新しいp0_wとして、姿勢制御の関数に代入すれば、姿勢制御しつつ前進することができます。

```walk_mpu().c
void walk_mpu(){
  float p0_w,p1_w,p2_w,p3_w,p4_w,p5_w,p6_w,p7_w,p8_w,p9_w,p10_w,p11_w;
  st = 32;
  st2 =st*0.5;
  st4 = st*0.05;
  pk=0.5;

  
  dt = (micros()-preTime)/1000000. ;
  preTime = micros();

  P_r  = (target_r - roll) ; /// 90;      // target - roll -90~90を取るので180で割って-1.0~1.0にする
  I_r += P_r * dt;                    // 偏差を積分する
  D_r  = (P_r - preP_r) / dt;           // 偏差を微分する
  preP_r = P_r;                       // 偏差を記録する
  if (abs(Ki*I_r)>120) I_r=0;

  P_p  = -(target_p - pitch); // / 90;      // target - roll -90~90を取るので180で割って-1.0~1.0にする
  I_p += P_p * dt;                    // 偏差を積分する
  D_p  = (P_p - preP_p) / dt;           // 偏差を微分する
  preP_p = P_p;                       // 偏差を記録する
  if (abs(Ki*I_p)>120) I_p=0;

  float lx_, ly_;

  lx_ = Kp * P_r + Ki * I_r + Kd * D_r;
  lx_ = constrain(lx_, -120, 120);  // ±120に制限
  ly_ = (Kp_ * P_p + Ki_ * I_p + Kd_ * D_p);
  ly_ = constrain(ly_, -120, 120);  // ±120に制限
  
  Serial.print(" roll="); Serial.print(roll);
  Serial.print(" pitch="); Serial.print(pitch);Serial.print("\n");
  Serial.print(" lx_="); Serial.print(lx_);
  Serial.print(" ly_="); Serial.print(ly_);Serial.print("\n");

  p0_w=p0_-10-0.5*pk*(st2-int(sk-st2)%st); p1_w=2*pk*(st2-int(sk-st2)%st)+p1_, p2_w=1*pk*(st2-int(sk-st2)%st)+p2_;
  p3_w=p3_+10-0.5*pk*(st2-int(sk-st2)%st); p4_w=2*pk*(sk%st-st2)+p4_; p5_w= 1*pk*(sk%st-st2)+p5_;
  p6_w=p6_; p7_w=-2*pk*(int(sk-st4)%st-st2)+p7_; p8_w= -1*pk*(int(sk-st4)%st-st2)+p8_;
  p9_w=p9_; p10_w=-2*pk*(st2-int(sk-st2-st4)%st)+p10_; p11_w= -1*pk*(st2-int(sk-st2-st4)%st)+p11_;
  servo_write_func(
    p0_w,p1_w-10*pk*func_sin(1*(ly_))-10*pk*func_sin(1*(lx_)), p2_w+10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx_)),
    p3_w,p4_w+10*pk*func_sin(1*(ly_))-10*pk*func_sin(1*(lx_)), p5_w-10*pk*func_sin(1*(ly))+10*pk*func_sin(1*(lx_)),
    p6_w, p7_w-10*pk*func_sin(1*(ly_))+10*pk*func_sin(1*(lx_)), p8_w+10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx_)), 
    p9_w, p10_w+10*pk*func_sin(1*(ly_))+10*pk*func_sin(1*(lx_)), p11_w-10*pk*func_sin(1*(ly))-10*pk*func_sin(1*(lx_)));
    
  delay(10*D10);
}
```
次回は、距離計測しつつ、自律走行をさせます。