#include <BluetoothSerial.h>
#define ServoPin1 2
#define ServoPin2 4
#define ServoPin3 5
#define ServoPin4 18
int Pos1 = 90;
int Pos2 = 90;
int Pos3 = 90;
int Pos4 = 90;

BluetoothSerial BT;

void ctrlServo(char);
void servoPulse(int,int);

void setup()
{  
  Serial.begin(115200);
  //BT.begin("ESP32_llywelyn");
  pinMode(ServoPin1, OUTPUT);
  pinMode(ServoPin2, OUTPUT);
  pinMode(ServoPin3, OUTPUT);
  pinMode(ServoPin4, OUTPUT);
  servoPulse(ServoPin1, Pos1);
  servoPulse(ServoPin2, Pos2);
  servoPulse(ServoPin3, Pos3);
  servoPulse(ServoPin4, Pos4);
  Serial.println("Start");
}

void loop()
{
  if (Serial.available())
  {    
    char Serial_data = Serial.read();
    Serial.println(Serial_data);
    ctrlServo(Serial_data);
  } 
//   if(BT.available())
//   {
//     char BT_data = BT.read();
//     ctrlServo(BT_data);
//   }
}

int Step = 5;
void ctrlServo(char Data)
{
  if(Data == '1')
  {
    Pos1 += Step;
    servoPulse(ServoPin1, Pos1);
  }
  else if(Data == '2')
  {
    Pos1 -= Step;
    servoPulse(ServoPin1, Pos1);
  }
  else if(Data == '3')
  {
    Pos2 += Step;
    servoPulse(ServoPin2, Pos2);
  }
  else if(Data == '4')
  {
    Pos2 -= Step;
    servoPulse(ServoPin2, Pos2);
  }
  else if(Data == '5')
  {
    Pos3 += Step;
    servoPulse(ServoPin3, Pos3);
  }
  else if(Data == '6')
  {
    Pos3 -= Step;
    servoPulse(ServoPin3, Pos3);
  }
  else if(Data == '7')
  {
    Pos4 += Step;
    servoPulse(ServoPin4, Pos4);
  }
  else if(Data == '8')
  {
    Pos4 -= Step;
    servoPulse(ServoPin4, Pos4);
  }
}

// Pulse Cycle 20ms，使用50Hz(每秒重複50次)，週期為20ms的PWM訊號來控制馬達
// pulse width 0.5ms~2.4ms => 0 ~ 180
double pulseWidth_min = 500;
double pulseWidth_max = 2400;
double anglePulse = (pulseWidth_max-pulseWidth_min)/180;

void servoPulse(int ServoPin, int angle)
{  
  unsigned int pulseWidth = (angle*anglePulse) + pulseWidth_min; //將角度轉為脈寬值
  for(int i=0; i<5; i++)             // 50Hz(給伺服馬達足夠時間轉至指定角度即可) 
  {              
    digitalWrite(ServoPin, HIGH);    // 將servoPin拉至高電位持續pulseWidth微秒
    delayMicroseconds(pulseWidth);
    digitalWrite(ServoPin, LOW);     // 將servoPin拉至高電位持續pulseWidth微秒
    delayMicroseconds(20000 - pulseWidth);
  }
}