Mecanum-dualshock3

//ESP32 Arduino Core 2.0.5
//PS3 Controller Host library 1.1.0

#include <Ps3Controller.h>

//#define NUM_LEDS 12
//#define LED_PIN 27
//#define BTN_PIN 39
//CRGB leds[NUM_LEDS];

#define DEBUG

//Bluetooth MAC変更(変更しない場合はコメント)
//#define BTMAC "1a:2b:3c:01:01:01"

//ピン設定
#define M1p 17
#define M1n 18
#define M2p 4
#define M2n 5
#define M3p 22
#define M3n 23
#define M4p 19
#define M4n 21

//パラメータ
#define PWMfreq 2000
#define PWMreso 8 //8bit 0-255
#define PWMmax 255 //2^PWMreso - 1
#define DEADZone 8

//進行方向用各要素定義
int8_t MoveY[4] = { 50, 50, 50, 50 };
int8_t MoveX[4] = { -50, 50, 50, -50 };
int8_t ROTATE[4] = { 30, -30, 30, -30 };

//コントローラーの値代入用
int lx = 0;
int ly = 0;
int rx = 0;
int ry = 0;
int lt = 0;
int rt = 0;
boolean BTconnected = false;

//モーター出力設定
int8_t speed_sendbuff[4] = { 0 };

//ポーリング用
unsigned long lastMotor = 0;
unsigned long lastupdate = 0;

void notify()
{
  lx = Ps3.data.analog.stick.lx;
  ly = Ps3.data.analog.stick.ly;
  rx = Ps3.data.analog.stick.rx;
  ry = Ps3.data.analog.stick.ry;
  if (abs(lx) < DEADZone) {
    lx = 0;
  }
  if (abs(ly) < DEADZone) {
    ly = 0;
  }
  if (abs(rx) < DEADZone) {
    rx = 0;
  }
  if (abs(ry) < DEADZone) {
    ry = 0;
  }
  lt = Ps3.data.analog.button.l2;
  rt = Ps3.data.analog.button.r2;

  /*
    Serial.print(lx);
    Serial.print(",");
    Serial.print(ly);
    Serial.print(",");
    Serial.print(rx);
    Serial.print(",");
    Serial.println(ry);
  */
}

void btdisconn() {
  BTconnected = false;
#ifdef DEBUG
  Serial.println("Controller Disconnected");
#endif
}

void btconn() {
  BTconnected = true;
#ifdef DEBUG
  Serial.println("Controller Connected");
#endif
}

void setup() {
#ifdef DEBUG
  Serial.begin(115200);
#endif

  //PWMタイマ設定
  ledcSetup(0, PWMfreq, PWMreso); //ch,kHz,bit
  ledcSetup(1, PWMfreq, PWMreso);
  ledcSetup(2, PWMfreq, PWMreso);
  ledcSetup(3, PWMfreq, PWMreso);
  ledcSetup(4, PWMfreq, PWMreso);
  ledcSetup(5, PWMfreq, PWMreso);
  ledcSetup(6, PWMfreq, PWMreso);
  ledcSetup(7, PWMfreq, PWMreso);

  //PWMタイマに出力PIN関連付け
  ledcAttachPin(M1p, 0);
  ledcAttachPin(M1n, 1);
  ledcAttachPin(M2p, 2);
  ledcAttachPin(M2n, 3);
  ledcAttachPin(M3p, 4);
  ledcAttachPin(M3n, 5);
  ledcAttachPin(M4p, 6);
  ledcAttachPin(M4n, 7);

  //DUALSHOCK3初期化
  Ps3.attach(notify);
  Ps3.attachOnDisconnect(btdisconn);
  Ps3.attachOnConnect(btconn);
#ifdef BTMAC
  Ps3.begin(BTMAC);
#else
  Ps3.begin();
#endif

  //Wire.begin(26,32);//SDA SCL
}



void loop() {
  /* if (lastupdate + 2000 <= millis()) {
     lastupdate = millis();//次の実行のために時間更新
     Serial.println("time");
     if (!Ps3.isConnected()) {
       setspeed(0, 0, 0); //切断されたら2秒以内に停止
     }
    }*/
  if (lastMotor + 50 <= millis()) {
    lastMotor = millis();//次の実行のために時間更新
    if (BTconnected == true) {
      setspeed(lx / 127.00, ly / -127.00, rx / 127.00);
    } else {
      setspeed(0, 0, 0);
    }

  }
  // Serial.println("scanning:" + String(scanning) + " connected:" + String(connected) + " advDevice is nullptr:" + String(advDevice == nullptr));
  //delay(2000);
}

void setspeed(float x, float y, float r) {  // 前後左右回転それぞれを係数と掛けて足す
  for (int i = 0; i < 4; i++) {
    speed_sendbuff[i] = MoveY[i] * y;
    speed_sendbuff[i] += MoveX[i] * x;
    speed_sendbuff[i] += ROTATE[i] * r;
  }
  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;
    }
  }

  int motorpwm[4] = {0};
  for (uint8_t i = 0; i < 4; i++) {
    motorpwm[i] = int(speed_sendbuff[i] * 2.55);
  }

  setMotor(motorpwm);
}

void setMotor(int *pwm) {
  for (uint8_t i = 0; i <= 3; i++) {
    if (pwm[i] == 0) { //フリー
      ledcWrite((i * 2), 0);
      ledcWrite(((i * 2) + 1), 0);
    } else if (pwm[i] > 0 && pwm[i] <= 255) {
      ledcWrite((i * 2), PWMmax - pwm[i]);
      ledcWrite(((i * 2) + 1), PWMmax);
    } else if (pwm[i] < 0 && pwm[i] >= -255) {
      ledcWrite((i * 2), PWMmax);
      ledcWrite(((i * 2) + 1), PWMmax + pwm[i]);
    }
  }
#ifdef DEBUG
  for (uint8_t i = 0; i <= 3; i++) {
    Serial.print(pwm[i]);
    Serial.print(",");
  }
  Serial.println("");
#endif
}