PS4_Mecanum_Wheels_Car_One_Driver1.ino

#include <PS4Controller.h>
#include <vector>

#define FORWARD 1
#define BACKWARD 2
#define TURN_LEFT_BACK 7
#define TURN_RIGHT_BACK 8
#define TURN_LEFT 9
#define TURN_RIGHT 10
#define STOP 0

#define BACK_RIGHT_MOTOR 0
#define BACK_LEFT_MOTOR 1
#define FRONT_RIGHT_MOTOR 2
#define FRONT_LEFT_MOTOR 3

#define BUZZER1 21
#define BUZZER2 22
#define BUZZER_CHANNEL1 8
#define BUZZER_CHANNEL2 9

struct MOTOR_PINS
{
  int pinIN1;
  int pinIN2;
  int pinEn;
  int pwmSpeedChannel;
};

std::vector<MOTOR_PINS> motorPins =
{
  {15, 13, 14, 4},  // BACK_RIGHT_MOTOR
  {2,  0,  12, 5},  // BACK_LEFT_MOTOR
  {4,  16, 17, 6},  // FRONT_RIGHT_MOTOR - UPDATED
  {18, 19, 21, 7}   // FRONT_LEFT_MOTOR - UPDATED
};

#define MAX_MOTOR_SPEED 255

const int PWMFreq = 1000; /* 1 KHz */
const int PWMResolution = 8;

#define SIGNAL_TIMEOUT 1000
unsigned long lastRecvTime = 0;

void notify()
{
  String inputData = "values " + String(PS4.LStickX()) + "  " + String(PS4.L2()) + "  " +
                     String(PS4.L2Value()) + "  " + String(PS4.R2()) + "  " + String(PS4.R2Value());
  Serial.println(inputData);

  int forwardSpeed = PS4.R2Value();
  int reverseSpeed = PS4.L2Value();

  if (PS4.LStickX() > 75)
  {
    if (PS4.R2())
      processCarMovement(TURN_RIGHT);
    else if (PS4.L1())
      processCarMovement(TURN_RIGHT_BACK);
    else
      processCarMovement(STOP);
  }
  else if (PS4.LStickX() < -75)
  {
    if (PS4.R2())
      processCarMovement(TURN_LEFT);
    else if (PS4.L1())
      processCarMovement(TURN_LEFT_BACK);
    else
      processCarMovement(STOP);
  }
  else if (forwardSpeed > 0)
  {
    processCarMovement(FORWARD);
  }
  else if (reverseSpeed > 0)
  {
    processCarMovement(BACKWARD);
  }
  else if (PS4.Touchpad())
  {
    ledcWrite(BUZZER_CHANNEL1, 128);
    ledcWrite(BUZZER_CHANNEL2, 128);
  }
  else
  {
    processCarMovement(STOP);
    ledcWrite(BUZZER_CHANNEL1, 0);
    ledcWrite(BUZZER_CHANNEL2, 0);
  }

  lastRecvTime = millis();
}

void processCarMovement(int inputValue)
{
  int forwardSpeed = PS4.R2Value();
  int reverseSpeed = PS4.L2Value();

  switch (inputValue)
  {
    case FORWARD:
      rotateMotor(FRONT_RIGHT_MOTOR, forwardSpeed);
      rotateMotor(BACK_RIGHT_MOTOR, forwardSpeed);
      rotateMotor(FRONT_LEFT_MOTOR, forwardSpeed);
      rotateMotor(BACK_LEFT_MOTOR, forwardSpeed);
      break;

    case BACKWARD:
      rotateMotor(FRONT_RIGHT_MOTOR, -reverseSpeed);
      rotateMotor(BACK_RIGHT_MOTOR, -reverseSpeed);
      rotateMotor(FRONT_LEFT_MOTOR, -reverseSpeed);
      rotateMotor(BACK_LEFT_MOTOR, -reverseSpeed);
      break;

    case TURN_RIGHT:
      rotateMotor(FRONT_RIGHT_MOTOR, forwardSpeed);
      rotateMotor(BACK_RIGHT_MOTOR, forwardSpeed);
      rotateMotor(FRONT_LEFT_MOTOR, -forwardSpeed * 0.1);
      rotateMotor(BACK_LEFT_MOTOR, -forwardSpeed * 0.1);
      break;

    case TURN_LEFT:
      rotateMotor(FRONT_RIGHT_MOTOR, -forwardSpeed * 0.1);
      rotateMotor(BACK_RIGHT_MOTOR, -forwardSpeed * 0.1);
      rotateMotor(FRONT_LEFT_MOTOR, forwardSpeed);
      rotateMotor(BACK_LEFT_MOTOR, forwardSpeed);
      break;

    case TURN_LEFT_BACK:
      rotateMotor(FRONT_RIGHT_MOTOR, reverseSpeed * 0.1);
      rotateMotor(BACK_RIGHT_MOTOR, reverseSpeed * 0.1);
      rotateMotor(FRONT_LEFT_MOTOR, -reverseSpeed);
      rotateMotor(BACK_LEFT_MOTOR, -reverseSpeed);
      break;

    case TURN_RIGHT_BACK:
      rotateMotor(FRONT_RIGHT_MOTOR, -reverseSpeed);
      rotateMotor(BACK_RIGHT_MOTOR, -reverseSpeed);
      rotateMotor(FRONT_LEFT_MOTOR, reverseSpeed * 0.1);
      rotateMotor(BACK_LEFT_MOTOR, reverseSpeed * 0.1);
      break;

    case STOP:
      rotateMotor(FRONT_RIGHT_MOTOR, 0);
      rotateMotor(BACK_RIGHT_MOTOR, 0);
      rotateMotor(FRONT_LEFT_MOTOR, 0);
      rotateMotor(BACK_LEFT_MOTOR, 0);
      break;
  }
}

void rotateMotor(int motorNumber, int motorSpeed)
{
  if (motorSpeed < 0)
  {
    digitalWrite(motorPins[motorNumber].pinIN1, LOW);
    digitalWrite(motorPins[motorNumber].pinIN2, HIGH);
  }
  else if (motorSpeed > 0)
  {
    digitalWrite(motorPins[motorNumber].pinIN1, HIGH);
    digitalWrite(motorPins[motorNumber].pinIN2, LOW);
  }
  else
  {
    digitalWrite(motorPins[motorNumber].pinIN1, LOW);
    digitalWrite(motorPins[motorNumber].pinIN2, LOW);
  }

  ledcWrite(motorPins[motorNumber].pwmSpeedChannel, abs(motorSpeed));
}

void setUpPinModes()
{
  for (int i = 0; i < motorPins.size(); i++)
  {
    pinMode(motorPins[i].pinIN1, OUTPUT);
    pinMode(motorPins[i].pinIN2, OUTPUT);
    pinMode(motorPins[i].pinEn, OUTPUT);

    ledcSetup(motorPins[i].pwmSpeedChannel, PWMFreq, PWMResolution);
    ledcAttachPin(motorPins[i].pinEn, motorPins[i].pwmSpeedChannel);

    rotateMotor(i, STOP);
  }
}

void setup()
{
  setUpPinModes();
  Serial.begin(115200);
  PS4.attach(notify);
  PS4.attachOnConnect([]() {
    Serial.println("Connected!");
  });
  PS4.attachOnDisconnect([]() {
    processCarMovement(STOP);
    Serial.println("Disconnected!");
  });
  PS4.begin();
  Serial.println("Ready.");

  ledcSetup(BUZZER_CHANNEL1, 1000, 8);
  ledcAttachPin(BUZZER1, BUZZER_CHANNEL1);
  ledcSetup(BUZZER_CHANNEL2, 1000, 8);
  ledcAttachPin(BUZZER2, BUZZER_CHANNEL2);
}

void loop()
{
  if (millis() - lastRecvTime > SIGNAL_TIMEOUT)
  {
    processCarMovement(STOP);
  }
}