#include <Servo.h>
 
Servo myservo;  // 创建Servo对象用以控制伺服电机
int servopos = 90;  // 初始位置为 90 度
int DSD = 15; // 舵机转向延时时间
void setup() {
  myservo.attach(11);  // 连接到数字引脚11
  myservo.write(servopos);  // 将舵机初始化到 90 度位置
  Serial.begin(9600);
}

void loop() {
  delay(500);  // 延时 500 毫秒
  Serial.print("servopos = ");
  servopos = myservo.read();  // 读取当前舵机位置
  Serial.print(servopos);
  Serial.println(" degrees");
  
  if(Serial.available() > 0){
    char serialcmd = Serial.read();  // 读取串口命令
    if (serialcmd == 's'){  // 假设使用 's' 命令来设置角度
      int topos = Serial.parseInt();  // 解析接收到的整数值
      if (topos >= 0 && topos <= 180){  // 确保角度值在有效范围内
        servocmd(topos);
      } else {
        Serial.println("Invalid angle, must be between 0 and 180.");
      }
    }
  }
}

void servocmd(int topos) {
  Serial.print("Moving servo to ");
  Serial.println(topos);
  int frompos = servopos;  // 当前舵机位置
  
  if(topos <= frompos){
    for(int i = frompos; i >= topos; i--){
      myservo.write(i);  // 设置舵机位置
      delay(DSD);  // 每步延时，控制移动速度
    }
  } else {
    for(int i = frompos; i <= topos; i++){
      myservo.write(i);
      delay(DSD);
    }
  }
  
  servopos = topos;  // 更新当前舵机位置
  Serial.print("Set servo to ");
  Serial.println(topos);
}