#include<ESP32Servo.h>
#define oneserPin 27
#define twoserPin 14
#define threeserPin 12
#define fourserPin 13

// #include<ESP32Servo.h>
// #define serPin 2
// Servo ser;
// int pos = 90;     

// void setup() {  
//   Serial.begin(115200);
//   ser.attach(serPin);             
//   ser.write(pos);                   
// }

// void loop() {
//   if (Serial.available()) {    
//     String Serial_data = Serial.readString();
//     pos = atoi(Serial_data.c_str());    
//     Serial.println(pos);              
//     ser.write(pos);		      
//   }  
// }

Servo oneser;
Servo twoser;
Servo threeser;
Servo fourser;

int oneserPos = 90;
int twoserPos = 90;
int threeserPos = 90;
int fourserPos = 90;

void setup() {  
  Serial.begin(115200);
  oneser.attach(oneserPin);             // 設置伺服馬達的控制腳位
  twoser.attach(twoserPin);
  threeser.attach(threeserPin);
  fourser.attach(fourserPin);
  oneser.write(oneserPos);                   // 初始化為90度置中
  twoser.write(twoserPos);  
  threeser.write(threeserPos);  
  fourser.write(fourserPos);  
  Serial.println("0:馬達1逆轉5度");
  Serial.println("1:馬達1正轉5度");
  Serial.println("2:馬達2逆轉5度");
  Serial.println("3:馬達2正轉5度");
  Serial.println("4:馬達3逆轉5度");
  Serial.println("5:馬達3正轉5度");
  Serial.println("6:馬達4逆轉5度");
  Serial.println("7:馬達4正轉5度");
  Serial.println("請輸入數字");
}


int step = 5;
void controlServo(char data)
{
  if(data == '0')
  {
    oneserPos -= step;
    oneser.write(oneserPos);
  }

  else if(data == '1')
  {
    oneserPos += step;
    oneser.write(oneserPos);
  }
  
  else if(data == '2')
  {
    twoserPos -= step;
    twoser.write(twoserPos);
  }

  else if(data == '3')
  {
    twoserPos += step;
    twoser.write(twoserPos);
  }
  
  else if(data == '4')
  {
    threeserPos -= step;
    threeser.write(threeserPos);
  }

  else if(data == '5')
  {
    threeserPos += step;
    threeser.write(threeserPos);
  }
  
  else if(data == '6')
  {
    fourserPos -= step;
    fourser.write(fourserPos);
  }

  else if(data == '7')
  {
    fourserPos += step;
    fourser.write(fourserPos);
  }
}

void loop() 
{
  if (Serial.available()) 
  {    
    char Serial_data = Serial.read(); 
    controlServo(Serial_data);
    delay(50);
    Serial.print("馬達1角度:");  //我不知道為什麼這裡會print兩次:(
    Serial.println(oneserPos-90);
    Serial.print("馬達2角度:");
    Serial.println(twoserPos-90);
    Serial.print("馬達3角度:");
    Serial.println(threeserPos-90);
    Serial.print("馬達4角度:");
    Serial.println(fourserPos-90);
    Serial.println("請輸入數字");
    delay(50);
  }
}