記事一覧

M5StickCで倒立振子2モータ化(I2CモータードライブDRV8830)その3

M5StickCで倒立振子2モータ化(I2CモータードライブDRV8830)その3



こんにちはRoboTakaoです。

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

M5tickC_Balance13.jpg


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

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

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

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

M5tickC_Balance11.jpg

M5tickC_Balance12.jpg


参考にしているのはこちらとこちらのサイトです。
//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);
}

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

Author:RoboTakao
みなさんご訪問ありがとうございます。ロボット作りたいけどお小遣いそんなにないし、簡単でローコストでロボットを作るための私のプロジェクトを紹介します。

ウェブサイトもありますのでそちらもよろしくお願いします。
http://robotakao.jp/