 |
| 総重量 |
MPU6050 6軸モーションセンサからMadgwickFilter・PID制御に加えてVL53レーザー測距センサモジュールによる浮上テストを行いました。距離センサ読み込みにタイムラグありドロンの浮上・降下位置がズレています。
●VL53レーザー測距センサモジュール【https://www.amazon.co.jp/gp/product/B0C36FB7HB/ref=ppx_yo_dt_b_search_asin_title?ie=UTF8&psc=1】
●同ブログ関連記事：【ドローン製作⑪ ESP8266 MPU6050 VL53 MadgwickFilter PID 】
動画（距離大）
動画（距離小）
回路図
シリアルモニタ
.png)
シリアルプロッタ
.png) |
| 青-ROLL_M 赤-PITCH_M |
RemoteXYアプリ
●同ブログ関連記事：【RemoteXY使い方】
【Properties】【Configuration】設定変更
［Connection］WiFi access point
［Board］ESP8266 based board
［Module］WiFi on chip
【Apply】実行
【Slider】をスマホ画面に貼付け、大きさ・色等を変更
（コントロール配置は５個以内）
【Get source code】実行
コードをArduino IDE エディタ画面に貼付てスケッチを行う。
.png)
スライダー01：Kp
スライダー02：Ki x 0.01
スライダー03：浮上高
CAL：キャリブレーション（ROLL・PITCH値を０にする）
ON：自動浮上
おおよそ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__WIFI_POINT
#include <ESP8266WiFi.h>
// RemoteXY connection settings
#define REMOTEXY_WIFI_SSID "RemoteXY"
#define REMOTEXY_WIFI_PASSWORD "12345678"
#define REMOTEXY_SERVER_PORT 6377
#include <RemoteXY.h>
// RemoteXY GUI configuration
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] = // 72 bytes
{ 255,5,0,0,0,65,0,17,0,0,0,180,1,106,200,1,1,5,0,4,
4,14,103,15,128,190,24,4,4,45,103,15,128,190,24,4,4,75,103,15,
128,190,24,10,61,129,41,41,49,148,26,31,79,78,0,31,79,70,70,0,
1,4,129,41,41,0,235,31,67,65,76,0 };
// this structure defines all the variables and events of your control interface
struct {
// input variables
int8_t slider_01; // from 0 to 100
int8_t slider_02; // from 0 to 100
int8_t slider_03; // from 0 to 100
uint8_t pushSwitch_01; // =1 if state is ON, else =0
uint8_t button_01; // =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;
#include <VL53L0X.h>
VL53L0X gVL530X;
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;
int dist;
int throttle;
int S1, S2, S3, cal;
#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);
gVL530X.init();
gVL530X.setTimeout(500);
gVL530X.startContinuous();
}
void loop()
{
RemoteXY_Handler ();
S1 = RemoteXY.slider_01; //kp
S2 = RemoteXY.slider_02; //ki
S3 = RemoteXY.slider_03; //浮上高
cal = RemoteXY.button_01; //キャリブレーション
int Auto = RemoteXY.pushSwitch_01; //自動浮上
if (cal == 1) { //"CALボタン"
R = ROLL;
P = PITCH;
Y = YAW;
ROLL_M = 0;
PITCH_M = 0;
}
ROLL = ROLL - R; //キャリブレーション
PITCH = PITCH - P;
YAW = YAW - Y;
//自動浮上
if(Auto == 1 ){
if(dist < S3*10 ){
throttle = throttle+10;
throttle = min(throttle,80);
}else{
throttle = throttle-10;
throttle = max(throttle,40);
}
}else{
throttle = 0;
}
dist = gVL530X.readRangeContinuousMillimeters();
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 = 10;
// 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("ROLL");Serial.print(ROLL); Serial.print("｜ ");
//Serial.print("PITCH");Serial.print(PITCH); Serial.print("｜ ");
//Serial.print("dist");Serial.print(dist); Serial.print("｜ ");
//Serial.print("\n");
//シリアルプロッタ
//Serial.print(ROLL_M);Serial.print(",");
//Serial.print(PITCH_M);
//Serial.print("\n");
analogWrite(M1, (throttle + ROLL_M + PITCH_M - YAW));
analogWrite(M2, (throttle + ROLL_M - PITCH_M + YAW));
analogWrite(M3, (throttle - ROLL_M - PITCH_M - YAW));
analogWrite(M4, (throttle - ROLL_M + PITCH_M + YAW));
delay(5);
}
0 件のコメント:
コメントを投稿