/*
可由序列阜輸入，亦可由藍芽輸入(需更改註解的區塊)
*/
#include<ESP32Servo.h>
#include <BluetoothSerial.h>

#define ServoPin1 2
#define ServoPin2 4
#define ServoPin3 5
#define ServoPin4 18
//Declare
Servo Motor1;
Servo Motor2;
Servo Motor3;
Servo Motor4;
// Define Degree
int Pos1 = 90;
int Pos2 = 90;
int Pos3 = 90;
int Pos4 = 90;
BluetoothSerial BT;

// void setup()
// {
//   BT.begin("ESP32_Llywelyn");
//   //PinModeSetting
//   Motor1.attach(ServoPin1);
//   Motor2.attach(ServoPin2);
//   Motor3.attach(ServoPin3);
//   Motor4.attach(ServoPin4);
//   //Init Degree
//   Motor1.write(Pos1);
//   Motor2.write(Pos2);
//   Motor3.write(Pos3);
//   Motor4.write(Pos4);
// }
//
// void loop()
// {
//   if(BT.available())
//   {
//     char Input = BT.read;
//     ControlServo(Input);
//   }
// }

void setup()
{
  Serial.begin(115200);
  //PinModeSetting
  Motor1.attach(ServoPin1);
  Motor2.attach(ServoPin2);
  Motor3.attach(ServoPin3);
  Motor4.attach(ServoPin4);
  //Init Degree
  Motor1.write(Pos1);
  Motor2.write(Pos2);
  Motor3.write(Pos3);
  Motor4.write(Pos4);
}

void loop()
{
  if(Serial.available())
  {
    char Input = Serial.read;
    ControlServo(Input);
  }
}

int Step = 10;
void ControlServo(char Data)
{
  if(Data == '1')
  {
    Pos1 += Step;
    Motor1.write(Pos1);
  }
  else if(Data == '2')
  {
    Pos1 -= Step;
    Motor1.write(Pos1);
  }
  else if(Data == '3')
  {
    Pos2 += Step;
    Motor2.write(Pos2);
  }
  else if(Data == '4')
  {
    Pos2 -= Step;
    Motor2.write(Pos2);
  }
  else if(Data == '5')
  {
    Pos3 += Step;
    Motor3.write(Pos3);
  }
  else if(Data == '6')
  {
    Pos3 -= Step;
    Motor3.write(Pos3);
  }
  else if(Data == '7')
  {
    Pos4 += Step;
    Motor4.write(Pos4);
  }
  else if(Data == '8')
  {
    Pos4 -= Step;
    Motor4.write(Pos4);
  }
}