前回、倒立振り子のほぼ静止制御までできたが、今回は以下二点を目的にPS3コントローラを利用して実施した。
1.　m5sickc制御の静止制御パラメータを決定する
2.　回転と前進後進を制御する
今回もPS3コントローラの利用法は、参考のとおりである。
なお、このアイディアは参考②でwifi利用してPID制御パラメータを変更してうまくできていることからやってみました。また、①ではRoverCを動かしています。
※PS3コントローラは購入もしましたが、古いものでも動きました。PS4のコントローラーは反応はしましたが、キー配置が異なり動かせませんでした。
【参考】
①ESP32(M5StickC)をPS3コントローラで操作する
②BalaC(M5StickC内蔵倒立振子)をPID制御で立たせる
###やったこと
・PS3コントローラへのMacアドレス書き込み
・ESP32ボードのPS3コントローラー制御
・m5stickcボードの制御
###・PS3コントローラへのMacアドレス書き込み
まず、参考③からダウンロードして、動かしてみる。
環境は、Windows10で実施しました。Raspi4ではダウンロードは出来ますが、動きませんでした。
※ここで、このページはWindowsで開くと余分なアプリのインストールを進められたりしますが、当該アプリのインストーラをダウンロードして実行しましょう。
このアプリは、PS３コントローラへマイコンボードのMacアドレスを書き込むソフトです。インストール出来れば、単にPS3コントローラを接続すれば現在の接続先のMacアドレスが表示されるので、新たな接続先のMacアドレスを入力してupdateすれば書き込みできます。
【参考】
③Download SixaxisPairTool by Dancing Pixel Studios
####マイコンボードのMacアドレスを読む
この作業は、接続先のマイコンボード毎に実施する必要があります。
inoをマイコンに書き込んで実行すればシリアルモニタに出力されます。
※なお、以下のプログラムは、ArduinoIDEのライブラリ管理でPS3と入力するとPackageが出力するので、インストールすれば
スケッチ例ーPS3ControlerHost-PS3Addressで利用できます。
ただし、シリアルモニターに出現しない場合があったので、以下のようにloop内で連続的に読むように変更しました。

#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: 7c:9e:bd:f4:be:2e esp32old
    //The ESP32's Bluetooth MAC address is: 7c:9e:bd:f4:f5:96 esp32
    //The ESP32's Bluetooth MAC address is: 94:b9:7e:a2:7c:22 mstickc

###・ESP32ボードのPS3コントローラー制御
いよいよ本編。
PS3コントローラーを利用してパラメータを変更します。
参考①と同様に、スケッチ例のPS3Demoを動かしてみると、今回利用できそうなものとして
//--- Digital cross/square/triangle/circle button events ---
//--------------- Digital D-pad button events --------------
などがあります。
ということで、以下のコードのようにこれらを使ってみます.
参考①では、notify()で出来なかったと記載がありますが、ここでは利用できました。
なお、loop()に
if(!Ps3.isConnected()) return;
を記載すると、PS3コントローラーが接続しないと開始しないのがいやなので、あえて外しています.
ということで、crossとかでKuとTdの値を変更して、PID制御のパラメータを変更できるようにしました。また、PS3コントローラーの左側に配置されている十字ボタンでpower0, power0_を変更して、前進後進、そして回転制御ができるようにしました。

#include <Ps3Controller.h>

int player = 0;

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.cross ) {
      Ku *=1.1;
      Serial.println(Ku);
      Serial.println("Change Ku up");
    }
    ...
    //--------------- Digital D-pad button events --------------
    if( Ps3.event.button_down.up ){
      power0 +=1;
      Serial.println(power0);
      Serial.println("Change power0 up");
    }
    ...
    Kp = Ku*0.6; 
    Ki = Ku*1.2/0.12;
    Kd = 3*Ku*0.12/Td;
}

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

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

  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.begin("7c:9e:bd:f4:f5:96");
...
}

void loop() {
...
}

esp32の倒立振り子PID制御コード

touritu_esp32.ino.c

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

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

//ジーグラー・ニコルズの手法で決定後Kd大きめに設定
float Ku = 150;
float Td = 18;
float Kp = Ku*0.6;       //point2 // Pゲイン 全体的なモータのパワーに効く
float Ki = Ku*1.2/0.12;  // Iゲイン バランスが崩れて全体が平行移動しだしたときに効く
float Kd = 3*Ku*0.12/Td;  //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フィルタのオブジェクトを設定

#include <Ps3Controller.h>

int player = 0;
//int battery = 0;


void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.cross ) {
      Ku *=1.1;
      Serial.println(Ku);
      Serial.println("Change Ku up");
    }
    if( Ps3.event.button_down.square ){
      Td *=1.1;
      Serial.println(Td);
      Serial.println("Change Td up");
    }
    if( Ps3.event.button_down.triangle ){
      Ku *=0.9;
      Serial.println(Ku);
      Serial.println("Change Ku down");
    }
    if( Ps3.event.button_down.circle ){
      Td *=0.9;
      Serial.println(Td);
      Serial.println("Change Td down");
    }
    
    //--------------- 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");
    }
    Kp = Ku*0.6; 
    Ki = Ku*1.2/0.12;
    Kd = 3*Ku*0.12/Td;
}

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

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

  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.begin("7c:9e:bd:f4:f5:96");
  myservo1.attach(0);       // 0番ピンに接続
  myservo2.attach(2);       // 2番ピンに接続

  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に制限

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

  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ボードの制御
PS3コントローラーで制御するために追加するコードはほとんど同一です。ここではm5stickcの画面にパラメータを出力できるので、USB線をはずした状態でも、パラメータの変化がみられて便利です。
ということで、結果は以下のとおりになりました.

表示もさせて、ほぼ永遠に(電池持てば)安定🎵
右回転左回転も出来ました....前後がいまいちです#m5stickc倒立振り子 pic.twitter.com/MeNGKaMEyd
— ウワン (@MuAuan) June 4, 2021

上記の結果のとおり、やっと静止制御ができました。さらに、回転も制御出来ているようです. 一方、前進・後進は今ひとつな感じです.

m5stickcの倒立振り子PID制御コード

touritu_m5stick.ino.c

#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 power0 = -4;
float power0_ = -5;              
float target = 54; //46// 83.0;  
float P, I, D, preP, dt;

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

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

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.cross ) {
      Ku *=1.1;
      Serial.println(Ku);
      Serial.println("Change Ku up");
    }
    if( Ps3.event.button_down.square ){
      Td *=1.1;
      Serial.println(Td);
      Serial.println("Change Td up");
    }
    if( Ps3.event.button_down.triangle ){
      Ku *=0.9;
      Serial.println(Ku);
      Serial.println("Change Ku down");
    }
    if( Ps3.event.button_down.circle ){
      Td *=0.9;
      Serial.println(Td);
      Serial.println("Change Td down");
    }
    
    //--------------- 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");
    }
    Kp = Ku*0.6;       
    Ki = Ku*1.2/0.12;  
    Kd = 3*Ku*0.12/Td;  
    M5.Lcd.setCursor(20, 30);
    M5.Lcd.printf("Ku%5.1f,Td%5.1f\n",Ku,Td); M5.Lcd.print("\n");
    M5.Lcd.setCursor(20, 50);
    M5.Lcd.printf("forback%5.1f,LR%5.1f\n",power0,power0_); M5.Lcd.print("\n");
    
}

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

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

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

  power1 = 0 - power -power0+power0_; //point4
  power2 = 0 + power +power0+power0_; //回転方向注意
  servo_write(PWMCH, power1); 
  servo_write(PWMCH1, power2);

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

###まとめ・PS3コントローラーを利用してPID制御パラメータを変更し、最適化した・同様にPowerに追加で、Power0とPower0_を追加し、追加量を変更することにより左右回転することを確認した。前進行進は動くことは確認できるが制御できていない。

・静止制御を目標としたが、もっとアクティブに運動させてみたいと思う
・制御をPowerで実施したが、バランス（やtarget角度）などを利用してやってみようと思う