// --------------------------------------------------------------------------------
/*

21 同時改變多個 Servo Motor 角度 (不使用 Library)

[學習重點]

1. 直接輸出 PWM 訊號控制 Servo 角度

[挑戰]

- 嘗試增加 Servo Motor 的數量吧！


Created by Jason on 21 Aug 2022.

*/
// --------------------------------------------------------------------------------

int numberOfServos = 5; // Servo 數量
int servoPin[] = {11, 10, 9, 6, 5, 3}; // 各 Servo 連接的 Pin

void setup() {
  // 設定各 Servo Pin 為 Output Mode
  for(int s = 0; s < 6; s++){
    pinMode(servoPin[s], OUTPUT);
  }
}

void loop() {
  servoPos(2, 2, 2, 2, 2, 2);

  delay(1000);

  servoPos(178, 178, 178, 178, 178, 178);

  delay(1000);
}

void servoPos(int a1, int a2, int a3, int a4, int a5, int a6) {
  servoPulse(servoPin[0], a1);
  servoPulse(servoPin[1], a2);
  servoPulse(servoPin[2], a3);
  servoPulse(servoPin[3], a4);
  servoPulse(servoPin[4], a5);
  servoPulse(servoPin[5], a6);
  delay(400);
}

void servoPulse (int servo, int angle) {
  int pwm = (angle*11) + 500; // Convert angle to microseconds
  digitalWrite(servo, HIGH);
  delayMicroseconds(pwm);
  digitalWrite(servo, LOW);
}