ドローン製作⑦ ESP8266 MPU6050 MadgwickFilter フライト

総重量　26.61ｇ

ESP8266のシールド板にMPU6050を両面テープで貼付けて配線しました。

MadgwickFilterによる姿勢制御（PID制御無し）を行いましたが、RemoteXY操作からスロットルを上げ過ぎると反転、少し浮く状態では水平方向に移動します。次回、MadgwickFilterによるPID制御をテストします。　

同ブログ関連記事：【ドローン製作⑥ ESP8266 MPU6050 MadgwickFilter】

動画

基板配線

ESP8266コントローラー

ESP8266のシールド板にMPU6050を両面テープで貼付

同ブログ関連記事：【ドローン製作③  ESP8266コントローラ 組立】

MPU6050基板搭載

MPU-6050 使用 ３軸ジャイロスコープ・３軸加速度センサー モジュール

アマゾン【https://www.amazon.co.jp/gp/product/B008BOPN40/ref=ppx_yo_dt_b_asin_image_o06_s00?ie=UTF8&psc=1】

モータ位置

ドローン用LIPO バッテリー3.7V(250mA)満充電（4.2V）を直接ESP8266接続して使用しました。

ESP8266規格を超える使用

・電源電圧min.：2.7V

・電源電圧max.：3.6V

回路図

RemoteXYアプリ

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

【Properties】【Configuration】設定変更

［Connection］WiFi access point

［Board］ESP8266 based board

［Module］WiFi on chip

【Apply】実行

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

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

【Get source code】実行

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

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

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

S１：ROLL

S２：PITCH

S３：YAW

S４：スロットル

プログラム書込み

パソコンUSBポートのFT232RL USBシリアル変換とESP8266のTXD RXD VCC GND接続して、プログラム書込みを行います。

●FT232RL

ショートピン➨3.3V側

●ESP8266

IO0➨GNDにショート（書込み）

Arduino IDE

プログラム

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

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

}