2026年4月1日水曜日

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

Bluetooth (Classic / BLE)対応のBluepad32ライブラリー使用して、無印ESP32制御ドローンを純正PS4コントローラーで操作しました。（Xboxコントローラーに入替て操作出来ます）













参考サイト


回路図





















PS4コントローラー(Bluetooth)
PSボタンを押して電源オン（接続）














PS4コントローラー
【MODEL CUH-ZCT2J】
技適製品





R Stick X↔ 左旋回／右旋回   
R Stick Y↕ 上昇／下降　 
L Stick X↔ 左移動／右移動
L Stick Y ↕  前進／後進
R1 モータ起動  
R2 モータ停止 　　  
L1 キャリブレーション




ESP32-WROOM-32E

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












基板ハンダ












丸ピンヘッダ ・ソケット
●丸ピンヘッダ ・ソケット2.54mmピッチ
ESP32・BMX055とドライブ基板接続

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






丸ピンヘッダハンダ












SI2302A









ADP3338
●表面実装低損失レギュレーターADP3338AKCZ-3.3
ドロップアウト電圧：190mV









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





●BETAFPV プロペラ HQ 31mm 3-Blade Propellers (0.8mm Shaft 4PCS) 【クリアグ
レー】（BETA 65S 2022Ver.などに）






基板







9軸センサBMX055







●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　⊖











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




ドライバーインストール
●【CH9102_VCP_SER_Windows】選択してドライバーをダウンロードする
●ドライバーをインストールする
操作
プッシュスイッチ操作無しでプログラム書込
み・実行を自動で行ってくれる。






プログラム書込み
変換コネクタ製作
ピンソケットと丸ピンヘッダをはんだし、チューブを熱収縮する（ピンヘッダの表面にハンダを流す）
























モータ位置
0716モーター
■CWモーター（時計回転）
赤⊕　黒⊖
M1 M3
■CCWモーター（反時計回転）
白⊕　黒⊖
M2 M4









ボードマネージャ
追加のボードマネージャー
【https://raw.githubusercontent.com/ricardoquesada/esp32-arduino-lib-builder/master/bluepad32_files/package_esp32_bluepad32_index.json】

【ツール】【ボード】【ボードマネージャ】【bluepad32】
















ボードマネージャ【ESP32 Arduino + Bluepad32】
ボード【ESP32 Dev Module】選択

プログラム　Arduino IDE  【ESP32 Arduino + Bluepad32】【ESP32 Dev Module】

#include <Bluepad32.h>
ControllerPtr myControllers[BP32_MAX_GAMEPADS];

#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 onConnectedController(ControllerPtr ctl) {
 bool foundEmptySlot = false;
 for (int i = 0; i < BP32_MAX_GAMEPADS; i++) {
 if (myControllers[i] == nullptr) {
  Serial.printf("CALLBACK: Controller is connected, index=%d\n", i);
  ControllerProperties properties = ctl->getProperties();
  Serial.printf("Controller model: %s, VID=0x%04x, PID=0x%04x\n", ctl->getModelName().c_str(), properties.vendor_id, properties.product_id);
  myControllers[i] = ctl;
  foundEmptySlot = true;
  break;
  }
 }
 if (!foundEmptySlot) {
  Serial.println("CALLBACK: Controller connected, but could not found empty slot");
 }
}

void onDisconnectedController(ControllerPtr ctl) {
 bool foundController = false;

 for (int i = 0; i < BP32_MAX_GAMEPADS; i++){
  if (myControllers[i] == ctl){
   Serial.printf("CALLBACK: Controller disconnected from index=%d\n", i);
   myControllers[i] = nullptr;
   foundController = true;
   break;
  }
 }
 if (!foundController) {
   Serial.println("CALLBACK: Controller disconnected, but not found in myControllers");
 }
}

/*操作ボタン
 if (ctl->a()) Serial.printf("A\n");
 if (ctl->b()) Serial.printf("B\n");
 if (ctl->x()) Serial.printf("X\n");
 if (ctl->y()) Serial.printf("Y\n");
 if (ctl->l1()) Serial.printf("L‾\n"); //上左ボタン
 if (ctl->r1()) Serial.printf("R‾\n"); //上右ボタン
 if (ctl->l2()) Serial.printf("L_\n"); //下左ボタン
 if (ctl->r2()) Serial.printf("R_\n"); //下右ボタン

 //方向パッド
 if (ctl->dpad()& DPAD_UP) Serial.printf("UP\n");
 if (ctl->dpad()& DPAD_DOWN) Serial.printf("DOWN\n");
 if (ctl->dpad()& DPAD_LEFT) Serial.printf("LEFT\n");
 if (ctl->dpad()& DPAD_RIGHT) Serial.printf("RIGHT\n");

 //他ボタン
 if (ctl->miscButtons()& MISC_BUTTON_SYSTEM) Serial.printf("XBOX\n");
 if (ctl->miscButtons()& MISC_BUTTON_BACK) Serial.printf("BACK\n");
 if (ctl->miscButtons()& MISC_BUTTON_HOME) Serial.printf("HOME\n");

 //左右ジョイスティック
 int RX = ctl->axisRX();  //右ジョイスティック
 Serial.print("RX:");  
 Serial.println(RX);  //左　-512 ~ 511　右
 int RY = ctl->axisRY();
 Serial.print("RY:");  //上　-512 ~ 511　下
 Serial.println(RY);  
 int LX = ctl->axisX();  //左ジョイスティック
 Serial.print("LX:");
 Serial.println(LX);  //左　-512 ~ 511　右
 int LY = ctl->axisY();
 Serial.print("LY:");
 Serial.println(LY);  //上　-512 ~ 511　下
*/

void processGamepad(ControllerPtr ctl) {
//方向パッド
 if (ctl->r1()){ //モータ起動
  Auto = 1;
  }
 if (ctl->r2()){ //モータ停止
  Auto = 0;
 }
 if (ctl->l1()){ //キャリブレーション
  CAL = 1;
 }
 if (ctl->l2()){  
 }
  int RX = ctl->axisRX();  //左　-512 ~ 511　右
  int RY = ctl->axisRY();  //上　-512 ~ 511　下
  int LX = ctl->axisX();  //左　-512 ~ 511　右
  int LY = ctl->axisY();  //上　-512 ~ 511　下
 
  JRX =  map(RX, -500, 500, -255, 255);  //左旋回／右旋回
  JRY =  map(-RY, -500, 500, -255, 255); //上昇／下降
  JLX =  map(LX, -500, 500, -255, 255);  //左移動／右移動    
  JLY =  map(-LY, -500, 500, -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;
  throttle = (JRY + 255)/2 * 0.4; //test
  }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);
}  

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 processControllers() {
 for (auto myController : myControllers) {
  if (myController && myController->isConnected() && myController->hasData()) {
   if (myController->isGamepad()) {
    processGamepad(myController);
   }
   else {
    Serial.println("Unsupported controller");
   }
  }
 }
}

void setup()
{
  pinMode(12, OUTPUT);   //LED +
  digitalWrite(12, HIGH);
  pinMode(27, OUTPUT);  
  digitalWrite(27, LOW); //LED -
  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();  
  BMX055_Init(); //BMX055の初期化
  delay(300);
  MadgwickFilter.begin(100);//フィルタのサンプリングを100Hzで
  R = 0;
  P = 0;
  Y = 0;
  Kp = 0;
  Ki = 0;
  Kd = 0;
  Serial.begin(115200);
  Serial.printf("Firmware: %s\n", BP32.firmwareVersion());
  const uint8_t* addr = BP32.localBdAddress();
  Serial.printf("BD Addr: %2X:%2X:%2X:%2X:%2X:%2X\n", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);

 // Setup the Bluepad32 callbacks
  BP32.setup(&onConnectedController, &onDisconnectedController);
  BP32.forgetBluetoothKeys();
}

void loop()
{
 bool dataUpdated = BP32.update();
 if (dataUpdated) processControllers();
 delay(10);
}


時刻:
ラベル: , ,

0 件のコメント:

コメントを投稿

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

ESP32 - Bluepad32 ロボカー PS4コントローラー操作

  Bluetooth (Classic / BLE)対応の Bluepad32ライブラリー使用して、 無印E SP32制御ロボットカーを純正PS4コントローラーで操作しました。 （Xboxコントローラーに入替て操作出来ます） 同ブログ【 ESP32 - Bluepad32 PS...

  • Pi Pico Rx - SDR試作 AM放送受信
    Pi Pico Rx(0-30MHz,CW/SSB/AM/FM)シンプルSDRを試作し、注目したPico PIOアッセンブルによるIQ局発と直交ミキサの構成でAM放送の受信を行いました。  同ブログ関連記事 【 Pi Pico Rx - SDR試作 14MHz FT8受信 】 【...
  • Pi Pico Rx - SDR試作 7MHz SSB CW受信
    Pi Pico Rx(0-30MHz,CW/SSB/AM/FM)シンプルSDRを試作し、注目したPico PIOアッセンブルによるIQ局発と直交ミキサの構成で7MHz SSB CWの受信を行いました（ FMは29MHzFMモードで受信確認） 同ブログ関連記事 【 Pi Pico ...
  • uSDXトランシーバー製作（受信）
    Arduinoを使ってuSDXトランシーバー製作、こちらのブログを参考にしました。【 http://ja2gqp.blogspot.com/2021/ 】 【 https://hf5l.pl/en/transceiver-usdx-with-arduino-nano/ 】 JA2...