#include <ESP32Servo.h>

const int servol_pin = 12;
const int servoR_pin = 14;
const int btn_pin = 27;
const int led_pin = 26;
const int poten1_pin = 25; //โพเทนชิออมิเตอร์แบบหมุน - ควบคุมความเร็ว
const int poten2_pin = 33; //โพเทนชิออมิเตอร์แบบสไลด์ - ควบคุมทิศทาง

Servo servoL;
Servo servoR;

bool isSystemOn = false;
bool lastButtonState = HIGH
int currentSpeed = 0;
int angeL = 90;
int angeR = 90;

void setup() {
  Serial.begin(115200);

  pinMode(led_pin, OUTPUT);
  pinMode(btn_pin, INPUT_PULLUP);

  servoL.setPeriodHertz(50); 
  servoR.setPeriodHertz(50);

  servoL.attach(servoL_pin);
  servoR.attach(servoR_pin);
}

void loop() {
  //อ่านค่าปุ่ม
  bool currentButtonState = digitalRead(btn_pin);
  if (currentButtonState = LOW && lastButtonState = HIGH) {
    delay(50);
    if (digitalRead(btn_pin) = LOW)  {
      isSystemOn = !isSystemOn;
      digitalWrite(led_pin,  isSystem);

      // เมื่อกดปุ่มเพื่อปิดระบบ ไห้เริ่มลดความเร็ว
      if (!isSystemOn) {
        //ลดความเร็วลงจนเป็น 0
        while (currentSpeed > 0) {
          currentSpeed--;
          servoL.write(angleL);
          servoR.write(angleR);

          Serial.print("Speed: ");
          Serial.print(currentSpeed);
          Serial.println(" km/hr.");
          delay(10);
        }
        digitalWrite(led_pin, LOW);
      }
    }
  }


 if (isSystemOn) { 
  //อ่านค่าโพเทนชิออมิเตอร์
  int poten1_Value = analogRead(poten1_pin); //หมุนสำหรับความเร็ว 
  int poten2_Value = analogRead(poten2_pin); //หมุนสำหรับความเร็ว
  
  //ควบคุมความเร็ว
  currentSpeed = map(poten_Value, 0, 4095, 0, 200);

  //ควบคุมทิศทาง (เลี้ยว)
  //***แก้ไข: ไห้ Servo ทั้งสองหมุนไปทิศทางเดียวกัน ***
  int angle_offset = map(poten2_Value, 0, 4095, -50, 50);
  int center_angle = 90;
  angleL = center_angle + angle_offset;
  angleR = center_angle + angle_offset;

  servoL.write(angleL);
  servoR.write(angleR);

  // แสดงผลความเร็ว
  Serial.print("Speed: ");
  Serial.print(currentSpeed);
  Serial.println(" km/hr. ");

 } else {
   // เมื่อระบบปิด ไห้ Servo อยู่ในตำแหน่งศูนย์กลาง (ไม่หมุ่น)
   servoL.write(90);
   servoR.write(90);
   currentSpeed = 0; // ตั้งค่าความเร็วเป็น 0 เมื่อระบบปิด
 }

 delay(10);
}