ESP32でDualShock4のIMUを使う

# 概要
ESP32とBluetooth接続したDualShock4(DS4)のIMUを使う方法です。
DS4のIMUはIntel RealSenseにも搭載されている「BMI055」という6軸IMUらしいです。
  
https://forum.beyond3d.com/threads/inertial-sensors-dualshock-4-vs-sixaxis-ds3-accelerometer-gyroscope-chips.56000/
  
ESP32でDS4を使用できるライブラリ[PS4-esp32](https://github.com/aed3/PS4-esp32.git)を使用してIMUの値を取得します。
  
PS4-esp32ライブラリにはIMUやタッチパッドの入力を取得する関数が用意されていません。[ソースコード](https://github.com/aed3/PS4-esp32/blob/master/src/ps4_parser.c)には実装しようとした跡がありますが完成していないみたい？  
  
DS4からデバイスに送信されるバイナリデータにはIMUやタッチパッドを含めた全ての値が含まれているため、バイナリデータを読み取る ```PS4.LatestPacket()```関数を利用して、欲しい値を抜き取ります。 
  
https://github.com/aed3/PS4-esp32.git  
  
## 前提

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

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

ESP32とDS4の接続は解説しません。[ライブラリのREADME](https://github.com/aed3/PS4-esp32.git)や[日本語での解説記事](https://qiita.com/Geek493/items/8402ad875b88822e75ab)を参考にして行ってください。
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のデータフォーマットについての記載があります。
  
https://www.psdevwiki.com/ps4/DS4-USB#Data_Format
  
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](https://github.com/aed3/PS4-esp32/blob/master/examples/Ps4ViewIncomingBits/PS4ViewIncomingBits.ino)で値を確認してみます。

```text:操作なし
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
```

```text: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
```

```text: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は[ここ](https://www.psdevwiki.com/ps4/DS4-USB#Data_Format)に記載されているindexよりも12大きくなっているため、それを考慮してマクロ定数を設定しました。

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

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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2050833/5f9890e8-9c38-bd3e-02f8-48bf951a0533.png "gyro mode")

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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2050833/891a62b6-e371-94e8-e9e4-6bf7be60edbc.png "accel mode")


# まとめ
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