M5StickCで倒立振子２モータ化（I2CモータードライブDRV8830）その３

こんにちはRoboTakaoです。

引き続きM5StickCで作る倒立振子2モータ版です。




前回と前々回の記事はこちらになります。

今回は試しに動かしていきたいと思います。

ハード構成は前々回の投稿を見てください。

ただし、モーターだけ交換しました。どうもモーターのバラツキなのか、左右で速度に差が生まれてしまっていました。
これで左右差が少なくなりました。






参考にしているのはこちらとこちらのサイトです。
//https://homemadegarbage.com/bala03
//https://qiita.com/coppercele/items/e4d71537a386966338d0

まだまだ立ったとは言い難いです。
PIDのゲイン調整が必要ですが、おそらくハード側もいじらないといけないだろうと考えています。



スケッチ

#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 = 20;
int VMIN = 5;

float Kp = 8.0; //PIDゲインはまだまだ調整必要
float Ki = 0.1;
float Kd = 0.0;
float target = -90.4;
float P, I, D, preP;

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

bool started = false;

float roll, pitch, yaw;
float now;

void drawScreen() {
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.printf("T:%6.1f\n",target);
    // 目標角度(オフセット込み)
    M5.Lcd.printf("roll:%6.1f\n",roll);
    // 現在のロール角
    M5.Lcd.printf("now:%4.1f\n", target - roll);
    // 現在の偏差
    M5.Lcd.printf("Power:%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 vset_R, byte vset_L, byte data1){
  int vdata_R = vset_R << 2 | data1;
  int vdata_L = vset_L << 2 | data1;
  Wire.beginTransmission(motor_R);
  Wire.write(0x00);
  Wire.write(vdata_R);
  Wire.endTransmission(true);

  Wire.beginTransmission(motor_L);
  Wire.write(0x00);
  Wire.write(vdata_L);
  Wire.endTransmission(true);
}

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

BLYNK_WRITE(V1) {
  Ki = param[0].asFloat();
  Serial.print("Ki:");
  Serial.println(Ki);
}

BLYNK_WRITE(V2) {
  Kd = param[0].asFloat();
  Serial.print("Kd:");
  Serial.println(Kd);
}

BLYNK_WRITE(V3) {
  VMAX = param[0].asInt();
  Serial.print("VMAX");
  Serial.println(VMAX);
}

BLYNK_WRITE(V4) {
  target = param[0].asInt();
  Serial.print("target");
  Serial.println(target);
}

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

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

  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");
    Initial_Value();
    preTime = micros();
    started = true;
  }

  if (!started) {
    return;
  }

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

  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;
// 出力を-1.0~1.0の範囲に収める
  if (power < -1.0) {
    power = -1.0;
  }
  if (1.0 < power) {
    power = 1.0;
  }
// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
  if (1 < abs(I * Ki)) {
    I = 0;
    // anti windup
  }

  int Vdata_R = ((float)(VMAX - VMIN) * fabs(power)) + VMIN;
  int Vdata_L = ((float)(VMAX - VMIN) * fabs(power)) + VMIN;
  
  if(power > 0){
    writeMotorResister(byte(Vdata_R), byte(Vdata_L), 0x01);
  }else if(power < 0){
    writeMotorResister(byte(Vdata_R), byte(Vdata_L), 0x02);
  }
  
  drawScreen();

  delay(10);
}






Tweet










スポンサードリンク