#include <Arduino.h>
#include <ESP32Servo.h>

// Servo bên trái
Servo leftServo;

// Servo bên phải
Servo rightServo;

// Góc lệch dùng để mô phỏng tốc độ
int speedValue = 45;

// =========================
// Robot đi tới
// =========================

void forward() {

  // Hai servo nghiêng ngược chiều
  // để mô phỏng robot chạy tới

  leftServo.write(135);
  rightServo.write(45);

  Serial.println("Robot moving forward");
}


// =========================
// Robot đi lùi
// =========================

void backward() {

  leftServo.write(45);
  rightServo.write(135);

  Serial.println("Robot moving backward");
}


// =========================
// Robot rẽ trái
// =========================

void turnLeft() {

  leftServo.write(45);
  rightServo.write(45);

  Serial.println("Robot turning left");
}


// =========================
// Robot rẽ phải
// =========================

void turnRight() {

  leftServo.write(135);
  rightServo.write(135);

  Serial.println("Robot turning right");
}


// =========================
// Robot dừng
// =========================

void stopRobot() {

  leftServo.write(90);
  rightServo.write(90);

  Serial.println("Robot stopped");
}

void setup() {

  Serial.begin(115200);

  leftServo.attach(18);
  rightServo.attach(19);

  stopRobot();

  Serial.println("===== ESP32 ROBOT CONTROL =====");
  Serial.println("F = Forward");
  Serial.println("B = Backward");
  Serial.println("L = Left");
  Serial.println("R = Right");
  Serial.println("S = Stop");
}

void loop() {

  if (Serial.available()) {

    char cmd = Serial.read();

    if (cmd == 'F' || cmd == 'f') {

      forward();
    }

    else if (cmd == 'B' || cmd == 'b') {

      backward();
    }

    else if (cmd == 'L' || cmd == 'l') {

      turnLeft();
    }

    else if (cmd == 'R' || cmd == 'r') {

      turnRight();
    }

    else if (cmd == 'S' || cmd == 's') {

      stopRobot();
    }
  }
}