ESP32(M5StickC)をPS3コントローラで操作する

もともとBLEコントローラでESP32を操作したい！ということでNimBLEを使ってたのですが、

NimBLEでESP32(M5StickC)にBLE HIDデバイスを接続する(追記:複数デバイス対応) - Qiita
https://qiita.com/coppercele/items/4ce0e8858a92410c81e3

BLEコントローラが売ってなくて再現性がないということで心が折れたのでPS3コントローラでESP32を操作してみることにします

こんな感じで操作できます

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">PS3コントローラで <a href="https://twitter.com/hashtag/RoverC?src=hash&amp;ref_src=twsrc%5Etfw">#RoverC</a> を操作してみました<br>アナログで繊細な操作はコントローラの利点ですね<a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a><br><br>自分流メカナムホイール(RoverC)制御法<a href="https://t.co/hP6kiBWoLl">https://t.co/hP6kiBWoLl</a> <a href="https://t.co/YT05rkDavL">pic.twitter.com/YT05rkDavL</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1305900719417876481?ref_src=twsrc%5Etfw">September 15, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

こちらのサイトを参考にさせていただきました

・ESP32 (M5StickC) で PS3 の コントローラ を使う : (Blog)SKYZOO(ヨッシーの備忘録(プチ情報))
http://gijin77.blog.jp/archives/21966552.html


##ハードウェア##

**M5StickC**
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/cd2f180f-a233-a51d-ee30-19dc459e1dd3.png)

M5StickC - スイッチサイエンス
https://www.switch-science.com/catalog/5517/
Arduino互換なESP32がケースに入っててボタンとディスプレイとバッテリーと赤外線LEDがついててWifiとBluetoothも使える欲張りセット

---

PS3コントローラー

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/1c124307-ba72-5d1c-f8af-3ee1dad600f8.png)

純正品のいわゆるDUAL SHOCK3
ハードオフにて1,000円前後で売ってるのでコスパが良い
ただ手垢汚れをきれいにするのがめんどくさい

##Bluetoothアドレスの登録##

PS3コントローラをペアリングするためにはコントローラにEPS32のBluetoothアドレスを書き込まないといけません

ESP32に以下のスケッチを書き込み実行します

```cpp
void setup(void) {
  Serial.begin(115200);
  uint8_t btmac[6];
  esp_read_mac(btmac, ESP_MAC_BT);
  Serial.printf("[Bluetooth] Mac Address = %02X:%02X:%02X:%02X:%02X:%02X\r\n", btmac[0], btmac[1], btmac[2], btmac[3], btmac[4], btmac[5]);
}

void loop() {
}
```
このスケッチを実行するとシリアルモニタにアドレスが表示されるのでメモしておきます

こちらから書き込みソフトをダウンロードします

Download SixaxisPairTool by Dancing Pixel Studios
https://sixaxispairtool.software.informer.com/download/

PS3コントローラをPCに接続し、ソフトをインストールして実行するとこのような画面が表示されますので先ほどメモしたアドレスを入力してUpdateボタンを押すと書き込みが完了します

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/e6e3968a-e17e-eb3e-f7f8-c626ecb6b518.png)


##ライブラリのインストール##

Arduino IDEのライブラリマネージャでPS3を検索すると出てきますのでインストールしましょう

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/e42f1cf6-c02c-5d74-2091-c6b2fbec3200.png)

スケッチ例 > PS3 Controller Host > Ps3Demo
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/298776/501bc145-0aaa-356e-ed65-8df5855213cd.png)

のBluetoothアドレス部分をESP32の物に変更して実行し、PS3コントローラのPSボタンを押すと接続できます

```
Ps3.begin("01:02:03:04:05:06");　←この部分
```
接続できたらPS3コントローラのボタンを押すと結果がシリアルモニタに表示されます

```cpp
#include <Ps3Controller.h>

void notify() {
  // ボタンを押したときに呼び出されるコールバック関数
    // ボタンそれぞれにメンバが用意されている
    if( Ps3.event.button_down.up )
        Serial.println("Started pressing the up button");
    if( Ps3.event.button_up.up )
        Serial.println("Released the up button");
  // 以下略
}
void onConnect(){
  // 接続されたときに呼び出されるコールバック関数
  Serial.println("Connected!.");
}

void onDisconnect(){
  // 切断されたときに呼び出されるコールバック関数
  Serial.println("DisConnected!.");
}

void setup() {
    Serial.begin(115200);
    // コールバック関数の登録(オプション)
    Ps3.attach(notify);
    // コールバック関数の登録(オプション)
    Ps3.attachOnConnect(onConnect);
    // コールバック関数の登録(オプション)
    Ps3.attachOnDisconnect(onDisconnect);
    // 接続するデバイスのアドレスに書き換える
    Ps3.begin("01:02:03:04:05:06");
    Serial.println("Ready.");
}
```

##RoverCを操作してみた##

せっかくなのでRoverCを操作してみることにします

こちらでRoverCの制御は確立していたのでこちらを組み合わせます

自分流メカナムホイール(RoverC)制御法 - Qiita
https://qiita.com/coppercele/items/1596b7b9904eb4403191

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">PS3コントローラで <a href="https://twitter.com/hashtag/RoverC?src=hash&amp;ref_src=twsrc%5Etfw">#RoverC</a> を操作してみました<br>アナログで繊細な操作はコントローラの利点ですね<a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a><br><br>自分流メカナムホイール(RoverC)制御法<a href="https://t.co/hP6kiBWoLl">https://t.co/hP6kiBWoLl</a> <a href="https://t.co/YT05rkDavL">pic.twitter.com/YT05rkDavL</a></p>&mdash; もけ@ムギ㌠ (@coppercele) <a href="https://twitter.com/coppercele/status/1305900719417876481?ref_src=twsrc%5Etfw">September 15, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

notify()内で左右スティックの値を取得してRoverCの操作をしています


```cpp
#include <M5StickC.h>
#include <Ps3Controller.h>

int8_t speed_sendbuff[4] = { 0 };

int8_t FORWARD[4] = { 50, 50, 50, 50 };
int8_t LEFT[4] = { -50, 50, 50, -50 };
int8_t BACKWARD[4] = { -50, -50, -50, -50 };
int8_t RIGHT[4] = { 50, -50, -50, 50 };
int8_t ROTATE_R[4] = { 30, -30, 30, -30 };
int8_t ROTATE_L[4] = { -30, 30, -30, 30 };

float f, b, l, r, rr, rl = 0.0;

void onConnect() {
    Serial.println("Connected!.");
    digitalWrite(GPIO_NUM_10, LOW);
}
void onDisconnect() {
    Serial.println("Disconnected!.");
    digitalWrite(GPIO_NUM_10, HIGH);
}

void SetChargingCurrent(uint8_t CurrentLevel) {
    Wire1.beginTransmission(0x34);
    Wire1.write(0x33);
    Wire1.write(0xC0 | (CurrentLevel & 0x0f));
    Wire1.endTransmission();
}

int8_t I2CWrite1Byte(uint8_t Addr, uint8_t Data) {
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    Wire.write(Data);
    return Wire.endTransmission();
}

uint8_t I2CWritebuff(uint8_t Addr, uint8_t *Data, uint16_t Length) {
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    for (int i = 0; i < Length; i++) {
        Wire.write(Data[i]);
    }
    return Wire.endTransmission();
}

uint8_t setspeed() {  // 前後左右回転それぞれを係数と掛けて足す
    for (int i = 0; i < 4; i++) {
        speed_sendbuff[i] = FORWARD[i] * f;
        speed_sendbuff[i] += BACKWARD[i] * b;
        speed_sendbuff[i] += RIGHT[i] * r;
        speed_sendbuff[i] += LEFT[i] * l;
        speed_sendbuff[i] += ROTATE_L[i] * rl;
        speed_sendbuff[i] += ROTATE_R[i] * rr;
    }
    float limit = 0.0;
    for (int i = 0; i < 4; i++) {
        // speedが100を超えないようにリミッターをかける
        limit = 100.0 / max(abs(speed_sendbuff[3]), max(abs(speed_sendbuff[2]), max(abs(speed_sendbuff[1]), abs(speed_sendbuff[0]))));
    }
    //    printf("limit = %f\n", limit);
    if (limit < 1.0) {
        for (int i = 0; i < 4; i++) {
            speed_sendbuff[i] = speed_sendbuff[i] * limit;
        }
    }

    return I2CWritebuff(0x00, (uint8_t*) speed_sendbuff, 4);
}

void notify() {
    if (Ps3.isConnected()) {
        if (Ps3.data.analog.stick.ly < -10) {
            Serial.print("Moved the left stick:");
            Serial.printf(" ly=%d\n", Ps3.data.analog.stick.ly);
            f = (abs(Ps3.data.analog.stick.ly) / 128.0);
            b = 0.0;
        }
        else if (10 < Ps3.data.analog.stick.ly) {
            Serial.print("Moved the left stick:");
            Serial.printf(" ly=%d\n", Ps3.data.analog.stick.ly);
            b = (abs(Ps3.data.analog.stick.ly) / 128.0);
            f = 0.0;
        }
        else {
            Serial.printf(" ly=%d", Ps3.data.analog.stick.ly);
            Serial.printf(" left V neutral\n");
            b = 0.0;
            f = 0.0;
        }
        if (Ps3.data.analog.stick.lx < -10) {
            Serial.print("Moved the left stick:");
            Serial.printf(" lx=%d", Ps3.data.analog.stick.lx);
            l = (abs(Ps3.data.analog.stick.lx) / 128.0);
            r = 0.0;
        }
        else if (10 < Ps3.data.analog.stick.lx) {
            Serial.print("Moved the left stick:");
            Serial.printf(" lx=%d", Ps3.data.analog.stick.lx);
            r = (abs(Ps3.data.analog.stick.lx) / 128.0);
            l = 0.0;
        }
        else {
            Serial.printf("lx=%d", Ps3.data.analog.stick.lx);
            Serial.printf(" left H neutral\n");
            r = 0.0;
            l = 0.0;
        }
        if (Ps3.data.analog.stick.rx < -10) {
            Serial.print("Moved the right stick:");
            Serial.printf(" rx=%d", Ps3.data.analog.stick.rx);
            rl = (abs(Ps3.data.analog.stick.rx) / 128.0);
            rr = 0.0;
        }
        else if (10 < Ps3.data.analog.stick.rx) {
            Serial.print("Moved the right stick:");
            Serial.printf(" rx=%d", Ps3.data.analog.stick.rx);
            rr = (abs(Ps3.data.analog.stick.rx) / 128.0);
            rl = 0.0;
        }
        else {
            Serial.printf("rx=%d", Ps3.data.analog.stick.rx);
            Serial.printf(" right H neutral\n");
            rr = 0.0;
            rl = 0.0;
        }
        setspeed();
    }
}

void setup() {
    Serial.begin(115200);

    M5.begin();
    M5.update();
    Wire.begin(0, 26, 10000);
    SetChargingCurrent(2);
    M5.Axp.ScreenBreath(7);
    M5.Lcd.fillScreen(BLACK);
    pinMode(GPIO_NUM_10, OUTPUT);
    digitalWrite(GPIO_NUM_10, HIGH);

    Ps3.attach(notify);
    Ps3.attachOnConnect(onConnect);
    Ps3.attachOnDisconnect(onDisconnect);
    Ps3.begin("D8:A0:1D:52:9C:C2");
    Serial.println("Ready.");
}

void loop() {
    M5.update();
    if (M5.BtnB.wasReleased()) {
        esp_restart();
    }

    if (M5.BtnA.wasReleased()) {
        M5.Axp.PowerOff();
    }


    delay(1);
}

```