【倒立振り子入門】ArduinoNanoとm5stickcをそれぞれ利用して、PID制御で倒立振り子を立てる♪

今回は、参考の記事におんぶにだっこな内容です.
悪戦苦闘な末、やっと自律倒立振り子が静止しました。
今回は、参考①のarduino nanoと参考②のm5stickcでやってみた記録です。

#自律倒立振り子
遂に止まった‼️ pic.twitter.com/V4AWTx05BJ

— ウワン (@MuAuan) May 30, 2021

【参考】
①Arduino nanoと6軸センサMPU6050と連続回転サーボFS90Rで倒立振子を作った
②BalaC(M5StickC内蔵倒立振子)をPID制御で立たせる

ArduinoNanoで立てる

ほぼ、先日の追跡カメラのアプリと同様なコードです。
異なるのは、以下の二点
1.servo1とservo2にほぼ同一のパワーを出力し、車輪を回す
2.パワーはPID制御する
そして、倒立振り子はブレッドボード、360度連続回転servoそしてタイヤ,mpu9250,arduino nanoで作成しました。
結線は、以下のとおりです.

Arduino nano	mpu9250	servo	LIPO
GND	GND	GND	LIPO(-)
5v			LIPO0(+)
3.3V	3.3v
SCL	SCL
SDA	SDA
myservo1.attach(10)	-	Servo1(yellow)	-
		Servo1(red)	LIPO1(+)
		Servo1(black)	LIPO1(-)
myservo2.attach(11)	-	Servo2(yellow)	-
		Servo1(red)	LIPO2(+)
		Servo1(black)	LIPO2(-)

パワーはPID制御する

車輪を回転させるのは、特に問題ないと思うのでPID制御パラメタ−の決定のしかたを記載する。
ここでは、無闇に変更するのではなく、参考③を頼りに次の手順とした。
1.P制御のみで継続的な持続振動になるまでKpをあげる
2.持続振動がおこりはじめるKpの値をKuとし、その周期Tu（だいたい10周期/secより0.1secとする）を元にして、I制御及びD制御の係数を以下のとおり決定する

K_p=K_u*0.6 \\
K_i=\frac{K_u*1.2}{T_u} \\
K_d=\frac{3*Ku*Tu}{40}

3.この係数を出発点として、二分法にて40やTuを増減させてちょうど良い係数を探す
この方法だと、一応の目安があるので、系に最適な係数が見つかるだろうと思う。
なお、最初の持続振動が得られると、前方か後方へ倒れるので
4.平衡点targetの値をそちらに動かしてできるだけ倒れにくい角度にすることを推奨する；当初平衡点をmpu9250で計測してその値を設定した
5.そもそも上記の倒立振り子の車体から分かると思うが、重心は適度に安定するように電源などを配置することをおすすめする

因みに制御無しなら...倒れる(笑)

— ウワン (@MuAuan) May 30, 2021

【参考】
③【PID入門】制御して遊んでみた♬

ArduinoNano倒立振り子コード

#include "Wire.h"
#include <Servo.h>         // サーボモータを動かすライブラリ
Servo myservo1;             
Servo myservo2;             

float power1 = 0;            // サーボの出力　sg９０
float power2 = 0;
float power = 0;              
float target = 76; //平衡点を探す;point1
float P, I, D, preP;

//ジーグラー・ニコルズの手法で決定後Kd大きめに設定
float Kp = 270*0.6;       //point2 // Pゲイン 全体的なモータのパワーに効く
float Ki = 270*1.2/0.12;  // Iゲイン バランスが崩れて全体が平行移動しだしたときに効く
float Kd = 3*270*0.12/18;  //point3 // Dゲイン 激しく動いたときのブレーキの役割をする

int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, Temperature;
float rawX, rawY, rawZ;
float acc_x=0, acc_y=0, acc_z=0;
float gyro_x=0, gyro_y=0, gyro_z=0;
float roll;
float dt, preTime;
#include <MadgwickAHRS.h>   // 角度と角速度にMadgwickフィルタをかけるライブラリ
Madgwick MadgwickFilter;    // Madgwickフィルタのオブジェクトを設定

void setup() {
  Serial.begin(115200);     //シリアル通信を115200bpsに設定
  myservo1.attach(10);       // 10番ピンに接続
  myservo2.attach(11);       // 11番ピンに接続

  Wire.begin();
  setupMPU9250();
  preTime = micros();
  MadgwickFilter.begin(100);  // Madgwickフィルタの周波数を100Hzに設定。
}

void loop() {
  readData(); //gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_zを取得
  MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
  float roll  = MadgwickFilter.getRoll(); //rollのみ使う

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

  P  = (target - roll) / 90;      // -90~90を取るので90で割って-1.0~1.0にする
  I += P * dt;                    // 偏差を積分する
  D  = (P - preP) / dt;           // 偏差を微分する
  preP = P;                       // 偏差を記録する
  if (abs(Ki*I)>90) I=0;
  power = Kp * P + Ki * I + Kd * D;
  power = constrain(power, -90, 90);  // ±90に制限
  Serial.print("power =\t");  Serial.print(power);  Serial.print("\n");

  power2 = 90 - power; //point4
  power1 = 90 + power; //回転方向注意

  myservo1.write(power2); //Servoは角度入力をPWM変換して出力
  myservo2.write(power1);
  //delay(4); //sampling rateを変える
}

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 ;
}

//初期化関数は、参考の解説みてください
void setupMPU9250() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  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);
}

m5stickcで立てる

当初の予想では、こちらの方が安定しているように見えたので簡単だと思ったが、ほぼ同様な苦労をした。

上記とことなり、m5stickcではPWMの出力が1個なのだが、前回の参考④のとおり、ledcWriteを使うと４個制御できる。
結線

m5stickc	mpu6886(内蔵)	servo	LIPO
GND	GND（内部）	GND	LIPO(-)
5v			LIPO0(+)
3.3V	内部
SCL	SCL（内部）
SDA	SDA（内部）
pinMode(32, OUTPUT)	-	Servo1(yellow)
		Servo1(red)	LIPO1(+)
		Servo1(black)	LIPO1(-)
pinMode(33, OUTPUT)	-	Servo2(yellow)
		Servo1(red)	LIPO2(+)
		Servo1(black)	LIPO2(-)

※mpu6886(内蔵)は特に結線しなくともM5.MPU6886.Init();などで計測できる
※m5stickは内蔵Batで少しの時間動くが、LIPOでGNDと5Vから電源供給して動かした

【参考】
④【M5StickC入門】内蔵MPU6886と１６chServoコントローラPCA9685を利用して追跡カメラを制御する♪
ということで、参考④の追跡カメラはPCA9685を利用したが、今回はコンパクトにしたいので、このledcWriteを利用した。
そのために、FS90R用に以下の関数を作成した。

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);
}

こちらは、止まるところまで制御できていないが、コードはかなりシンプルで読みやすい思う。

m5stickc倒立振り子コード

#include <M5StickC.h>

int PIN0 = 32;
int PIN1 = 33;
int PWMCH = 0;
int PWMCH1 = 1;

float power1 = 0;            // サーボの出力
float power2 = 0;
float power = 0;              
float target = 54; //46// 83.0;  
float P, I, D, preP, dt;

float Kp = 180*0.6;
float Ki = 180*1.2/0.12;
float Kd = 3*180*0.12/18;

float roll, pitch, yaw;
float preTime;

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

  pinMode(PIN0, OUTPUT);
  pinMode(PIN1, OUTPUT);
  ledcSetup(PWMCH, 50, 12); //12bit
  ledcAttachPin(PIN0, PWMCH);
  ledcSetup(PWMCH1, 50, 12); 
  ledcAttachPin(PIN1, PWMCH1);

  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();
  preTime = micros();                   // 時間記録
}

void loop() {
  M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);

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

  P  = (target - roll) / 90;      // -90~90を取るので180で割って-1.0~1.0にする
  I += P * dt;                    // 偏差を積分する
  D  = (P - preP) / dt;           // 偏差を微分する
  preP = P;                       // 偏差を記録する
  if (abs(Ki*I)>90) I=0;

  power = Kp * P + Ki * I + Kd * D;
  power = constrain(power, -90, 90);  // ±90に制限
  M5.Lcd.setCursor(0, 15);
  M5.Lcd.print("power =\t");  M5.Lcd.print(power);  M5.Lcd.print("\n");
  Serial.print("power =\t");  Serial.print(power);  Serial.print("\n");

  power1 = 0 - power;
  power2 = 0 + power;
  servo_write(PWMCH, power1); 
  servo_write(PWMCH1, power2);

  Serial.printf("%5.1f\n",roll); Serial.print("\n");
  M5.Lcd.printf("%5.1f\n",roll); M5.Lcd.print("\n");
  //delay(10);
}

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);
}

まとめ

・無事に自律倒立振り子が立った
・ArduinoNanoとm5stickcでやってみた

・ロボット作って同じように立ててみたい