写一个舵机pd位置式控制程序

以下是一个简单的舵机PD位置式控制程序示例，假设舵机连接在Arduino的Pin 9上，目标角度为90度：

#include <Servo.h>

Servo myservo;
int target_angle = 90;
float Kp = 1.0; // 比例系数
float Kd = 0.1; // 微分系数
int last_error = 0;

void setup() {
  myservo.attach(9); // 舵机连接到Pin 9
}

void loop() {
  int current_angle = myservo.read(); // 读取当前角度
  int error = target_angle - current_angle; // 计算角度偏差
  int derivative = error - last_error; // 计算微分项
  last_error = error; // 更新上一次偏差值

  int output = Kp * error + Kd * derivative; // 计算控制输出值

  myservo.write(current_angle + output); // 更新舵机角度
  delay(10); // 等待10毫秒
}

在上面的代码中，调整Kp和Kd的值可以控制舵机的响应速度和稳定性，具体的调试需要根据实际情况进行。