float PID_Left1(int16_t speed1,int16_t tar1) { static float err = 0, err_last = 0, err_last_last = 0, add = 0; speed1 = absolute_value_speed(speed1); tar1 = absolute_value_speed(tar1); err = tar1-speed1; add = P * (err - err_last) + I * err + D * (err + err_last_last - 2 * err_last); out_left1 += add; out_left1 = pwm_control(out_left1); err_last_last = err_last; err_last = err; return out_left1; }
时间: 2024-04-18 11:31:31 浏览: 149
这段代码实现了一个 PID 控制器的左侧计算。函数名为 PID_Left1,接收两个参数 speed1 和 tar1,分别表示当前速度和目标速度。其中,P、I、D 是三个控制参数,err、err_last、err_last_last 和 add 是静态变量。
代码首先对 speed1 和 tar1 取绝对值,然后计算误差 err = tar1 - speed1。接着,根据 PID 控制器的公式计算增量 add = P * (err - err_last) + I * err + D * (err + err_last_last - 2 * err_last)。
增量 add 被累加到 out_left1 变量上,并通过 pwm_control 函数进行限幅处理。最后,更新 err_last_last、err_last 的值为当前的 err 和 err_last,并返回 out_left1。
需要注意的是,这段代码中的变量 out_left1 和 P、I、D 的定义没有提供,可能是在其他地方定义的全局变量。另外,pwm_control 函数的具体实现也没有在这段代码中给出。
阅读全文