// 控制機械手臂(序列埠)
#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");     
  }
}