LoginSignup
5
3

More than 1 year has passed since last update.

@thun_build

ESP32でDualShock4のIMUを使う

Last updated at Posted at 2022-03-06

概要

ESP32とBluetooth接続したDualShock4(DS4)のIMUを使う方法です。
DS4のIMUはIntel RealSenseにも搭載されている「BMI055」という6軸IMUらしいです。

ESP32でDS4を使用できるライブラリPS4-esp32を使用してIMUの値を取得します。

PS4-esp32ライブラリにはIMUやタッチパッドの入力を取得する関数が用意されていません。ソースコードには実装しようとした跡がありますが完成していないみたい？

DS4からデバイスに送信されるバイナリデータにはIMUやタッチパッドを含めた全ての値が含まれているため、バイナリデータを読み取る PS4.LatestPacket()関数を利用して、欲しい値を抜き取ります。

前提

ソフト バージョン
Arduino IDE 1.8.15
arduino-esp32 ボードマネージャ 2.0.2

PS4-esp32は、2022年3月7日時点のバージョンを使用。
以前はarduino-esp32 ボードマネージャver. 1.0.4でないとコンパイルエラーになっていたが更新されていました。
更新したい場合はArduino IDEのライブラリ格納フォルダから削除した後、新バージョンのZIPファイルをダウンロードし、Arduino IDEからインストールしてください。

ESP32とDS4の接続は解説しません。ライブラリのREADMEや日本語での解説記事を参考にして行ってください。
ESP32-DS4の接続とボタン入力等が確認できているとして進めます。

本編

以下のプログラムをESP32に書き込み、DS4とペアリングすれば値が見えるようになるはず。MACアドレスは各自の値を書き込んでください。
ご使用は自己責任でお願いします。
処理は単純で、DS4から返ってくるbyte列をps4_getIMU()関数で受け取り、ジャイロ加速度センサの値を抜き出しているだけです。
キャリブレーションを行う関数も追加しました。

#include <PS4Controller.h>

#define GYRO_X_INDEX 25
#define GYRO_Y_INDEX 27
#define GYRO_Z_INDEX 29
#define ACC_X_INDEX  31
#define ACC_Y_INDEX  33
#define ACC_Z_INDEX  35

int16_t ds4_imu[6]; // 0:gx, 1:gy, 2:gz, 3:ax, 4:ay, 5:az
float gyroOffset[6] = {};
float accOffset[6] = {};

// DS4のIMUを取得する関数
// 第一引数: DS4からの生データ
// 第二引数: 抽出したIMU値を格納する配列
void ps4_getIMU(uint8_t* packet, int16_t* imudata) {
  imudata[0] = (packet[GYRO_X_INDEX + 1] << 8) + packet[GYRO_X_INDEX]; // gx
  imudata[1] = (packet[GYRO_Y_INDEX + 1] << 8) + packet[GYRO_Y_INDEX]; // gy
  imudata[2] = (packet[GYRO_Z_INDEX + 1] << 8) + packet[GYRO_Z_INDEX]; // gz
  imudata[3] = (packet[ACC_X_INDEX + 1] << 8) + packet[ACC_X_INDEX]; // ax
  imudata[4] = (packet[ACC_Y_INDEX + 1] << 8) + packet[ACC_Y_INDEX]; // ay
  imudata[5] = (packet[ACC_Z_INDEX + 1] << 8) + packet[ACC_Z_INDEX]; // az
}

void ps4_calibrationIMU() {
  Serial.print("IMU Calibration Start.");
  float gyroSum[3];
  float accSum[3];
  int counter = 500;
  for (int i = 0; i < counter; i++) {
    ps4_getIMU(PS4.LatestPacket(), ds4_imu);
    gyroSum[0] += ds4_imu[0];
    gyroSum[1] += ds4_imu[1];
    gyroSum[2] += ds4_imu[2];
    accSum[0] += ds4_imu[3];
    accSum[1] += ds4_imu[4];
    accSum[2] += ds4_imu[5];
    delay(2);
    Serial.print('.');
  }
  Serial.println("Calibration Done.");
  gyroOffset[0] = gyroSum[0] / counter;
  gyroOffset[1] = gyroSum[1] / counter;
  gyroOffset[2] = gyroSum[2] / counter;
  accOffset[0] = accSum[0] / counter;
  accOffset[1] = accSum[1] / counter;
  accOffset[2] = accSum[2] / counter;
}

void ps4_applycalibrationIMU() {
  ds4_imu[0] -= gyroOffset[0];
  ds4_imu[1] -= gyroOffset[1];
  ds4_imu[2] -= gyroOffset[2];
  ds4_imu[3] -= accOffset[0];
  ds4_imu[4] -= accOffset[1];
  ds4_imu[5] -= accOffset[2];
}

void setup() {
  Serial.begin(115200);
  PS4.begin("YOUR_DS4_BLUETOOTH_MAC_ADDRESS"); // DS4と接続開始 MACアドレスは自分のコントローラの物に要変更
  Serial.println("Ready.");
  while (!PS4.isConnected()) {} // 接続されるまで待機
  Serial.println("Connected!");
  delay(1000); // DS4から値が送られていない状態でesp32が処理を行うと再起動してしまうため少し待機
  ps4_calibrationIMU(); // キャリブレーション
}

void loop() {
  ps4_getIMU(PS4.LatestPacket(), ds4_imu); // IMUデータを取得
  ps4_applycalibrationIMU(); // オフセットを適用

  static uint8_t mode = 0;

  if ( mode == 0 ) { // ジャイロmode
    Serial.print(ds4_imu[0]);
    Serial.print(", ");
    Serial.print(ds4_imu[1]);
    Serial.print(", ");
    Serial.println(ds4_imu[2]);
  } else if ( mode == 1 ) { // 加速度mode
    Serial.print(ds4_imu[3]);
    Serial.print(", ");
    Serial.print(ds4_imu[4]);
    Serial.print(", ");
    Serial.println(ds4_imu[5]);
  }

  if (PS4.PSButton()) ps4_calibrationIMU(); // PSボタンでキャリブレーション
  if (PS4.Triangle()) mode = 0; // △ボタンでジャイロmode 
  if (PS4.Circle()) mode = 1; // ○ボタンで加速度mode
}

解説

以下のページにDS4のデータフォーマットについての記載があります。

DS4から返ってくる64bytesのデータのうち最下位から数えてindex[13~24]がジャイロと加速度センサの値となっているようです。また、[1]はLスティックのX軸の値のようです。

byte index data
[0] Report ID (USB)
[1] Dualshock Left stick X axis (0 = left)
~ ~
[13 - 14] Gyro X: angular velocity measures (follows right-hand-rule)
[15 - 16] Gyro Y
[17 - 18] Gyro Z
[19 - 20] Accel X (signed): acceleration (positive: right)
[21 - 22] Accel Y (signed): acceleration (positive: up)
[23 - 24] Accel Z (signed): acceleration (positive: towards player)

PS4-esp32ライブラリのサンプルプログラムPs4ViewIncomingBitsで値を確認してみます。

操作なし
BYTE 63 :	00000000 00000000 00000000 10000000	: BYTE 60
BYTE 59 :	00000000 00000000 00000000 10000000	: BYTE 56
BYTE 55 :	00000000 00000000 00000000 00000000	: BYTE 52
BYTE 51 :	10000000 00000000 00000000 00000000	: BYTE 48
BYTE 47 :	10000000 00000000 00000000 00000000	: BYTE 44
BYTE 43 :	00000000 00000100 00000000 00000000	: BYTE 40
BYTE 39 :	00000000 00000000 00000000 00000110	: BYTE 36
BYTE 35 :	11010010 00011110 11110101 11111111	: BYTE 32
BYTE 31 :	10111110 00000000 00000000 11111111	: BYTE 28
BYTE 27 :	11111111 11111111 11110111 00000010	: BYTE 24
BYTE 23 :	10111110 10010100 00000000 00000000	: BYTE 20
BYTE 19 :	10001100 00000000 00001000 01111110	: BYTE 16
BYTE 15 :	10000001 10000010 01111000 00000000	: BYTE 12
BYTE 11 :	11000000 00010001 10100001 00000000	: BYTE 8
BYTE 7 :	01000000 00000000 01001111 00000000	: BYTE 4
BYTE 3 :	01010011 00100000 10000000 00000010	: BYTE 0
Lスティック左に倒す
BYTE 63 :	00000000 00000000 00000000 10000000	: BYTE 60
BYTE 59 :	00000000 00000000 00000000 10000000	: BYTE 56
BYTE 55 :	00000000 00000000 00000000 00000000	: BYTE 52
BYTE 51 :	10000000 00010111 10110001 00010111	: BYTE 48
BYTE 47 :	00000001 10001011 00000001 00000000	: BYTE 44
BYTE 43 :	00000000 00000011 00000000 00000000	: BYTE 40
BYTE 39 :	00000000 00000000 00000000 00000110	: BYTE 36
BYTE 35 :	10100100 00011111 00100011 11111111	: BYTE 32
BYTE 31 :	11001101 11111111 11111111 11111111	: BYTE 28
BYTE 27 :	00000000 11111111 11111000 00000101	: BYTE 24
BYTE 23 :	10000000 00000100 00000000 00000000	: BYTE 20
BYTE 19 :	00110000 00000000 00001000 01111101	: BYTE 16
BYTE 15 :	01111110 10010100 00000000 00000000	: BYTE 12
BYTE 11 :	11000000 00010001 10100001 00000000	: BYTE 8
BYTE 7 :	01000000 00000000 01001111 00000000	: BYTE 4
BYTE 3 :	01010011 00100000 10000000 00000010	: BYTE 0
Lスティック右に倒す
BYTE 63 :	00000000 00000000 00000000 10000000	: BYTE 60
BYTE 59 :	00000000 00000000 00000000 10000000	: BYTE 56
BYTE 55 :	00000000 00000000 00000000 00000000	: BYTE 52
BYTE 51 :	10000000 00010110 11100001 01000111	: BYTE 48
BYTE 47 :	10000001 10001110 00000001 00000000	: BYTE 44
BYTE 43 :	00000000 00000011 00000000 00000000	: BYTE 40
BYTE 39 :	00000000 00000000 00000000 00000110	: BYTE 36
BYTE 35 :	10010110 00011110 11111101 11111111	: BYTE 32
BYTE 31 :	11111001 11111111 11111010 11111111	: BYTE 28
BYTE 27 :	11111110 11111111 11111001 00000101	: BYTE 24
BYTE 23 :	10011010 01110000 00000000 00000000	: BYTE 20
BYTE 19 :	11001000 00000000 00001000 01111101	: BYTE 16
BYTE 15 :	01111111 01101100 11111111 00000000	: BYTE 12
BYTE 11 :	11000000 00010001 10100001 00000000	: BYTE 8
BYTE 7 :	01000000 00000000 01001111 00000000	: BYTE 4
BYTE 3 :	01010011 00100000 10000000 00000010	: BYTE 0

byte index[13]の値が目一杯に変化しているのがわかるでしょうか？
index[1]であるはずのLスティックの値が[13]の値になっています。
このように、ESP32で受け取る値のindexはここに記載されているindexよりも12大きくなっているため、それを考慮してマクロ定数を設定しました。

動作確認

プログラムを実行してシリアルプロッタを起動すると、IMUの値がグラフに描画されます。
DS4の△ボタンでジャイロセンサの値を表示するgyro mode、○ボタンで加速度センサの値を表示するaccel modeに切り替わります。

gyro mode

青:X軸, 赤:Y軸, 緑:Z軸周りの角速度を検知しています。

image.png

accel mode

青:X軸, 赤:Y軸, 緑:Z軸向きの加速度を検知しています。

image.png

まとめ

DualShock4のIMUをESP32で利用できるようになりました。
読み込むindexを変更すればタッチパッドも使えるようになるはずです。

参考

https://lang-ship.com/blog/work/m5stickc-imu-mpu6886/
https://qiita.com/Geek493/items/8402ad875b88822e75ab
https://qiita.com/hatt_takumi/items/a9691e5236a1cacafa7e
https://www.psdevwiki.com/ps4/DS4-USB

5
3
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
5
3