2026年1月1日木曜日

ドローン ESP32 PS4コントローラー 机上テスト

PS4コントローラー とESP32制御ドローンをBluetooth接続による操作。9軸センサBMX055を読込Madgwickフィルタ・PID姿勢制御の動作確認を行いました。    　  
●ボードマネージャ　esp32    　 
　 by Espressif Systems 
　バージョン1.0.6 
※【PS4-esp32-master】使用するとesp32ボードマネージャ バージョン 2... , 3...はコンパイルエラーになる。


 



動画
スロットルを弱くして撮影
throttle = (JRY + 255)/2 * 0.02;





参考サイト
●趣味的ロボット研究所

回路図





















PS4-esp32-master libraries入手













【Code】【Download ZIP】
【PS4-esp32-master】圧縮ファイルを展開
【PS4-esp32-master】フォルダを任意のArduino librariesに貼付け


MACアドレスの確認

















【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の登録デバイスを解除）











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













モード１
PS4.LStickY() ↕  前進／後進　   PS4.LStickX() ↔ 左移動／右移動
PS4.RStickY() ↕ 上昇／下降　  PS4.RStickX() ↔ 左旋回／右旋回
PS4.R1() モータ起動     　　  PS4.R2() モータ停止
PS4.L1() キャリブレーション


ESP-32S 開発ボード
●ESP32 NodeMCU開発ボード2.4GHz WiFi + Bluetoothデュアルモード（技適製品）
アマゾン購入








SI2302A MOSFET
SI2302A MOSFETをヘッダーピンにハンダ付け、ブレッドボードに差込できるようにしました。
アマゾン購入








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 9軸センサ


























ドローン制御
          M3                             M4
M2                             M1

●フレーム　プロペラ　0615 モーター　














前進
M３・M４の回転 → 小
M１・M２の回転 → 大













後進

M３・M４の回転  → 大
M１・M２の回転  → 小













左移動
M１・M４の回転 → 大
M２・M３の回転 → 小













右移動

M2・M3の回転 → 大
M1・M4の回転  → 小












右旋回
M2・M４の回転 → 大
M1・M３の回転  → 小









 



左旋回

M1・M3の回転 → 大
M2・M4の回転  → 小














ESP32を2台登録すると1台目が接続できなくなる→解除方法
ESP32
【スケッチ例】
【BluetoothSerial】
【bt_remove_paired_devices】



bt_remove_paired_devices
【0】→【1】に変更

接続できないESP32に同プログラム書込→
プログラムを戻すと接続出来ました。
(ESP32の登録デバイスを解除）













ボードマネージャ esp32  by Espressif Systems バージョン1.0.6
※pwmを【ー１】以下にすると出力がHigh（最大回転）になる。
バージョン1.0.6
void setup() {
  ledcSetup(0,1000,8); //チャンネル,周波数,8bit=0-255(PWM)
  ledcSetup(1,1000,8);
  ledcSetup(2,1000,8);
  ledcAttachPin(25, 0); //ピン番号,チャンネル
  ledcAttachPin(26, 1);
  ledcAttachPin(27, 2);
  }
void loop() {
  ledcWrite(0,128);  //チャンネル,デューティ(50％）
  ledcWrite(1,300);
  ledcWrite(2,-1);  
}

ロジックアナライザ
G25 -D0
G26 -D1
G27 -D2
 ledcWrite(2,-1); 
pwmを-1以下に指定すると
出力→High






バージョン3.2.0
void setup() {
  //ESP32ボードマネージャ バージョン3以降
  ledcAttach(25,1000,8); //ピン番号,周波数,8bit=0-255(PWM)
  ledcAttach(26,1000,8);
  ledcAttach(27,1000,8);
}
void loop() {  
  //ESP32ボードマネージャ バージョン3以降
  ledcWrite(25,128);  //ピン番号,デューティ(50％）
  ledcWrite(26,300);
  ledcWrite(27,-1);
}

G25 -D0
G26 -D1
G27 -D2
 ledcWrite(2,-1); 
pwmを-1以下に指定すると
出力→Low






プログラム　Arduino IDE【ボード：NodeMCU-32s】
//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.2;
  ROLL_M = ROLL_M + JLY * 0.2;
  PITCH_M = PITCH_M - JLX * 0.2;

 //自動浮上
 if(Auto == 1 ){
   //定数
  Kp = 40;
  Ki = 0.1;
  Kd = 10;
  throttle = (JRY + 255)/2 * 0.8;
  }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);
}
時刻:
ラベル: , , ,

0 件のコメント:

コメントを投稿

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

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

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

  • 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 ...
  • 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受信 】 【...
  • uSDXトランシーバー製作（受信）
    Arduinoを使ってuSDXトランシーバー製作、こちらのブログを参考にしました。【 http://ja2gqp.blogspot.com/2021/ 】 【 https://hf5l.pl/en/transceiver-usdx-with-arduino-nano/ 】 JA2...