# coding:utf-8
#JSRUN引擎2.0，支持多达30种语言在线运行，全仿真在线交互输入输出。 
print("Hello world!   -  python.jsrun.net .")
class PIDController:
    def __init__(self, Kp, Ki, Kd, dt):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.dt = dt
        self.integral = 0
        self.previous_error = 0

    def compute(self, error):
        self.integral += error * self.dt
        derivative = (error - self.previous_error) / self.dt
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.previous_error = error
        return output

# 初始化参数
target_yaw = 10.0  # 目标偏航角（度）
current_yaw = 2.0   # 当前偏航角（度）
dt = 0.1            # 控制周期（秒）
max_voltage = 15.0  # 最大允许电压（伏）
min_voltage = -15.0 # 最小允许电压（伏）

# PID参数（需根据系统调整）
Kp = 2.5
Ki = 0.5
Kd = 1.2

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

# 控制循环
for _ in range(100):
    error = target_yaw - current_yaw
    if abs(error) <= 2.0:  # 满足误差要求
        break
    
    # 计算控制电压
    u = pid.compute(error)
    
    # 电压限幅
    u = max(min(u, max_voltage), min_voltage)
    
    # 根据传递函数更新偏航角（dα/dt = 0.3*u）
    delta_yaw = 0.3 * u * dt
    current_yaw += delta_yaw
    
    print(f"误差: {error:.2f}度, 电压: {u:.2f}V, 当前偏航角: {current_yaw:.2f}度")

print(f"\n最终偏航角: {current_yaw:.2f}度，控制误差: {target_yaw - current_yaw:.2f}度")