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

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() {
}

Ps3.begin("01:02:03:04:05:06");　←この部分

#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.");
}

#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);
}