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

こんにちはRoboTakaoです。

前回、最近twitterでもM5StickCで倒立振子が流行ってるようなので作ってみた
と書きました。

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



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

スケッチの構成
参考にしているのはこちらとこちらのサイトです。

1)モータードライブDRV8830(I2C接続）制御部分
2)M5StickC搭載の６軸センサのMPU6886を使用する部分
3)PID制御部分
4)BlynkでKp、Ki、KdなどのパラメータをiPhoneから設定できるようにする部分


DRV8830(I2C接続）制御部分
基本的には以前のM5StickCでタンクを作った時のスケッチがベースになります。
DRV8830がFaultで停止してもすぐにクリアするようにしています。
http://blog.robotakao.jp/blog-entry-366.html

MPU6882部分

以下の部分でジャイロと加速度センサーのスケールを設定しています。

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

Loop内の以下でRoll角を呼び出しています。

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

PID制御部分

PID制御そのものはこちらのサイトがとても分かり易かったので参考にしました。
PID制御について宇宙一わかりやすく解説してみる

Blynkで最大速度VMAXとKp,Ki,Kdそしてターゲットの角度を設定できます。

ボタンAを押すとBlynkでM5StickC内の各パラメータをiPhoneに送って、初期値とします。

ちなみにボタンBでリセットします。

動作確認

まだまだ立っているという感じではないですね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";

int VMAX = 16;
//int VMIN = 1;

float Kp = 4.0;
float Ki = 0.11;
float Kd = 0.51;
float target = -82.7;
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 = 0x60;

//モータドライバ I2C制御 motor driver I2C
void writeMotorResister(byte vset, byte data1){
int vdata = vset << 2 | data1;
Wire.beginTransmission(motor);
Wire.write(0x00);
Wire.write(vdata);
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() {
uint8_t result = 0x00;

Wire.beginTransmission(motor);
Wire.write(0x01); // read register 0x01
Wire.endTransmission();

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

return result;
}

void resetMotorStatus() { //DRV8830リセット
Wire.beginTransmission(motor);
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);

drawScreen();
}

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

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

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

//int Vdata = ((float)(VMAX - VMIN) * fabs(power)) + VMIN;
int Vdata = (float)VMAX * fabs(power);

if(power > 0){
writeMotorResister(byte(Vdata), 0x01);
}else if(power < 0){
writeMotorResister(byte(Vdata), 0x02);
}

drawScreen();

delay(10);
}