// // 控制機械手臂(序列埠)
// #include <ESP32Servo.h>       // 使用伺服馬達函式庫
// // 定義伺服馬達接線
// #define upServoPin 27         // 爪子
// #define downServoPin 14       // 底部
// #define leftServoPin 12       // 左側
// #define rightServoPin 13      // 右側

// // 宣告伺服馬達物件
// Servo upServo;                // 爪子
// Servo downServo;              // 底部
// Servo leftServo;              // 左側
// Servo rightServo;             // 右側

// // 宣告馬達角度位置變數
// int upServoVal = 90;          // 爪子(60~120)
// int downServoVal = 90;        // 底部(0~180)
// int leftServoVal = 90;        // 左側(70~150)
// int rightServoVal = 90;       // 右側(60~180)

// // 馬達每次轉動步伐
// int step = 3;

// void setup() {  
//   Serial.begin(115200);
//   // 設置伺服馬達的控制腳位
//   upServo.attach(upServoPin); 
//   downServo.attach(downServoPin);
//   leftServo.attach(leftServoPin);
//   rightServo.attach(rightServoPin);

//   // 馬達初始化置中
//   upServo.write(upServoVal);
//   downServo.write(downServoVal);
//   leftServo.write(leftServoVal);
//   rightServo.write(rightServoVal);                
// }


// void loop() {
//   // 如果收到序列埠資料
//   if (Serial.available()) {    
//     // 讀取訊號
//     char Serial_data = Serial.read();
//     // 呼叫自訂函式控制機械手臂
//     controlServo(Serial_data);
//   }  
// }

// // 自訂函式控制機械手臂
// void controlServo(char data){
//   if(data == '1'){    
//     upServoVal += step;
//     if(upServoVal <=180){
//       upServo.write(upServoVal);
//     }else{
//       upServoVal -= step;
//     }
//     Serial.printf("up : %d \n", upServoVal);    
//   }else if(data == '2'){    
//     upServoVal -= step;
//     if(upServoVal >=0){
//       upServo.write(upServoVal);
//     }
//     Serial.printf("up : %d \n", upServoVal);    
//   }else if(data == '3'){    
//     downServoVal += step;
//     downServo.write(downServoVal);
//     Serial.printf("down : %d \n", downServoVal);    
//   }else if(data == '4'){    
//     downServoVal -= step;
//     downServo.write(downServoVal);
//     Serial.printf("down : %d \n", downServoVal);    
//   }else if(data == '5'){    
//     leftServoVal += step;
//     leftServo.write(leftServoVal);
//     Serial.printf("left : %d \n", leftServoVal);    
//   }else if(data == '6'){    
//     leftServoVal -= step;
//     leftServo.write(leftServoVal);   
//   }else if(data == '7'){    
//     rightServoVal += step;
//     rightServo.write(rightServoVal);
//     Serial.printf("right : %d \n", rightServoVal);    
//   }else if(data == '8'){    
//     rightServoVal -= step;
//     rightServo.write(rightServoVal);
//     Serial.printf("right : %d \n", rightServoVal);     
//   }else if(data == '\n'){
//     Serial.println("OK");
//   }else{
//     Serial.println("error");     
//   }
// }





// // 控制機械手臂(序列埠)
// #include <ESP32Servo.h>       // 使用伺服馬達函式庫
// // 定義伺服馬達接線
// #define upServoPin 27         // 爪子
// #define downServoPin 14       // 底部
// #define leftServoPin 12       // 左側
// #define rightServoPin 13      // 右側

// // 宣告伺服馬達物件
// Servo upServo;                // 爪子
// Servo downServo;              // 底部
// Servo leftServo;              // 左側
// Servo rightServo;             // 右側

// // 宣告馬達角度位置變數
// int upServoVal = 90;          // 爪子(60~120)
// int downServoVal = 90;        // 底部(0~180)
// int leftServoVal = 90;        // 左側(70~150)
// int rightServoVal = 90;       // 右側(60~180)

// // 馬達每次轉動步伐
// int step = 3;

// // 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){  
//   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);
//   }
// }




// void setup() {  
//   Serial.begin(115200);
//   // 設置伺服馬達的控制腳位
//   upServo.attach(upServoPin); 
//   downServo.attach(downServoPin);
//   leftServo.attach(leftServoPin);
//   rightServo.attach(rightServoPin);

//   // 馬達初始化置中
//   servoPulse(upServoPin, upServoVal);
//   //upServo.write(upServoVal);
//   downServo.write(downServoVal);
//   leftServo.write(leftServoVal);
//   rightServo.write(rightServoVal);                
// }


// void loop() {
//   // 如果收到序列埠資料
//   if (Serial.available()) {    
//     // 讀取訊號
//     char Serial_data = Serial.read();
//     // 呼叫自訂函式控制機械手臂
//     controlServo(Serial_data);
//   }  
// }

// // 自訂函式控制機械手臂
// void controlServo(char data){
//   if(data == '1'){    
//     upServoVal += step;
//     if(upServoVal <=180){
//       upServo.write(upServoVal);
//     }else{
//       upServoVal -= step;
//     }
//     Serial.printf("up : %d \n", upServoVal);    
//   }else if(data == '2'){    
//     upServoVal -= step;
//     if(upServoVal >=0){
//       upServo.write(upServoVal);
//     }
//     Serial.printf("up : %d \n", upServoVal);    
//   }else if(data == '3'){    
//     downServoVal += step;
//     downServo.write(downServoVal);
//     Serial.printf("down : %d \n", downServoVal);    
//   }else if(data == '4'){    
//     downServoVal -= step;
//     downServo.write(downServoVal);
//     Serial.printf("down : %d \n", downServoVal);    
//   }else if(data == '5'){    
//     leftServoVal += step;
//     leftServo.write(leftServoVal);
//     Serial.printf("left : %d \n", leftServoVal);    
//   }else if(data == '6'){    
//     leftServoVal -= step;
//     leftServo.write(leftServoVal);   
//   }else if(data == '7'){    
//     rightServoVal += step;
//     rightServo.write(rightServoVal);
//     Serial.printf("right : %d \n", rightServoVal);    
//   }else if(data == '8'){    
//     rightServoVal -= step;
//     rightServo.write(rightServoVal);
//     Serial.printf("right : %d \n", rightServoVal);     
//   }else if(data == '\n'){
//     Serial.println("OK");
//   }else{
//     Serial.println("error");     
//   }
// }



// 控制機械手臂(序列埠)
#include <ESP32Servo.h>       // 使用伺服馬達函式庫
#include <BluetoothSerial.h>

// 定義伺服馬達接線
#define upServoPin 27         // 爪子
#define downServoPin 14       // 底部
#define leftServoPin 12       // 左側
#define rightServoPin 13      // 右側

// 宣告伺服馬達物件
Servo upServo;                // 爪子
Servo downServo;              // 底部
Servo leftServo;              // 左側
Servo rightServo;             // 右側
// 宣告藍牙物件
BluetoothSerial BT;


// 宣告馬達角度位置變數
int upServoVal = 90;          // 爪子(60~120)
int downServoVal = 90;        // 底部(0~180)
int leftServoVal = 90;        // 左側(70~150)
int rightServoVal = 90;       // 右側(60~180)

// 馬達每次轉動步伐
int step = 3;

// 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){  
  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);
  }
}




void setup() {  
  // Serial.begin(115200);
  BT.begin("ESP32_12201");
  // 設置伺服馬達的控制腳位
  upServo.attach(upServoPin); 
  downServo.attach(downServoPin);
  leftServo.attach(leftServoPin);
  rightServo.attach(rightServoPin);

  // 馬達初始化置中
  servoPulse(upServoPin, upServoVal);
  //upServo.write(upServoVal);
  downServo.write(downServoVal);
  leftServo.write(leftServoVal);
  rightServo.write(rightServoVal);                
}


void loop() {
  // 如果收到序列埠資料
  if (BT.available()) {    
    // 讀取訊號
    char BT_data = BT.read();
    // 呼叫自訂函式控制機械手臂
    controlServo(BT_data);
  }  
}

// 自訂函式控制機械手臂
void controlServo(char data){
  if(data == '1'){    
    upServoVal += step;
    if(upServoVal <=180){
      upServo.write(upServoVal);
    }else{
      upServoVal -= step;
    }
    //Serial.printf("up : %d \n", upServoVal);    
  }else if(data == '2'){    
    upServoVal -= step;
    if(upServoVal >=0){
      upServo.write(upServoVal);
    }
    Serial.printf("up : %d \n", upServoVal);    
  }else if(data == '3'){    
    downServoVal += step;
    downServo.write(downServoVal);
    Serial.printf("down : %d \n", downServoVal);    
  }else if(data == '4'){    
    downServoVal -= step;
    downServo.write(downServoVal);
    Serial.printf("down : %d \n", downServoVal);    
  }else if(data == '5'){    
    leftServoVal += step;
    leftServo.write(leftServoVal);
    Serial.printf("left : %d \n", leftServoVal);    
  }else if(data == '6'){    
    leftServoVal -= step;
    leftServo.write(leftServoVal);   
  }else if(data == '7'){    
    rightServoVal += step;
    rightServo.write(rightServoVal);
    Serial.printf("right : %d \n", rightServoVal);    
  }else if(data == '8'){    
    rightServoVal -= step;
    rightServo.write(rightServoVal);
    Serial.printf("right : %d \n", rightServoVal);     
  }else if(data == '\n'){
    Serial.println("OK");
  }else{
    Serial.println("error");     
  }
}