#include<ESP32Servo.h>
#define servoPinDown 2
#define servoPinRight 4
#define servoPinLeft 5
#define servoPinUp 18
Servo servoDown;
Servo servoRight;
Servo servoLeft;
Servo servoUp;
int posDown = 90;                           // 定義角度位置
int posRight = 90; 
int posLeft = 90; 
int posUp = 90; 
void setup() {  
  Serial.begin(115200);
  servoDown.attach(servoPinDown);             // 設置伺服馬達的控制腳位           
  servoDown.write(posDown);                   // 初始化為90度置中
  servoRight.attach(servoPinRight);             // 設置伺服馬達的控制腳位           
  servoRight.write(posRight);                   // 初始化為90度置中
  servoLeft.attach(servoPinLeft);             // 設置伺服馬達的控制腳位           
  servoLeft.write(posLeft);                   // 初始化為90度置中
  servoUp.attach(servoPinUp);             // 設置伺服馬達的控制腳位           
  servoUp.write(posUp);                   // 初始化為90度置中
}
void loop() {
  if (Serial.available()) {
    char Serial_data = Serial.read();
    controlServo(Serial_data);
  }  
  delay(100);
}
int step = 5;
void controlServo(char data){
  if(data == '1'){
    posUp += step;
    servoUp.write(posUp);
    }
  else if(data =='2'){
    posDown += step;
    servoDown.write(posDown);
    }
  else if(data =='3'){
    posLeft += step;
    servoLeft.write(posLeft);
    }
  else if(data =='4'){
    posRight += step;
    servoRight.write(posRight);
    }
  else if(data == '5'){
    posUp -= step;
    servoUp.write(posUp);
    }
  else if(data =='6'){
    posDown -= step;
    servoDown.write(posDown);
    }
  else if(data =='7'){
    posLeft -= step;
    servoLeft.write(posLeft);
    }
  else if(data =='8'){
    posRight -= step;
    servoRight.write(posRight);
    }
}