#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);
  BT.begin("ESP32_acer");
  pinMode(upservoPin, OUTPUT);
  servoPulse(upservoPin, u_pos); 
  pinMode(downservoPin, OUTPUT);
  servoPulse(downservoPin, d_pos); 
  pinMode(leftservoPin, OUTPUT);
  servoPulse(leftservoPin, l_pos); 
  pinMode(rightservoPin, OUTPUT);
  servoPulse(rightservoPin, r_pos);  
}
void loop() {
  if (BT.available()) {    
    char BT_data = BT.read();
    controlServo(BT_data);
  }  
  if (Serial.available()) {    
    char Serial_data = Serial.read();
    controlServo(Serial_data);
  }  

}
// Pulse Cycle 20ms，使用50Hz(每秒重複50次)，週期為20ms的PWM訊號來控制馬達
// pulse width 0.5ms~2.4ms => 0 ~ 180
double pulseWidth_min = 500;      // 0.5毫秒 = 500微秒
double pulseWidth_max = 2400;
double anglePulse = (pulseWidth_max-pulseWidth_min)/180;     // 計算1度的脈寬值
void servoPulse(int ServoPin, int angle){  
  int pulseWidth = (angle*anglePulse) + pulseWidth_min;   // 將角度轉為脈寬值
  for(int i=0; i<10; i++){           // 50Hz(給伺服馬達足夠時間轉至指定角度即可)    
    digitalWrite(ServoPin, HIGH);    // 將servoPin拉至高電位持續pulseWidth微秒
    delayMicroseconds(pulseWidth);
    digitalWrite(ServoPin, LOW);     // 將servoPin拉至高電位持續pulseWidth微秒
    delayMicroseconds(20000- pulseWidth);
  }
}

void controlServo(char data){
  if(data=='1'){
  u_pos += step;
  servoPulse(upservoPin, u_pos);
  }else if(data=='2'){
  u_pos -= step;
  servoPulse(upservoPin, u_pos);
  }else if(data=='3'){
  d_pos += step;
  servoPulse(downservoPin, d_pos);
  }else if(data=='4'){
  d_pos -= step;
  servoPulse(downservoPin, d_pos);
  }else if(data=='5'){
  l_pos += step;
  servoPulse(leftservoPin, l_pos); 
  }else if(data=='6'){
  l_pos -= step;
  servoPulse(leftservoPin, l_pos); 
  }else if(data=='7'){
  r_pos += step;
  servoPulse(rightservoPin, r_pos); 
  }else if(data=='8'){
  r_pos -= step;
  servoPulse(rightservoPin, r_pos); 
  }
}