JA7MLC 電子工作ブログ: ドローン ESP32 PS4コントローラー操作

PS4コントローラーとESP32制御ドローンをBluetooth接続。9軸センサBMX055を読込Madgwickフィルタ・PID姿勢制御による浮上テストを行いましたが、ホバリングに程遠い操作です。PS4コントローラーは電源オンと同時に接続され操作しやすい。　

●ボードマネージャ　esp32

　 by Espressif Systems

　バージョン1.0.6

●同ブログ【ドローン ESP32 PS4コントローラー机上テスト】

動画

回路図

PS4コントローラー(Bluetooth)

PSボタンを押して電源オン（接続）

モード１

PS4.LStickY() ↕ 前進／後進　 PS4.LStickX() ↔ 左移動／右移動

PS4.RStickY() ↕ 上昇／下降　 PS4.RStickX() ↔ 左旋回／右旋回

PS4.R1() モータ起動　　 PS4.R2() モータ停止

PS4.L1() キャリブレーション

PS4-esp32-master libraries入手

●入手先【https://github.com/aed3/PS4-esp32】

【Code】【Download ZIP】

【PS4-esp32-master】圧縮ファイルを展開

【PS4-esp32-master】フォルダを任意のArduino librariesに貼付け

MACアドレスの確認

●入手先【https://sixaxispairtool.en.lo4d.com/download】

【LO4D.com UK】ダウンロード

【SixaxisPairToolSetup-0.3.1】任意ファイルにインストール

PS4コントローラーとパソコンをUSB接続

【SixaxisPairTool】実行する。

Current Master

【XX:XX:XX:XX:XX:XX】MACアドレス

　　　↓

ESP32 プログラム

PS4.begin("XX:XX:XX:XX:XX:XX")

MACアドレスを入力

ESP32を2台登録すると1台目が接続できなくなる→解除方法

ESP32

【スケッチ例】

【BluetoothSerial】

【bt_remove_paired_devices】

bt_remove_paired_devices

【0】→【1】に変更

接続できないESP32に同プログラム書込→

プログラムを戻すと接続出来ました。

(ESP32の登録デバイスを解除）

ESP32-WROOM-32E

●Wi-Fiモジュール ESP32-WROOM-32E 16MB

Bluetooth V4.2

【https://akizukidenshi.com/catalog/g/g115675/】

ピッチ変換基板

●ESP-WROOM-32 ピッチ変換基板コンパクト

【https://www.amazon.co.jp/dp/B06XP3TY1B?psc=1&ref=ppx_yo2ov_dt_b_product_details】

【https://www.switch-science.com/products/4116】

基板ハンダ

丸ピンヘッダ・ソケット

●丸ピンヘッダ・ソケット2.54mmピッチ

ESP32・BMX055とドライブ基板接続

接触が気になるので、丸ピンヘッダの表面にハンダを流しました。

丸ピンヘッダハンダ

SI2302A

●MOSFETドライバー

【https://www.amazon.co.jp/gp/product/B09F9TTTD1/ref=ppx_yo_dt_b_asin_title_o09_s00?ie=UTF8&psc=1】

ADP3338

●表面実装低損失レギュレーターADP3338AKCZ-3.3

ドロップアウト電圧：190mV

【https://akizukidenshi.com/catalog/g/g112635/】

ドローンフレーム・モータ

●コアレスモーター 0716【https://www.amazon.co.jp/dp/B08NGCCCQL?psc=1&ref=ppx_yo2ov_dt_b_product_details】

●フレーム　BETAFPV　65S　65mm Micro Whoop Frame for 7x16mm Motors Version 4【https://item.rakuten.co.jp/airstage/10015759/】

●BETAFPV プロペラ HQ 31mm 3-Blade Propellers (0.8mm Shaft 4PCS) 【クリアグ

レー】（BETA 65S 2022Ver.などに）

●ドローンアクセサリー 4 ペア 31mm 0.8mm 1.0mm 穴 3 ブレードプロペラ PC 小道具 CW CCWドローンアクセサリー 4 ペア【https://www.amazon.co.jp/gp/product/B0BGM633K5/ref=ox_sc_act_title_1?smid=A28LWE3HRWLAWN&th=1】

バッテリー

●BETAFPV バッテリー PH2.0 300mAh 1S 30C Battery(2PCS)

【https://item.rakuten.co.jp/airstage/16071/】

基板

●角型ランド両面スルーホールガラスコンポジット・ユニバーサル基板 Cタイプ(72×47mm)
【https://akizukidenshi.com/catalog/g/g109747/】

フレームに合わせて切断

9軸センサBMX055

●9軸(加速度3軸+ジャイロ3軸+磁気コンパス3軸)センサ BMX055にレベル変換回路

【https://akizukidenshi.com/catalog/g/g113010/】

●I2Cアドレス

　加速度0x19 ジャイロ0x69 磁気0x13　

　JP1オープン JP2オープン JP3オープン　

●プルアップ接続

　JP4ショート JP5ショート　

●電源3.3V　信号3.3V　

　JP6オープン　JP7ショート　JP8オープン

BMX055取付

ESP32基板の裏にBMX055基板を45度傾けて両面テープ固定。

基板ハンダ

LED取付

抵抗内臓LEDをIO12・ IO27穴にハンダ

プログラム制御による点灯

IO12 LED　⊕

IO27 LED　⊖

秋月電子通商

【https://akizukidenshi.com/catalog/g/g116692/】

CH9102搭載 ESP32 ダウンローダキット

CP2104/CH9102搭載 ESP32 ダウンローダキット【https://akizukidenshi.com/catalog/g/g117240/】

ドライバーインストール

●ESP32 Downloader Kitダウンロードサイト【https://docs.m5stack.com/en/accessory/esp32_downloader_kit】

●【CH9102_VCP_SER_Windows】選択してドライバーをダウンロードする

●ドライバーをインストールする

操作

プッシュスイッチ操作無しでプログラム書込

み・実行を自動で行ってくれる。

プログラム書込み

変換コネクタ製作

ピンソケットと丸ピンヘッダをはんだし、チューブを熱収縮する（ピンヘッダの表面にハンダを流す）

モータ位置

0716モーター

■CWモーター（時計回転）

赤⊕　黒⊖

M1 M3

■CCWモーター（反時計回転）

白⊕　黒⊖

M2 M4

プログラム　Arduino IDE【ボード：ESP32 Dev Module】

//ESP32ボードマネージャ バージョン1.0.6
#include <PS4Controller.h>
#include<Wire.h>
#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;

float ROLL_P, ROLL_I, ROLL_D, ROLL_M;
float PITCH_P, PITCH_I, PITCH_D, PITCH_M;
float YAW_P, YAW_I, YAW_D, YAW_M;
float R, P, Y;
float Kp, Kd, Ki;
int CAL, Auto, throttle;
int JRX, JRY, JLX, JLY; //Joystick
#define M1 18
#define M2 19
#define M3 23
#define M4 32

// BMX055
#define Addr_Accl 0x19  // 加速度センサのI2Cアドレス(JP1,JP2,JP3 = Openの時)
#define Addr_Gyro 0x69  // ジャイロセンサのI2Cアドレス(JP1,JP2,JP3 = Openの時)
#define Addr_Mag 0x13   // 磁気センサのI2Cアドレス(JP1,JP2,JP3 = Openの時)

// センサーの値を保存するグローバル変数
float ax = 0.00;//加速度
float ay = 0.00;
float az = 0.00;
float gx = 0.00;//角速度
float gy = 0.00;
float gz = 0.00;
int mx = 0;
int my = 0;
int mz = 0;
float ROLL = 0.00;
float PITCH = 0.00;
float YAW = 0.00;
int m1, m2, m3, m4;
void BMX055_Init() //BMX055の初期化
{
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x0F); // Select PMU_Range register
  Wire.write(0x03);   // Range = +/- 2g
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x10);  // Select PMU_BW register
  Wire.write(0x08);  // Bandwidth = 7.81 Hz
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x11);  // Select PMU_LPW register
  Wire.write(0x00);  // Normal mode, Sleep duration = 0.5ms
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x0F);  // Select Range register
  Wire.write(0x04);  // Full scale = +/- 125 degree/s
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x10);  // Select Bandwidth register
  Wire.write(0x07);  // ODR = 100 Hz
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x11);  // Select LPM1 register
  Wire.write(0x00);  // Normal mode, Sleep duration = 2ms
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x83);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x01);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4C);  // Select Mag register
  Wire.write(0x00);  // Normal Mode, ODR = 10 Hz 0x00 //100Hz 0x07
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x4E);  // Select Mag register
  Wire.write(0x84);  // X, Y, Z-is enabled
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x51);  // Select Mag register
  Wire.write(0x04);  // No. of Repetitions for X-Y is = 9
  Wire.endTransmission();
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Mag);
  Wire.write(0x52);  // Select Mag register
  Wire.write(0x16);  // No. of Repetitions for Z-Axis = 15
  Wire.endTransmission();
}

void BMX055_All() //BMX055から全データとMadgwickフィルタの結果を取得する
{
  unsigned int data[8];

  //加速度データを取得する
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Accl);
    Wire.write((2 + i));// Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Accl, 1);// Request 1 byte of data
    // Read 6 bytes of data
    // ax lsb, ax msb, ay lsb, ay msb, az lsb, az msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data to 12-bits
  ax = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
  if (ax > 2047)  ax -= 4096;
  ay = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
  if (ay > 2047)  ay -= 4096;
  az = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
  if (az > 2047)  az -= 4096;
  ax = ax * 0.0098; // range = +/-2g
  ay = ay * 0.0098; // range = +/-2g
  az = az * 0.0098; // range = +/-2g
  // 上記の乗数は秋月のマニュアル通りですが、誤値とのことです。
  // 正しくは、mg換算で0.98、G換算で0.00098
  // とのことです。

  //ジャイロデータを取得する
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Gyro);
    Wire.write((2 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Gyro, 1);    // Request 1 byte of data
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  gx = (data[1] * 256) + data[0];
  if (gx > 32767)  gx -= 65536;
  gy = (data[3] * 256) + data[2];
  if (gy > 32767)  gy -= 65536;
  gz = (data[5] * 256) + data[4];
  if (gz > 32767)  gz -= 65536;

  gx = gx * 0.0038; //  Full scale = +/- 125 degree/s
  gy = gy * 0.0038; //  Full scale = +/- 125 degree/s
  gz = gz * 0.0038; //  Full scale = +/- 125 degree/s

  //コンパスデータを取得する
  for (int i = 0; i < 8; i++)
  {
    Wire.beginTransmission(Addr_Mag);
    Wire.write((0x42 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Mag, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // mx lsb, mx msb, my lsb, my msb, mz lsb, mz msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  mx = ((data[1] << 5) | (data[0] >> 3));
  if (mx > 4095)  mx -= 8192;
  my = ((data[3] << 5) | (data[2] >> 3));
  if (my > 4095)  my -= 8192;
  mz = ((data[5] << 7) | (data[4] >> 1));
  if (mz > 16383)  mz -= 32768;
}


void setup()
{
  pinMode(12, OUTPUT);   //LED +
  digitalWrite(12, HIGH);
  pinMode(27, OUTPUT);  
  digitalWrite(27, LOW); //LED -
  PS4.begin("XX:XX:XX:XX:XX:XX");
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
  pinMode(M4, OUTPUT);
  ledcSetup(1, 1000, 8);   // モータ 0CH 490Hz 8bit=0-255(PWM)
  ledcSetup(2, 1000, 8);
  ledcSetup(3, 1000, 8);
  ledcSetup(4, 1000, 8);
  ledcAttachPin(M1, 1); // モータ CH
  ledcAttachPin(M2, 2);
  ledcAttachPin(M3, 3); 
  ledcAttachPin(M4, 4);  
  Wire.begin();
  Serial.begin(115200);
  BMX055_Init(); //BMX055の初期化
  delay(300);
  MadgwickFilter.begin(100);//フィルタのサンプリングを100Hzで
  R = 0;
  P = 0;
  Y = 0;
  Kp = 0;
  Ki = 0;
  Kd = 0;

}

void loop()
{
 if (PS4.isConnected()) {   
  if (PS4.R1()){  //モータ起動
   Auto = 1;
  }
  if (PS4.R2()){  //モータ停止
   Auto = 0;
  }  
  if (PS4.L1()){ //キャリブレーション
   CAL = 1;
  }
  if (PS4.L2()){
    
  }
  
  if (PS4.LStickX()) {
     int LX = PS4.LStickX(); //左移動／右移動
     JLX =  map(LX, -128, 128, -255, 255);     
    }
  if (PS4.LStickY()) {
     int LY = PS4.LStickY(); //前進／後進
     JLY =  map(LY, -128, 128, -255, 255);     
    }
  if (PS4.RStickX()) {
     int RX = PS4.RStickX(); //左旋回／右旋回
     JRX =  map(RX, -128, 128, -255, 255);     
    }
  if (PS4.RStickY()) {
     int RY = PS4.RStickY(); //上昇／下降
     JRY =  map(RY, -127, 128, -255, 255);    
    }
     
}  
  BMX055_All();//BMX055から全データ
  MadgwickFilter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW  = MadgwickFilter.getYaw();
  YAW  = YAW - 90;

if (CAL == 1) { //"CALボタン"
    R = ROLL;
    P = PITCH;
    Y = YAW;
    ROLL_M = 0;
    PITCH_M = 0;
    YAW_M = 0;
    CAL = 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 = gx / 125; // D成分：角速度-125～125dps → -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 = gy / 125; // D成分：角速度-125～125dps → -1～1
  PITCH_M = PITCH_P * Kp +
            PITCH_I * Ki +
            PITCH_D * Kd;

  YAW_P = YAW / 90; // P成分：傾き-90～90度 → -1～1
  YAW_I += YAW_P; // I成分：傾きの積算。
  if (10 < abs(YAW_I)) YAW_I = 0; //上限設定
  YAW_D = gz / 125; // D成分：角速度-125～125dps → -1～1
  YAW_M = YAW_P * Kp +
            YAW_I * Ki +
            YAW_D * Kd;
            
  YAW_M = YAW_M + JRX * 0.05;
  ROLL_M = ROLL_M + JLY * 0.05;
  PITCH_M = PITCH_M - JLX * 0.05;

 //自動浮上
 if(Auto == 1 ){ 
   //定数
  Kp = 40;
  Ki = 0.1;
  Kd = 10;
  throttle = (JRY + 255)/2 * 0.7;
  }else{   
  throttle = 0;
  ROLL_M = 0;
  PITCH_M = 0;
  YAW_M = 0;  
}  
   
    
    // シリアルモニタ
    //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("gx:");Serial.print(gx); Serial.print("｜ ");
    //Serial.print("gy:");Serial.print(gy); Serial.print("｜ ");
    //Serial.print("gz:");Serial.print(gz); 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("YAW_P:");Serial.print(YAW_P); Serial.print("｜ ");
    //Serial.print("YAW_I:");Serial.print(YAW_I); Serial.print("｜ ");
    //Serial.print("YAW_D:");Serial.print(YAW_D); Serial.print("｜ ");
    //Serial.print("YAW_M:");Serial.print(YAW_M); Serial.print("｜ ");
    //Serial.print("\n");

    //シリアルプロッタ
    //Serial.print(gx); Serial.print(",");
    //Serial.print(gy); Serial.print(",");
    //Serial.print(gz); Serial.print(",");
    Serial.print(ROLL); Serial.print(",");
    Serial.print(ROLL_P); Serial.print(",");
    Serial.print(ROLL_I); Serial.print(",");
    Serial.print(ROLL_D); Serial.print(",");
    Serial.print(ROLL_M); Serial.print(",");
    //Serial.print(PITCH); Serial.print(",");
    //Serial.print(PITCH_P); Serial.print(",");
    //Serial.print(PITCH_I); Serial.print(",");
    //Serial.print(PITCH_D); Serial.print(",");
    //Serial.print(PITCH_M); Serial.print(",");
    //Serial.print(YAW); Serial.print(",");
    //Serial.print(YAW_P); Serial.print(",");
    //Serial.print(YAW_I); Serial.print(",");
    //Serial.print(YAW_D); Serial.print(",");
    //Serial.print(YAW_M); Serial.print(",");
    Serial.print("\n");    
    
    //モータ暴走
    //ledcWrite(1,(throttle + ROLL_M + PITCH_M - YAW_M)); //M1
    //ledcWrite(2,(throttle + ROLL_M - PITCH_M + YAW_M)); //M2
    //ledcWrite(3,(throttle - ROLL_M - PITCH_M - YAW_M)); //M3
    //ledcWrite(4,(throttle - ROLL_M + PITCH_M + YAW_M)); //M4  

    //pwm 0-255の範囲
    int M1pwm = constrain((throttle + ROLL_M + PITCH_M - YAW_M),0,255);
    int M2pwm = constrain((throttle + ROLL_M - PITCH_M + YAW_M),0,255);
    int M3pwm = constrain((throttle - ROLL_M - PITCH_M - YAW_M),0,255);
    int M4pwm = constrain((throttle - ROLL_M + PITCH_M + YAW_M),0,255);
    ledcWrite(1,M1pwm); //M1
    ledcWrite(2,M2pwm); //M2
    ledcWrite(3,M3pwm); //M3
    ledcWrite(4,M4pwm); //M4
    delay(5);
}

JA7MLC 電子工作ブログ

2026年1月1日木曜日

ドローン ESP32 PS4コントローラー操作

0 件のコメント:

コメントを投稿

ドローン ESP32 PS4コントローラー操作