MPU6050 6軸モーションセンサからMadgwickFilter・PID制御調整とスロットル操作により一瞬の浮上が出来ました。次回VL53レーザー測距センサによる浮上制御。
動画
回路図
ESP8266配線
落下衝撃で端子が変形した際、端子にハンダしたチップ抵抗10ｋΩが割れたので金属皮膜抵抗に３個入れ替えました。
同ブログ関連記事：【ドローン製作③ ESP8266コントローラ 組立】
ESP8266+MPU6050
落下の衝撃により端子が変形しています。
ドローン用LIPO バッテリー3.7V(250mA)
満充電（4.2V）を直接ESP8266に接続してテストを行っています。
ESP8266規格を超えて使用
・電源電圧min.：2.7V
・電源電圧max.：3.6V
プログラム書込み
パソコンUSBポートのFT232RL USBシリアル変換とESP8266のTXD・RXD・VCC・GND接続して、プログラム書込みを行います。
※電源コネクタのケーブルを端子に直接ハンダ・プログラム書込みをシートピンに変更
●FT232RL
ショートピン➨3.3V側
●ESP8266
IO0とGNDにショートピン挿入（書込み）
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 x 0.01
S３：Kd
S４：スロットル
(YAW：未使用）
おおよそKp:40 Ki:0.1 Kd:10
プログラム
//////////////////////////////////////////////
// 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; //1-100
Ki = S2 * 0.01; //0.01-1
Kd = S3 * 1; //1-100
if (RemoteXY.button_1 == 1) { //"CALボタン"
R = ROLL;
P = PITCH;
Y = 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));
analogWrite(M2, (S4 * 0.9 + ROLL_M - PITCH_M + YAW));
analogWrite(M3, (S4 * 0.9 - ROLL_M - PITCH_M - YAW));
analogWrite(M4, (S4 * 0.9 - ROLL_M + PITCH_M + YAW));
delay(5);
}
0 件のコメント:
コメントを投稿