// 伺服馬達校正
#include<ESP32Servo.h>
#include <BluetoothSerial.h>

BluetoothSerial BT;

#define upservoPin 2
#define downservoPin 4
#define leftservoPin 19
#define rightservoPin 21
Servo upServo;
Servo downServo;
Servo leftServo;
Servo rightServo;
int u_pos = 90;
int d_pos = 90;
int l_pos = 90;
int r_pos = 90;                           // 定義角度位置
int step = 5;
void setup() {  
  Serial.begin(115200);
  upServo.attach(upservoPin);            // 設置伺服馬達的控制腳位
  upServo.write(u_pos);                   // 初始化為90度置中
  downServo.attach(downservoPin);
  downServo.write(d_pos);                 // 初始化為90度置中
  leftServo.attach(leftservoPin);
  leftServo.write(l_pos);                 // 初始化為90度置中
  rightServo.attach(rightservoPin);
  rightServo.write(r_pos);                // 初始化為90度置中
}
void loop() {
  if (Serial.available()) {    
    char Serial_data = Serial.read();
    controlServo(Serial_data);
  }  
  }

void controlServo(char data){
  if(data=='1'){
  u_pos += step;
  upServo.write(u_pos);
  }else if(data=='2'){
  u_pos -= step;
  upServo.write(u_pos);
  }else if(data=='3'){
  d_pos += step;
  downServo.write(d_pos);
  }else if(data=='4'){
  d_pos -= step;
  downServo.write(d_pos);
  }else if(data=='5'){
  l_pos += step;
  leftServo.write(l_pos);
  }else if(data=='6'){
  l_pos -= step;
  leftServo.write(l_pos);
  }else if(data=='7'){
  r_pos += step;
  rightServo.write(r_pos);
  }else if(data=='8'){
  r_pos -= step;
  rightServo.write(r_pos);
  }
}