2024年9月1日日曜日

ドローン製作⑪ ESP8266 MPU6050 VL53 MadgwickFilter PID


MPU6050 6軸モーションセンサからMadgwickFilter・PID制御に加えてVL53レーザー測距センサモジュールを取付けテストを行いましたが、VL53の処理に45-12=33㎳かかりました。
回路図

















プログラムの実行とスキップ（コメントアウト）
RemoteXY　→　101_ //RemoteXY_Handler ();
VL53　　　  →　102_ //dist = gVL530X.readRangeContinuousMillimeters();

シリアルモニター①
RemoteXY（実行）
VL53（実行）
　
VL53の処理に時間がかかる
45-12=33㎳












②
RemoteXY（スキップ）
VL53（実行）
VL53の処理に時間がかかる
508-469=39㎳













③
RemoteXY（実行）
VL53（スキップ）
MadgwickFilter PID処理に影響少ない














④
RemoteXY（スキップ）
VL53（スキップ）
MadgwickFilter PID処理に影響少ない


















シリアルモニター確認用プログラム
  1. //////////////////////////////////////////////
  2. // RemoteXY include library //
  3. //////////////////////////////////////////////
  5. // you can enable debug logging to Serial at 115200
  6. //#define REMOTEXY__DEBUGLOG
  8. // RemoteXY select connection mode and include library
  9. #define REMOTEXY_MODE__ESP8266WIFI_LIB_POINT
  11. #include <ESP8266WiFi.h>
  13. // RemoteXY connection settings
  14. #define REMOTEXY_WIFI_SSID "RemoteXY"
  15. #define REMOTEXY_WIFI_PASSWORD "12345678"
  16. #define REMOTEXY_SERVER_PORT 6377
  19. #include <RemoteXY.h>
  21. // RemoteXY GUI configuration
  22. #pragma pack(push, 1)
  23. uint8_t RemoteXY_CONF[] = // 71 bytes
  24.   { 255,6,0,0,0,64,0,17,0,0,0,139,1,106,200,1,1,5,0,5,
  25.   4,7,94,94,0,204,191,31,1,10,159,36,35,1,2,31,67,0,10,59,
  26.   159,38,35,49,4,26,31,85,80,0,31,79,70,70,0,4,3,109,100,16,
  27.   128,205,26,4,3,129,100,16,128,205,26 };
  28.   
  29. // this structure defines all the variables and events of your control interface
  30. struct {
  32.     // input variables
  33.   int8_t joystick_01_x; // from -100 to 100
  34.   int8_t joystick_01_y; // from -100 to 100
  35.   uint8_t button_01; // =1 if button pressed, else =0
  36.   uint8_t pushSwitch_01; // =1 if state is ON, else =0
  37.   int8_t slider_01; // from 0 to 100
  38.   int8_t slider_02; // from 0 to 100
  40.     // other variable
  41.   uint8_t connect_flag; // =1 if wire connected, else =0
  43. } RemoteXY;
  44. #pragma pack(pop)
  45.  
  46. /////////////////////////////////////////////
  47. // END RemoteXY include //
  48. /////////////////////////////////////////////
  52. #include "MPU6050.h"
  53. MPU6050 accelgyro;
  54. #include <MadgwickAHRS.h>
  55. Madgwick MadgwickFilter;
  56. #include <VL53L0X.h>
  57. VL53L0X gVL530X;
  59. int16_t ax, ay, az;//加速度　int16_tは2バイトの符号付き整数
  60. int16_t gx, gy, gz;//角速度　同上
  61. float ROLL, PITCH, YAW;
  62. float ROLL_P, ROLL_I, ROLL_D, ROLL_M;
  63. float PITCH_P, PITCH_I, PITCH_D, PITCH_M;
  64. float R, P, Y;
  65. float Kp, Kd, Ki;
  66. int dist;
  67. int throttle;
  69. #define M1 12
  70. #define M2 13
  71. #define M3 14
  72. #define M4 15
  74. #define PWN_FREQ 1000 // PWM frequency: 1000Hz(1kHz)
  75. #define PWN_RANGE 100 // PWN range: 100
  77. void setup()
  78. {
  79.   RemoteXY_Init ();
  80.   pinMode(M1, OUTPUT);
  81.   pinMode(M2, OUTPUT);
  82.   pinMode(M3, OUTPUT);
  83.   pinMode(M4, OUTPUT);
  84.   Wire.begin();
  85.   Serial.begin(115200);
  86.   accelgyro.initialize();//I2Cデバイスの初期化
  87.   delay(300);
  88.   MadgwickFilter.begin(100);//フィルタのサンプリングを100Hzで
  89.   analogWriteFreq(PWN_FREQ);
  90.   analogWriteRange(PWN_RANGE);
  91.   R = 0;
  92.   P = 0;
  93.   Y = 0;
  94.   gVL530X.init();
  95.   gVL530X.setTimeout(500);
  96.   gVL530X.startContinuous();
  97.   }
  99. void loop()
  100. {
  101.   //RemoteXY_Handler ();
  102.   //dist = gVL530X.readRangeContinuousMillimeters();
  103.   accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  104.   MadgwickFilter.updateIMU(gx / 131.0, gy / 131.0, gz / 131.0, ax / 16384.0, ay / 16384.0, az / 16384.0);
  105.   ROLL = MadgwickFilter.getRoll();
  106.   PITCH = MadgwickFilter.getPitch();
  107.   YAW = MadgwickFilter.getYaw();
  108.   YAW = YAW - 180;
  109.   YAW = 0 * YAW;
  111.   // 角速度値を分解能で割って角速度に変換する
  112.   float dpsX = gx / 131.0;
  113.   float dpsY = gy / 131.0;
  114.   float dpsZ = gz / 131.0;
  116.   //定数
  117.   Kp = 50;
  118.   Ki = 0.1;
  119.   Kd = 10;
  122.   // PID制御
  123.   ROLL_P = ROLL / 90; // P成分：傾き-90〜90度 → -1〜1
  124.   ROLL_I += ROLL_P; // I成分：傾きの積算。
  125.   if (10 < abs(ROLL_I)) ROLL_I = 0; //上限設定
  126.   ROLL_D = dpsX / 250; // D成分：角速度-250〜250dps → -1〜1
  127.   ROLL_M = ROLL_P * Kp +
  128.            ROLL_I * Ki +
  129.            ROLL_D * Kd;
  131.   PITCH_P = PITCH / 90; // P成分：傾き-90〜90度 → -1〜1
  132.   PITCH_I += PITCH_P; // I成分：傾きの積算。
  133.   if (10 < abs(PITCH_I)) PITCH_I = 0; //上限設定
  134.   PITCH_D = dpsY / 250; // D成分：角速度-250〜250dps → -1〜1
  135.   PITCH_M = PITCH_P * Kp +
  136.             PITCH_I * Ki +
  137.             PITCH_D * Kd;
  139.   // Serial.print("Kp:");Serial.print(Kp); Serial.print("｜ ");
  140.   // Serial.print("Ki:");Serial.print(Ki); Serial.print("｜ ");
  141.   // Serial.print("Kd:");Serial.print(Kd); Serial.print("｜ ");
  142.   Serial.print("ROLL:");Serial.print(ROLL); Serial.print("｜ ");
  143.   Serial.print("PITCH:");Serial.print(PITCH); Serial.print("｜ ");
  144.   // Serial.print("YAW:");Serial.print(YAW); Serial.print("｜ ");
  145.   // Serial.print("ROLL_P:");Serial.print(ROLL_P); Serial.print("｜ ");
  146.   // Serial.print("ROLL_I:");Serial.print(ROLL_I); Serial.print("｜ ");
  147.   // Serial.print("ROLL_D:");Serial.print(ROLL_D); Serial.print("｜ ");
  148.   // Serial.print("ROLL_M:");Serial.print(ROLL_M); Serial.print("｜ ");
  149.   // Serial.print("PITCH_P:");Serial.print(PITCH_P); Serial.print("｜ ");
  150.   // Serial.print("PITCH_I:");Serial.print(PITCH_I); Serial.print("｜ ");
  151.   // Serial.print("PITCH_D:");Serial.print(PITCH_D); Serial.print("｜ ");
  152.   // Serial.print("PITCH_M:");Serial.print(PITCH_M); Serial.print("｜ ");
  153.   Serial.print("dist:");Serial.print(dist); Serial.print("｜ ");
  154.   Serial.print("\n");
  156.   //シリアルプロッタ
  157.   //Serial.print(ROLL); Serial.print(",");
  158.   //Serial.print(ROLL_P); Serial.print(",");
  159.   //Serial.print(ROLL_I); Serial.print(",");
  160.   //Serial.print(ROLL_D); Serial.print(",");
  161.   //Serial.println(ROLL_M);
  162.   //Serial.print(PITCH); Serial.print(",");
  163.   //Serial.print(PITCH_P); Serial.print(",");
  164.   //Serial.print(PITCH_I); Serial.print(",");
  165.   //Serial.print(PITCH_D); Serial.print(",");
  166.   //Serial.println(PITCH_M);
  167.  
  168.  delay(5);
  169. }









時刻:
ラベル: , ,

0 件のコメント:

コメントを投稿

登録: コメントの投稿 (Atom)

Raspberry Pi Donkey Car スマートカー

  2020年に製作したDonkey Car スマートカー について記事にしました。 Donkey CarはRaspberry Pi のカメラからコースを ディープラーニングさせ自動走行を行います。（動画は白線上を自動走行）   動画

  • アンテナローテーター製作（方向検知・方向制御）
    Arduino制御 LM393 コンパレータ制御 同ブログ： 【 アンテナローテーター製作（方向検知・方向制御）2 】 動画 ※野鳥の鳴き声がします。 アルパイン製中空軸ボリューム値をArduinoで読取り、アンテナ方向を制御するローテーター・コントローラを製作しました。 ●同ブ...
  • uSDXトランシーバー製作（受信）
    Arduinoを使ってuSDXトランシーバー製作、こちらのブログを参考にしました。【 http://ja2gqp.blogspot.com/2021/ 】 【 https://hf5l.pl/en/transceiver-usdx-with-arduino-nano/ 】 JA2...
  • uSDXトランシーバー製作（送信）
    Arduinoを使ってuSDXトランシーバー製作、こちらのブログを参考にしました。【 http://ja2gqp.blogspot.com/2021/ 】 【 https://hf5l.pl/en/transceiver-usdx-with-arduino-nano/ 】 送信テ...