编辑代码

# coding:utf-8
#JSRUN引擎2.0，支持多达30种语言在线运行，全仿真在线交互输入输出。 
import time

class PIDController:
    def __init__(self, Kp, Ki, Kd, setpoint):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.setpoint = setpoint
        self.integral = 0
        self.previous_error = 0
        self.previous_time = time.time()
    
    def compute(self, current_value):
        current_time = time.time()
        dt = current_time - self.previous_time
        if dt <= 0:
            dt = 0.01  # 避免除以零或负的时间差
        
        error = self.setpoint - current_value
        self.integral += error * dt
        derivative = (error - self.previous_error) / dt if dt > 0 else 0
        
        # 计算控制量
        u = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        
        # 更新状态
        self.previous_error = error
        self.previous_time = current_time
        
        return u

# 初始化参数
setpoint = 30.0  # 目标偏航角30度
current_angle = 0.0  # 当前偏航角0度
current_velocity = 0.0  # 当前角速度

# PID参数，需要调整
Kp = 1.2
Ki = 0.1
Kd = 0.5

pid = PIDController(Kp, Ki, Kd, setpoint)

dt = 0.1  # 控制周期，秒
max_time = 30.0  # 最大仿真时间，防止无限循环
start_time = time.time()

while True:
    # 计算控制量
    u = pid.compute(current_angle)
    
    # 应用控制量到系统模型（假设a=0.5*u）
    a = 0.5 * u
    
    # 更新角速度和角度
    current_velocity += a * dt
    current_angle += current_velocity * dt
    
    # 检测是否达到目标
    error = setpoint - current_angle
    if abs(error) <= 2.0:
        print("目标达到，当前偏航角: {:.2f}度，误差: {:.2f}度".format(current_angle, error))
        break
    
    # 防止超时
    if time.time() - start_time > max_time:
        print("超时未收敛")
        break
    
    # 模拟实时控制，等待dt时间
    time.sleep(dt)

print("最终偏航角: {:.2f}度".format(current_angle))