以前作っていた、バランスロボットをついに立たせることが出来ました。
ハードは、下記のサイトを参考にしました。
M5StickC バランスロボ
ソフトは、下記のサイトを参考にしました。
実質500円&100Stepで作る超簡単「 ゆるメカトロ的 M5StickC 倒立振子」
M5StickCはMadgwickフィルタで倒立振子の夢を見るか？
■スケッチ
#include <M5StickC.h> //PIDパラメータ調整 #define OFFSET 90.0 // 重心 後方-←90°→+前方 #define COEF_P 35.0 // Propotional #define COEF_I 1.5 // Integral #define COEF_D 50.0 // Differential //Motor #define LED_PIN 10 // 内蔵LED #define MOTOR_R 0x65 //A0=low, A1=open #define MOTOR_L 0x63 //A0=high, A1=open //MPU6886 https://github.com/m5stack/M5StickC/blob/master/src/utility/MPU6886.h #define MPU6886_AFS M5.MPU6886.AFS_2G // Ascale [g] (±2,4,8,16) #define MPU6886_GFS M5.MPU6886.GFS_1000DPS // Gscale [deg/s] (±250,500,1000,200) #define DPS 1000 // Gscale [deg/s] #define CALIBRATION_COUNT 1000 //Function #define min(a,b) ((a) < (b) ? (a) : (b)) #define max(a,b) ((a) > (b) ? (a) : (b)) //キャリブレ結果 double offsetX; //Gyro X float base_angle; //Accl X //ローテーション結果 float dpsX; //Gyro X float angleX; //Accl X //積分 double gyro_angle_x = 0; float preInterval = 0; // I2Cドライバー void drvMotor(byte adr, int16_t pwr) { //pwr -255 to 255 byte dir, st; if (pwr < 0) dir = 2; else dir =1; byte ipower=(byte) (abs(pwr)/4); // if (ipower == 0) dir=3; //brake //constrain(x, a, b) xがa以上b以下のときはxがそのまま返ります。xがaより小さいときはa、bより大きいときはbが返ります。 ipower = constrain (ipower, 0, 63); st = drv8830read(adr); if (st & 1) drv8830write(adr, 0, 0); drv8830write(adr, ipower, dir); } void drv8830write(byte adr, byte pwm, byte ctrl) { Wire.beginTransmission(adr); Wire.write(0); Wire.write(pwm*4+ctrl); Wire.endTransmission(true); } int drv8830read(byte adr) { Wire.beginTransmission(adr); Wire.write(1); Wire.endTransmission(false); Wire.requestFrom((int)adr, (int)1, (int)1); return Wire.read(); } void setup() { //初期化 M5.begin(); M5.Lcd.setRotation(1); M5.Lcd.fillScreen(BLACK); M5.Lcd.setTextSize(2); M5.Lcd.setCursor(0, 0); M5.Lcd.println("<B-Robot v1>"); //Motor設定 pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, HIGH); // I2C設定 Wire.begin(0, 26, 400000); //SDA, SCL // Wire.setClock（50000）; drvMotor(MOTOR_R,0); drvMotor(MOTOR_L,0); //MPU設定 M5.MPU6886.Init(); M5.MPU6886.SetGyroFsr(MPU6886_GFS); M5.MPU6886.SetAccelFsr(MPU6886_AFS); delay(1000); //キャリブレーション calibration(); // base_angle-=1.5; // offsetX+=0.6; M5.Lcd.setRotation(2); M5.Lcd.fillScreen(BLACK); M5.Lcd.setCursor(0, 140); M5.Lcd.printf("BT%3.1fV\n", M5.Axp.GetBatVoltage()); face(); } void loop() { static int Duty; float power, powerP, powerD; static float powerI = 0; double angle; //MPUローテーション calcRotation(); //モーター制御 angle = angleX; M5.Lcd.setCursor(0, 0); if ((base_angle>=0)&&(base_angle<=1.0)) { M5.Lcd.setTextColor(GREEN); } else { M5.Lcd.setTextColor(RED); } M5.Lcd.printf(">%3.1f", base_angle); if ((angle - OFFSET > -20) && (angle - OFFSET < 20)) { // ３０度以下だけモーター駆動 powerP = (angle - OFFSET ) / 90; // angle=0～180 → -1～1 powerI += powerP; powerD = dpsX / DPS; // 角速度 → -1～1 power = powerP * COEF_P + powerI * COEF_I + powerD * COEF_D; power = max(-1, min(1, power)); // -1～1 //モーター駆動 Duty = (int)(255* power ); drvMotor(MOTOR_R,Duty); drvMotor(MOTOR_L,Duty); digitalWrite(LED_PIN, HIGH); } else { // 転倒したら停止 drvMotor(MOTOR_R,0); drvMotor(MOTOR_L,0); powerI = 0; digitalWrite(LED_PIN, LOW); } M5.update(); if (M5.BtnA.wasReleased()) { M5.Lcd.fillScreen(GREEN); ESP.restart(); while (true) ; } } //キャリブレーション関数 void calibration() { float raw_gyro_x, raw_gyro_y, raw_gyro_z; float raw_acc_x, raw_acc_y, raw_acc_z; float acc_x = 0, acc_y = 0, acc_z = 0; offsetX = 0; for (int i = 0; i < CALIBRATION_COUNT; i++) { M5.MPU6886.getGyroData(&raw_gyro_x, &raw_gyro_y, &raw_gyro_z); M5.MPU6886.getAccelData(&raw_acc_x, &raw_acc_y, &raw_acc_z); //ベースプログラムとM5StickCの組付け位置の違いによる座標系変換含む offsetX += -raw_gyro_x; acc_x += -raw_acc_x; acc_y += raw_acc_y; acc_z += -raw_acc_z; } //gyro offset offsetX /= CALIBRATION_COUNT; //加速度から角度を算出 acc_x /= CALIBRATION_COUNT; acc_y /= CALIBRATION_COUNT; acc_z /= CALIBRATION_COUNT; base_angle = (atan2(acc_y, acc_z + abs(acc_x)) * 360 / 2.0 / PI) + 180; } //MPUローテーション関数 void calcRotation() { float raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ; float acc_angle_x; M5.MPU6886.getGyroData(&raw_gyro_x, &raw_gyro_y, &raw_gyro_z); M5.MPU6886.getAccelData(&raw_acc_x, &raw_acc_y, &raw_acc_z); //ベースプログラムとM5StickCの組付け位置の違いによる座標系変換 raw_gyro_x *= -1; raw_acc_x *= -1; raw_acc_z *= -1; // acc_angle_x = (atan2(raw_acc_y, raw_acc_z + abs(raw_acc_x)) * 360 / 2.0 / PI) + 180; dpsX = raw_gyro_x - offsetX; //積分 float now_time = millis(); float interval = now_time - preInterval; //前回からの経過時間 preInterval = now_time; gyro_angle_x += dpsX * (interval * 0.000999999); //相補フィルター angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x) + base_angle; } void face(){ int x0=10; int y0=40; int w0=25; int h0=10; int col=TFT_ORANGE; M5.Lcd.fillRect(x0,y0,w0,h0,col); M5.Lcd.fillRect(x0+35,y0,w0,h0,col); M5.Lcd.fillCircle(x0+12,y0+25, 7,col); M5.Lcd.fillCircle(x0+12,y0+25, 5,BLACK); M5.Lcd.fillCircle(x0+48,y0+25, 7,col); M5.Lcd.fillCircle(x0+48,y0+25, 5,BLACK); M5.Lcd.fillTriangle(x0+30,y0+25, 30,90,50,90,col); M5.Lcd.fillRect(20,y0+60,40,15,col); }
