倒立振子ロボット ESP8266 Madgwickフィルタ

2019年製作の倒立振子ロボットにMPU-6050 ６軸センサをMadgwickフィルタからPID制御しました。

●同ブログ関連記事：【倒立振子ロボット ESP8266 Kalmanフィルタ】 　

動画

参考サイト

●MadgwickフィルタのライブラリをArduinoで使う

【https://qiita.com/Ninagawa123/items/9520bad3c78ee40194fc】

●セグウェイを自作してみた

 【https://qiita.com/sasaco/items/b72dec0edc16d19af6e4】

回路図

シリアルモニター

シリアルプロッタ

青-PITCH　赤-gPowerP　緑-gPowerI　橙-gPowerD　紫-power

Arduino IDE設定

プログラム

#include "MPU6050.h"

MPU6050 accelgyro;

#include <MadgwickAHRS.h>

Madgwick MadgwickFilter;

int16_t ax, ay, az;//加速度　int16_tは2バイトの符号付き整数

int16_t gx, gy, gz;//角速度　同上

float ROLL, PITCH, YAW;

// モータードライバの制御定数。

#define AIN1 12

#define AIN2 13

#define AIN3 14

#define AIN4 15

#define PWN_FREQ 1000 // PWM frequency: 1000Hz(1kHz)

#define PWN_RANGE 100 // PWN range: 100

#define V_MIN 50// モーター駆動電圧(0～255)の最低値。これより低いと回らない。

#define V_MAX 100 // モーター駆動電圧(0～255)の最高値。

// 倒立振子の制御変数。

float gPowerP, gPowerI, gPowerD; // 現在出力値とPID成分。

void setup() {

  Wire.begin();

  Serial.begin(115200);

  accelgyro.initialize();//I2Cデバイスの初期化  

  delay(300);

  MadgwickFilter.begin(100);//フィルタのサンプリングを100Hzで

  pinMode(AIN1,OUTPUT);

  pinMode(AIN2,OUTPUT); 

  pinMode(AIN3,OUTPUT);

  pinMode(AIN4,OUTPUT); 

  delay(2000);

  analogWriteFreq(PWN_FREQ);

  analogWriteRange(PWN_RANGE);

}

void loop() {

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // 各軸加速度と角速度を取得

  MadgwickFilter.updateIMU(gx / 131.0, gy / 131.0, gz / 131.0, ax / 16384.0, ay / 16384.0, az / 16384.0);

  ROLL = MadgwickFilter.getRoll(); //姿勢角の値を取得

  PITCH = MadgwickFilter.getPitch();

  YAW  = MadgwickFilter.getYaw();

 

 // 角速度値を分解能で割って角速度に変換する

  float dpsX = gx / 131.0; 

  float dpsY = gy / 131.0;

  float dpsZ = gz / 131.0; 

  // PID制御でモーター出力を計算。

  gPowerP = PITCH/ 90 ;  // P成分：傾き-90～90度 → -1～1

  gPowerI += gPowerP;     // I成分：傾きの積算。

  gPowerD = dpsY / 250;   // D成分：角速度-250～250dps → -1～1

  float power = gPowerP * 17.0 + // 17この数字は試行錯誤で調整。

                gPowerI *  1.5 +

                gPowerD *  2.0;

  power = max((float) -1, min((float)1, power)); // → -1～1

  float powerL = power;

  float powerR = power;

  // 方向転換させる場合はここでpowerL, Rに差分を与える。

  // powerをモーター駆動電圧に変換。0～1 → V_MIN～V_MAX

  int voltL = (int)((V_MAX - V_MIN) * abs(powerL) + V_MIN);

  int voltR = (int)((V_MAX - V_MIN) * abs(powerR) + V_MIN);

//シリアルモニター     

  //Serial.print("PITCH:"); Serial.print(PITCH);Serial.print("｜ ");

  //Serial.print("gPowerP:"); Serial.print(gPowerP);Serial.print("｜ ");

  //Serial.print("gPowerI:"); Serial.print(gPowerI);Serial.print("｜ ");

  //Serial.print("gPowerD:"); Serial.print(gPowerD);Serial.print("｜ ");  

  //Serial.print("power:"); Serial.print(power);Serial.print("｜ "); 

  //Serial.println(""); 

//シリアルプロッタ

  Serial.print(PITCH); Serial.print(","); 

  Serial.print(gPowerP); Serial.print(","); 

  Serial.print(gPowerI); Serial.print(","); 

  Serial.print(gPowerD); Serial.print(","); 

  Serial.println(power);     

  

 

 // モーター駆動。

  if(0 <= powerL){

    analogWrite(AIN3, voltL);

    digitalWrite(AIN4, LOW );

  }

  else{

    analogWrite(AIN4, voltL);

    digitalWrite(AIN3, LOW );

  }

 if(0 <= powerR){

    analogWrite(AIN1, voltL);

    digitalWrite(AIN2, LOW );

  }

  else{

    analogWrite(AIN2, voltL);

    digitalWrite(AIN1, LOW );

  } 

  

  if (30 < abs(PITCH)) {

    analogWrite(AIN1, 0);

    analogWrite(AIN2, 0);

    analogWrite(AIN3, 0);

    analogWrite(AIN4, 0);

    

    Serial.print("**** stop ");

    Serial.print(PITCH);

    while (1) delay(1000);

  }

  

 delay(1); 

}