M5StickCで倒立振子２モータ化（I2CモータードライブDRV8830）その６ 重心を下げてみた
こんにちはRoboTakaoです。
引き続きM5StickCで作る倒立振子2モータ版です。
前回までの記事はこちらになります。
その１、
その２、
その３、
その４、
その５今回は重心位置を下げるために鉄のプレートを数枚買ってきたので
ホットボンドで付けてみました
これでホイール 軸の15mm上ぐらいに重心が来ました。
バッテリも前回外付けにしましたので長く動かすことができて、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 = 45;
int VMIN = 1;
float Kp = 3.5;
float Ki = 3.5;
float Kd = 0.05;
float target = -87.4;
float P, I, D, preP;
float power = 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",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);
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;
Serial.print("roll_initail:");
Serial.println(roll_initial);
digitalWrite(GPIO_NUM_10, HIGH);
preTime = micros();
started = true;
}
if (!started) {
return;
}
M5.MPU6886.getAhrsData(&pitch,&roll,&yaw);
roll = roll - (90 + roll_initial);
Serial.println(roll);
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);
}
