マイクロドローンにMPU6050を搭載して、Madgwickフィルタ・PID制御による浮上テストを行いました。浮上が低いと反射風により不安定です。浮上を上げすぎると固定ワイヤーが引っ張られ正確なテストができません。
同ブログ関連記事：【ドローン製作⑧ MadgwickFilter PID シリアルプロッタ】
動画
回路図
RemoteXYアプリ
●同ブログ関連記事：【RemoteXY使い方】
Properties】【Configuration】設定変更
［Connection］WiFi access point
［Board］ESP8266 based board
［Module］WiFi on chip
【Apply】実行
【Slider】をスマホ画面に貼付け、大きさ・色等を変更
（コントロール配置は５個以内）
【Get source code】実行
コードをArduino IDE エディタ画面に貼付てスケッチを行う。
.png) |
| ”S1”～”S4”表示されません |
CAL：キャリブレーション（ROLL・PITCH値を０にする）
S１：Kp
S２：Ki
S３：Kd
S４：スロットル
(YAW：未使用）
シリアルモニター
.png)
シリアルプロッタ
モーター電源OFF
.png) |
| 青-PITCH 赤-PITCH_P 緑-PITCH_I 橙-PITCH_D 紫-PITCH_M |
モーター回転
.png) |
| 青-PITCH 赤-PITCH_P 緑-PITCH_I 橙-PITCH_D 紫-PITCH_M |
プログラム
//////////////////////////////////////////////
// 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;
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;
#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;
}
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();
YAW = YAW-180;
YAW = 0*YAW;
// 角速度値を分解能で割って角速度に変換する
float dpsX = gx / 131.0;
float dpsY = gy / 131.0;
float dpsZ = gz / 131.0;
//定数
Kp = S1; //1-100
Ki = S2 * 0.01; //0.01-1
Kd = S3 * 0.1; //0.1-10
if (RemoteXY.button_1 == 1){ //"CALボタン"
R = ROLL;
P = PITCH;
R = YAW;
ROLL_M = 0;
PITCH_M = 0;
}
ROLL = ROLL - R; //キャリブレーション
PITCH = PITCH - P;
YAW = YAW - Y;
// 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成分：角速度-250～250dps → -1～1
PITCH_M = PITCH_P * Kp +
PITCH_I * Ki +
PITCH_D * Kd;
// Serial.print("Kp:");Serial.print(Kp); Serial.print("｜ ");
// Serial.print("Ki:");Serial.print(Ki); Serial.print("｜ ");
// Serial.print("Kd:");Serial.print(Kd); Serial.print("｜ ");
// Serial.print("ROLL:");Serial.print(ROLL); Serial.print("｜ ");
// Serial.print("PITCH:");Serial.print(PITCH); Serial.print("｜ ");
// Serial.print("YAW:");Serial.print(YAW); Serial.print("｜ ");
// Serial.print("ROLL_P:");Serial.print(ROLL_P); Serial.print("｜ ");
// Serial.print("ROLL_I:");Serial.print(ROLL_I); Serial.print("｜ ");
// Serial.print("ROLL_D:");Serial.print(ROLL_D); Serial.print("｜ ");
// Serial.print("ROLL_M:");Serial.print(ROLL_M); Serial.print("｜ ");
// Serial.print("PITCH_P:");Serial.print(PITCH_P); Serial.print("｜ ");
// Serial.print("PITCH_I:");Serial.print(PITCH_I); Serial.print("｜ ");
// Serial.print("PITCH_D:");Serial.print(PITCH_D); Serial.print("｜ ");
// Serial.print("PITCH_M:");Serial.print(PITCH_M); Serial.print("｜ ");
// Serial.print("\n");
//シリアルプロッタ
//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);
analogWrite(M1, (S4*0.9+ROLL_M+PITCH_M-YAW)*1.0);
analogWrite(M2, (S4*0.9+ROLL_M-PITCH_M+YAW)*1.0);
analogWrite(M3, (S4*0.9-ROLL_M-PITCH_M-YAW)*1.0);
analogWrite(M4, (S4*0.9-ROLL_M+PITCH_M+YAW)*1.0);
delay(5);
}
0 件のコメント:
コメントを投稿