ドローン製作⑧ ESP8266 MPU6050 MadgwickFilter PID シリアルプロッタ

 ESP8266だと文字表示がされない為？、Arduino Nanoを使いMadgwickFilterのPITCHを変化させて、PID制御Kp・Ki・Kdの定数を変えたArduino IDE シリアルプロッタを確認しました。   

●参考サイト 【https://qiita.com/sasaco/items/b72dec0edc16d19af6e4】　

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

接続図

Arduino Nano　5V     ------  VCC    MPU6050

                          GND  ------  GND

                          A4     ------  SDA

                          A5     ------  SCL

シリアルプロッタ

Kp = 50  Ki = 0.1  Kd = 1　

Kp = 20  Ki = 0.1  Kd = 1

Kp = 5  Ki = 0.1  Kd = 1　

Kp = 20  Ki = 1  Kd = 1

Kp = 20  Ki = 0.01  Kd = 1

Kp = 20  Ki = 0.1  Kd = 10

Kp = 20  Ki = 0.01  Kd = 0.1

プログラム

#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;

float ROLL_P, ROLL_I, ROLL_D, ROLL_M;

float PITCH_P, PITCH_I, PITCH_D, PITCH_M;

float R,P,Y;

float Kp,Kd,Ki;

void setup() 

{  

  Wire.begin();

  Serial.begin(115200); 

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

  delay(300);

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

  //シリアルプロッタ文字表示 

  //Serial.print("ROLL"); Serial.print(","); 

  //Serial.print("ROLL_P"); Serial.print(","); 

  //Serial.print("ROLL_I"); Serial.print(","); 

  //Serial.print("ROLL_D"); Serial.print(","); 

  //Serial.println("ROLL_M");

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

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

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

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

  Serial.println("PITCH_M");  

}

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(); 

  YAW  = YAW-180;  

  YAW  = 0*YAW;

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

  float dpsX = gx / 131.0; 

  float dpsY = gy / 131.0;

  float dpsZ = gz / 131.0; 

  //定数

  Kp = 20;   

  Ki = 0.1;  

  Kd = 1;

  

  // PID制御

  ROLL_P = ROLL / 90; // P成分：傾き-90～90度 → -1～1

  ROLL_I += ROLL_P; // I成分：傾きの積算。

   if (10 <  abs(ROLL_I)) ROLL_I = 0; //上限設定

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

  ROLL_M = ROLL_P * Kp +

         ROLL_I * Ki +

         ROLL_D * Kd;   

           

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

  PITCH_I += PITCH_P; // I成分：傾きの積算。

   if (10 < abs(PITCH_I)) PITCH_I = 0; //上限設定

  PITCH_D = dpsY / 250; // D成分：角速度   

  PITCH_M = PITCH_P * Kp +

          PITCH_I * Ki +

          PITCH_D * Kd;

 //シリアルプロッタ

 //Serial.print(ROLL); Serial.print(","); 

 //Serial.print(ROLL_P); Serial.print(","); 

 //Serial.print(ROLL_I); Serial.print(","); 

 //Serial.print(ROLL_D); Serial.print(","); 

 //Serial.println(ROLL_M); 

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

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

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

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

 Serial.println(PITCH_M);  

 delay(5); 

}