// 伺服馬達校正
#include<ESP32Servo.h>
#define upservoPin 2
#define downservoPin 4
#define leftservoPin 5
#define rightservoPin 18
int step =5;
int rightservoPinpos=90;
int upservoPinpos=90;
int leftservoPinpos=90;
int downservoPinpos=90;
Servo upmyServo;
Servo dowmmyServo;
Servo leftmyServo;
Servo rightmyServo;
int pos = 90;                           // 定義角度位置
void setup() {  
  Serial.begin(115200);
  upmyServo.attach(upservoPin);   
  dowmmyServo.attach(downservoPin);  
  leftmyServo.attach(leftservoPin);  
  rightmyServo.attach(rightservoPin);            // 設置伺服馬達的控制腳位       
  upmyServo.write(upservoPinpos);   
  dowmmyServo.write(downservoPinpos); 
  leftmyServo.write(leftservoPinpos); 
  rightmyServo.write(rightservoPinpos); // 初始化為90度置中
}

void loop() {
  if (Serial.available()) {    
    String Serial_data = Serial.readString();
    control (Serial_data);
  }  
}
void control (String data){
if(data=="1"){
  upservoPinpos += step;
  upmyServo.write(upservoPinpos);
}else if(data=="2"){
 upservoPinpos -= step;
  upmyServo.write(upservoPinpos);
}else if(data=="3"){
 downservoPinpos -= step;
  dowmmyServo.write(downservoPinpos);
}else if(data=="4"){
 downservoPinpos -= step;
  dowmmyServo.write(downservoPinpos);
}else if(data=="5"){
 leftservoPinpos -= step;
  leftmyServo.write(leftservoPinpos);
}else if(data=="6"){
 leftservoPinpos -= step;
  leftmyServo.write(leftservoPinpos);
}else if(data=="7"){
  rightservoPinpos -= step;
  rightmyServo.write(rightservoPinpos);
}else if(data=="8"){
 rightservoPinpos -= step;
 rightmyServo.write(rightservoPinpos);
}
}