// RC car: ESP32 + PS5 controller over Bluetooth
// Left stick steers, R2 goes forward, L2 reverses.

#include <ps5Controller.h>
#include <ESP32Servo.h>

#define ENA 25        // L298N, jumper removed
#define IN1 26
#define IN2 27
#define STEER_PIN 13

Servo steering;
ESP32PWM motor;

void drive(int speed) {  // -255 to 255
  digitalWrite(IN1, speed > 0);
  digitalWrite(IN2, speed < 0);
  motor.writeScaled(abs(speed) / 255.0);
}

void setup() {
  Serial.begin(115200);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  motor.attachPin(ENA, 1000, 8);
  steering.setPeriodHertz(50);
  steering.attach(STEER_PIN, 1000, 2000);
  steering.writeMicroseconds(1500);

  // MAC address of whatever the controller was last paired with
  ps5.begin("00:00:00:00:00:00");
  Serial.println("waiting for controller");
}

void loop() {
  // controller off or out of range: stop and straighten the wheels
  if (!ps5.isConnected()) {
    drive(0);
    steering.writeMicroseconds(1500);
    delay(50);
    return;
  }

  int x = ps5.LStickX();  // -128 to 127
  if (abs(x) < 8) x = 0;
  steering.writeMicroseconds(1500 + x * 400 / 128);

  drive(ps5.R2Value() - ps5.L2Value());

  delay(10);
}