ドローン製作⑥ ESP8266 MPU6050 MadgwickFilter

マイクロドローンにMPU6050を搭載し、ESP8266開発ボードからMadgwickフィルタを使てROLL・PITCH・YAWの変化によるモーター回転テストをしました。　　　　

動画

ESP8266開発ボード MPU6050回路図

シリアルプロッタ

青-ROLL　赤-PITCH　緑-YAW

シリアルモニター

●RemoteXYアプリによるMadgwickFilter処理の影響は少ないように見える。

●[YAW]は地磁気を用いていないためドリフトが発生（時間とともに数値が変化）

MPU6050取付

　　             M3                               M4

M2                                   M1

MPU6050

モーター回転

0615モーター

■CWモーター（時計回転）

赤⊕　青⊖

M1 M3

■CCWモーター（反時計回転）

白⊕　黒⊖

M2 M4

ROLL制御

rollの値が大きくなると

(X軸矢印方向に時計回転)

M１・M２の回転を早く

M３・M４の回転を遅く

PITCH制御

pitchの値が大きくなると

(Y軸矢印方向に時計回転)

M１・M４の回転を早く

M２・M３の回転を遅く

YAW制御

yawの値が大きくなると

(Z軸矢印方向に時計回転)

M2・M４の回転を早く

M1・M３の回転を遅く

水平維持

M1モーター = throttle + roll + pitch - yaw

M2モーター = throttle + roll - pitch + yaw

M3モーター = throttle - roll - pitch - yaw

M4モーター = throttle - roll + pitch + yaw

RemoteXYアプリ

●同ブログ関連記事：【RemoteXY使い方】

【Properties】【Configuration】設定変更

［Connection］WiFi access point

［Board］ESP8266 based board

［Module］WiFi on chip

【Apply】実行

【Slider】をスマホ画面に貼付け、大きさ・色等を変更

（コントロール配置は５個以内）

【Get source code】実行

コードをArduino IDE　エディタ画面に貼付てスケッチを行う。

”S1”～”S4”表示されません

CAL：キャリブレーション（ROLL・PITCH値を０にする）

S１：ROLL

S２：PITCH

S３：YAW

S４：スロットル

 プログラム

//////////////////////////////////////////////

//        RemoteXY include library          //

//////////////////////////////////////////////

// you can enable debug logging to Serial at 115200

//#define REMOTEXY__DEBUGLOG    

// RemoteXY select connection mode and include library 

#define REMOTEXY_MODE__ESP8266WIFI_LIB_POINT

#include <ESP8266WiFi.h>

#include <RemoteXY.h>

// RemoteXY connection settings 

#define REMOTEXY_WIFI_SSID "RemoteXY"

#define REMOTEXY_WIFI_PASSWORD "12345678"

#define REMOTEXY_SERVER_PORT 6377

// RemoteXY configurate  

#pragma pack(push, 1)

uint8_t RemoteXY_CONF[] =   // 63 bytes

  { 255,5,0,0,0,56,0,17,0,0,0,195,1,126,200,1,1,5,0,4,

  2,112,122,20,128,204,26,4,2,134,122,20,128,204,26,4,2,156,122,20,

  128,204,26,4,100,252,18,114,0,134,26,1,25,34,39,38,1,234,31,67,

  65,76,0 };

  

// this structure defines all the variables and events of your control interface 

struct {

    // input variables

  int8_t slider_1; // =0..100 slider position

  int8_t slider_2; // =0..100 slider position

  int8_t slider_3; // =0..100 slider position

  int8_t slider_4; // =0..100 slider position

  uint8_t button_1; // =1 if button pressed, else =0

    // other variable

  uint8_t connect_flag;  // =1 if wire connected, else =0

} RemoteXY;

#pragma pack(pop)

 

/////////////////////////////////////////////

//           END RemoteXY include          //

/////////////////////////////////////////////

#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;//x,y,z軸の角度

float R,P,Y;//キャリブレーション変数

#define M1 12

#define M2 13

#define M3 14

#define M4 15

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

#define PWN_RANGE 100 // PWN range: 100

void setup() 

{

  RemoteXY_Init (); 

  pinMode(M1,OUTPUT);

  pinMode(M2,OUTPUT); 

  pinMode(M3,OUTPUT);

  pinMode(M4,OUTPUT);

  Wire.begin();

  Serial.begin(115200);

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

  delay(300);

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

  analogWriteFreq(PWN_FREQ);

  analogWriteRange(PWN_RANGE);

  R = 0;

  P = 0;

  Y = 0;

  //シリアルプロッタ文字表示（ESP02文字表示しない）

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

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

  Serial.println("YAW"); Serial.print(",");

  delay(100);

}

void loop() 

{

  RemoteXY_Handler ();  

  int S1 = RemoteXY.slider_1; 

  int S2 = RemoteXY.slider_2; 

  int S3 = RemoteXY.slider_3; 

  int S4 = RemoteXY.slider_4;

  

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

  ROLL = ROLL / 90; //傾き-90～90度 → -1～1

  PITCH = PITCH / 90; //傾き-90～90度 → -1～1

  ROLL = S1*0.2*ROLL;

  PITCH =S2*0.2*PITCH;

  YAW  = YAW-180; 

  YAW = YAW / 90; //傾き-90～90度 → -1～1

  YAW  = S3*0.2*YAW;

  

 if (RemoteXY.button_1 == 1){  //"CALボタン"

  R = ROLL;

  P = PITCH;

  Y = YAW;

  }

   ROLL = ROLL - R;  //キャリブレーション

   PITCH = PITCH - P;

   YAW = YAW - Y;

  //シリアルモニター

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

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

  //Serial.print("YAW");Serial.print(YAW);

  //Serial.print("\n");

  

  //シリアルプロッタ

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

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

  Serial.println(YAW); 

  

  //モーター制御

  analogWrite(M1, (S4*0.8+ROLL+PITCH-YAW)*1.0);

  analogWrite(M2, (S4*0.8+ROLL-PITCH+YAW)*1.0);

  analogWrite(M3, (S4*0.8-ROLL-PITCH-YAW)*1.0);

  analogWrite(M4, (S4*0.8-ROLL+PITCH+YAW)*1.0);

  delay(5);

}