编辑代码

target_yaw = 30  
error_threshold = 2  
current_yaw = 0  
Kp = 0.5  
Ki = 0.1  
Kd = 0.2  
integral = 0  
previous_error = 0  
dt = 0.1  
while True:
    error = target_yaw - current_yaw
    if abs(error) <= error_threshold:
        print("达到目标偏航角，控制完成！")
        break
    P = Kp * error
    integral += error * dt
    I = Ki * integral
    derivative = (error - previous_error) / dt
    D = Kd * derivative
    pid_output = P + I + D
    control_voltage = pid_output
    current_yaw += 0.5 * control_voltage * control_voltage * dt
    previous_error = error