#include <ESP32Servo.h>

#define POWER 12
#define SPEED1 15
#define SPEED2 5
#define SPEED3 3
#define TURN 14
#define LED 22

hw_timer_t *My_timer = NULL;
bool isOn = false, isTurn = false, isCW = false;

const int servoPin = 18;
Servo servo;
int pos = 0;

void IRAM_ATTR onTimer(){

 if(isOn && isTurn){
   if(pos = -1){
     pos = 0;
     isCW = true;
   } else if(pos == 181) {
     pos = 180;
     isCW = false;
   }

   if(isCW){
    servo.write(pos);
    pos++;
   } else {
    servo.write(pos);
    pos--;
   }
 }

}

void checkButton(){
  if(digitalRead(POWER) == LOW){
    Serial.println("POWER Off");
    isOn = false;
    ledcWrite(0, 0);
    while(digitalRead(POWER) == LOW){}

  } else if(digitalRead(SPEED1) == LOW){
    Serial.println("SPEED1");
    isOn = true;
    ledcWrite(0, 0.30*255);
    while(digitalRead(SPEED1) == LOW){}

  } else if(digitalRead(SPEED2) == LOW){
    Serial.println("SPEED2");
    isOn = true;
    ledcWrite(0, 0.60*255);
    while(digitalRead(SPEED2) == LOW){}

  } else if(digitalRead(SPEED3) == LOW){
    Serial.println("SPEED3");
    isOn = true;
    ledcWrite(0, 255);
    while(digitalRead(SPEED3) == LOW){}

  } else if(digitalRead(TURN) == LOW){
    isTurn = !isTurn;
    Serial.print("Turning " + isTurn);
    if(isTurn)  Serial.println("On");
    else      Serial.println("Off");
    while(digitalRead(TURN) == LOW){}
  } 
}

void setup() {
   Serial.begin(9600);
 pinMode(POWER, INPUT_PULLUP);
 pinMode(SPEED1, INPUT_PULLUP);
 pinMode(SPEED2, INPUT_PULLUP);
 pinMode(SPEED3, INPUT_PULLUP);
 pinMode(TURN, INPUT_PULLUP);
 pinMode(LED, OUTPUT);
 ledcSetup(0, 1000, 8);
 ledcAttachPin(LED, 0);
 servo.attach(servoPin, 500, 2400);

  My_timer = timerBegin(0, 80, true); 
  timerAttachInterrupt(My_timer, &onTimer, true);
  timerAlarmWrite(My_timer, 15000, true);  
  timerAlarmEnable(My_timer); 
}

void loop() {
  checkButton();
  delay(50);
}