/////////////////////////////////完美版本/////////////////////////////////////
////////////////////////多舵机同时转动，可以分别设置角度、转动速度、暂停时间//////////////////////////


#include <Servo.h>  
  
Servo servo1;  // 创建第一个舵机对象  
Servo servo2;  // 创建第二个舵机对象  
  
const int servo1Pin = 5;  // 舵机1连接的引脚  
const int servo2Pin = 6; // 舵机2连接的引脚  
const int servo1Pin = 9;  // 舵机3连接的引脚  
const int servo2Pin = 10; // 舵机4连接的引脚  
const int servo1Pin = 11;  // 舵机5连接的引脚  

//舵机1的角度值  
const int servo1Start = 0;  
const int servo1End1 = 90;  
const int servo1End2 = 10;  
const int servo1End3 = 180;  
//舵机2的角度值
const int servo2Start = 180;  
const int servo2End1 = 70;  
const int servo2End2 = 160;  
const int servo2End3 = 0;  
  
const int duration = 150; // 每个动作阶段的持续时间（毫秒）  ，就是舵机转动的速度
const int pauseDuration1 = 10; // 第一次和第二次停留时间（毫秒）  
const int PauseDuration2 = 3000; // 最后一次停留时间（毫秒）  
  
unsigned long startTime; // 开始时间  
  
void setup() {  
  servo1.attach(servo1Pin);  // 将舵机1连接到对应的引脚  
  servo2.attach(servo2Pin);  // 将舵机2连接到对应的引脚  
}  
  
void loop() {  
  // 舵机1从0度到90度，舵机2从180度到70度  
  moveTo(servo1Start, servo1End1, servo2Start, servo2End1, duration);  
  pause(pauseDuration1); // 停留2秒  
  
  // 舵机1从90度到10度，舵机2从70度到160度  
  moveTo(servo1End1, servo1End2, servo2End1, servo2End2, duration);  
  pause(PauseDuration2); // 停留3秒  
  
  // 舵机1从10度到180度，舵机2从160度到0度  
  moveTo(servo1End2, servo1End3, servo2End2, servo2End3, duration);  
  pause(duration); // 停留2秒（或者根据需要调整）  
  
  // 停止舵机  
 exit(0);
}  
  
void moveTo(int servo1Start, int servo1End, int servo2Start, int servo2End, int duration) {  
  startTime = millis(); // 获取当前时间作为起始时间  
  for (int i = 0; i <= duration; i++) {  
    int servo1Pos = map(i, 0, duration, servo1Start, servo1End);  
    int servo2Pos = map(i, 0, duration, servo2Start, servo2End);  
    servo1.write(servo1Pos);  
    servo2.write(servo2Pos);  
    delay(10); // 延迟以减少CPU使用率  
  }  
}  
  
void pause(int duration) {  
  unsigned long pauseStartTime = millis();  
  while (millis() - pauseStartTime < duration) {  
    // 等待直到达到指定的暂停时间  
  }  
}