M5StickCで倒立振子２モータ化（I2CモータードライブDRV8830）その９ Blynkでコントロール

こんにちはRoboTakaoです。
これまでにM5StickCで作る倒立振子を作って来ましたが、最後に前後左右をBlynkでコントロールできるようにして見ました。



Blynkのジョイスティックで動作します。





なかなかコントロールは難しいです。
またゲイン調整しましたが、外乱には弱いですね。
改善の余地は多々ありますね。



それではこの辺で失礼します。

スケッチ

#define BLYNK_PRINT Serial
#define BLYNK_USE_DIRECT_CONNECT

#include <M5StickC.h>
#include <BlynkSimpleEsp32_BLE.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <Wire.h>

#define MPU6886_AFS M5.MPU6886.AFS_2G       // Ascale [g]      (±2,4,8,16)
#define MPU6886_GFS M5.MPU6886.GFS_250DPS  // Gscale [deg/s]  (±250,500,1000,200)

char auth[] = "jZpGVzEJgw4g3SkzJkdpeAgdTGvRWAoW";  //メールで送られるAuth Token

int VMAX = 60;
int VMIN = 1;

float x = 0.0;
float y = 0.0;
float mXL = 0.0, mXR = 0.0, mY = 0.0;

float Kp = 2.5;
float Ki = 30.0;
float Kd = 0.06;
float target;
float target0 = -89.5;
float P, I, D, preP;
float aR = 0.85;
float aL = 1.0;

float power = 0;
float powerL = 0, powerR = 0;
float dt, preTime;
float TempTime;

bool started = false;

float roll, pitch, yaw;
float now;
int roll_initial;
int roll_initial_count = 100;

void drawScreen() {
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.printf("T:%6.1f\n",target0);
    // 目標角度(オフセット込み)
    M5.Lcd.printf("roll:%6.1f\n",roll);
    // 現在のロール角
    M5.Lcd.printf("nowL:%4.1f\n", target - roll);
    // 現在の偏差
    M5.Lcd.printf("PowerL:%5.1f\n", power);
    // PID出力
    M5.Lcd.printf("Kp:%5.1f\n", Kp);
    M5.Lcd.printf("Ki:%5.1f\n", Ki);
    M5.Lcd.printf("Kd:%5.1f\n", Kd);
    M5.Lcd.printf("Bat:%5.1fV\n", M5.Axp.GetBatVoltage());// バッテリー電圧
}

const int motor_R = 0x60;
const int motor_L = 0x64;

//モータドライバ I2C制御 motor driver I2C
void writeMotorResister(byte motor_adress, byte vset, byte data1){
  int vdata = vset << 2 | data1;
  Wire.beginTransmission(motor_adress);
  Wire.write(0x00);
  Wire.write(vdata);
  Wire.endTransmission(true);
}

//ヴァーチャルピンデータ受信
BLYNK_WRITE(V0) {
  Kp = param[0].asFloat();
}

BLYNK_WRITE(V1) {
  Ki = param[0].asFloat();
}

BLYNK_WRITE(V2) {
  Kd = param[0].asFloat();
}

BLYNK_WRITE(V3) {
  VMAX = param[0].asInt();
}

BLYNK_WRITE(V4) {
  target0 = param[0].asInt();
}

//ジョイスティックのデータ受信
BLYNK_WRITE(V5) {
  x = param[0].asInt();
  y = param[1].asInt();

  if(fabs(x) > 8){   //xが8以上で左右回転
    if(x > 0){
      mY = 0.0;  //左右回転でバランスを調整できるようにした
      mXL = 0.5*x;
      mXR = -0.5*x;
    }else{
      mY = 1.0;  //左右回転でバランスを調整できるようにした
      mXL = 0.5*x;
      mXR = -0.5*x;
    }
  }else if(fabs(y) > 10){     //yが10以上で前後進
    mY = 0.085*y;
    mXL = 0;
    mXR = 0;
  }else{
    mY = 0;
    mXL = 0;
    mXR = 0;
  }
}

void Initial_Value(){
  Blynk.virtualWrite(V0, Kp);
  Blynk.virtualWrite(V1, Ki);
  Blynk.virtualWrite(V2, Kd);
  Blynk.virtualWrite(V3, VMAX);
  Blynk.virtualWrite(V4, target0);
}

//DRV8830 Fault時にリセット
  uint8_t readMotorStatus(byte motor_adress) {
  uint8_t result = 0x00;
    
  Wire.beginTransmission(motor_adress);
  Wire.write(0x01); //  read register 0x01
  Wire.endTransmission();

  Wire.requestFrom(motor_adress, 1);
  if (Wire.available()) {
    result = Wire.read();
  } else {
 //   Serial.println("No status data");
  }

  return result;
}

void resetMotorStatus(byte motor_adress) {  //DRV8830リセット
  Wire.beginTransmission(motor_adress);
  Wire.write(0x01); //  fault
  Wire.write(0x80);  // clear
  Wire.endTransmission(true);
}

void setup() {
  Serial.begin(151200);

  pinMode(GPIO_NUM_10, OUTPUT);
  digitalWrite(GPIO_NUM_10, HIGH);

  M5.begin();
  M5.Lcd.setRotation(2);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Axp.ScreenBreath(8);
  M5.MPU6886.Init();
  M5.MPU6886.SetGyroFsr(MPU6886_GFS);
  M5.MPU6886.SetAccelFsr(MPU6886_AFS);
  Blynk.setDeviceName("Blynk");
  Blynk.begin(auth);

  Wire.begin(0, 26, 400000); //SDA, SCL
  writeMotorResister(0x00, 0x00, 0x00);
  
  drawScreen();
}

void loop() {
  M5.update();
  Blynk.run();

  int status_R = readMotorStatus(motor_R); //Motor Fault -> reset
  if (status_R & 0x01) {
    Serial.print("Right Motor Fault : ");
    Serial.println(status_R, HEX);
    resetMotorStatus(motor_R);
  }

  int status_L = readMotorStatus(motor_L); //Motor Fault -> reset
  if (status_L & 0x01) {
    Serial.print("Left Motor Fault : ");
    Serial.println(status_L, HEX);
    resetMotorStatus(motor_L);
  }
  
  if ( M5.BtnB.wasReleased() ) {
    // Bボタンを押すと再起動
    esp_restart();
  }

  if ( M5.BtnA.wasReleased() ) {
    Serial.println("START");
    digitalWrite(GPIO_NUM_10, LOW);
    Initial_Value();

    //roll_initail_count 分だけ捨てる
    for(int j=1; j      M5.MPU6886.getAhrsData(&pitch,&roll,&yaw);
      delay(10);
    }

    //roll_initail_count 分だけ初期値として平均値を求める
    for(int i=1; i<=roll_initial_count; i++){
      M5.MPU6886.getAhrsData(&pitch,&roll,&yaw);
      roll_initial += roll;
      delay(10);
    }
    roll_initial /= roll_initial_count;

    digitalWrite(GPIO_NUM_10, HIGH);
    preTime = micros();
    started = true;
  }

  if (!started) {
    return;
  }

  M5.MPU6886.getAhrsData(&pitch,&roll,&yaw);
  roll = roll - (90 + roll_initial);

  target = target0 - mY;
  now = target - roll;
  TempTime = micros();
  dt = (TempTime - preTime) / 1000000; // 処理時間を求める
  preTime = TempTime; // 処理時間を記録

// 目標角度から現在の角度を引いて偏差を求める
// -90~90を取るので90で割って-1.0~1.0にする
  P  = (target - roll) / 90;
// 偏差を積分する
  I += P * dt;
// 偏差を微分する
  D  = (P - preP) / dt;
// 偏差を記録する
  preP = P;
// 出力を計算する
  power = Kp * P + Ki * I + Kd * D;
// 出力を-100.0~100.0の範囲に収める
  if (power < -100.0) {
    power = -100.0;
  }
  if (100.0 < power) {
    power = 100.0;
  }
  
  if (power < -100.0) {
    power = -100.0;
  }
  if (100.0 < power) {
    power = 100.0;
  }
  Serial.print("power:");
  Serial.print(power);
  
// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
  if (1 < abs(I * Ki)) {
    I = 0;
    Serial.println("anti windup");
    // anti windup
  }
  powerL = power + mXL/90;
  powerR = power + mXR/90;
  int Vdata_R = aR * (((float)(VMAX - VMIN) * fabs(powerL)) + VMIN);
  int Vdata_L = aL * (((float)(VMAX - VMIN) * fabs(powerR)) + VMIN);
  
  if(powerL > 0){
    writeMotorResister(motor_L, byte(Vdata_L), 0x01);
  }else if(powerL < 0){
    writeMotorResister(motor_L, byte(Vdata_L), 0x02);
  }
  if(powerR > 0){
    writeMotorResister(motor_R, byte(Vdata_R), 0x01);
  }else if(powerR < 0){
    writeMotorResister(motor_R, byte(Vdata_R), 0x02);
  }
  
  drawScreen();

  delay(10);
}