以前作っていた、バランスロボットをついに立たせることが出来ました。




S__43638798

S__43638800

S__43638801


ハードは、下記のサイトを参考にしました。
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)) {    // 30度以下だけモーター駆動
    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);
}