PID 算法

最新推荐文章于 2025-06-18 15:05:13 发布

Q LI XING Q

最新推荐文章于 2025-06-18 15:05:13 发布

阅读量218

点赞数 3

CC 4.0 BY-SA版权

文章标签：算法

本文链接：https://blog.csdn.net/2301_81083929/article/details/148308297

void pid_cal(pid_t *pid)
{
// 计算当前偏差
pid->error[0] = pid->target - pid->now;

   // 计算输出
   if(pid->pid_mode == DELTA_PID) // 增量式
   {
       pid->pout = pid->p * (pid->error[0] - pid->error[1]);
       pid->iout = pid->i * pid->error[0];
       pid->dout = pid->d * (pid->error[0] - 2 * pid->error[1] + pid->error[2]);
       pid->out += pid->pout + pid->iout + pid->dout;
   }
   else if(pid->pid_mode == POSITION_PID) // 位置式
   {
       pid->pout = pid->p * pid->error[0];
       pid->iout += pid->i * pid->error[0];
       pid->dout = pid->d * (pid->error[0] - pid->error[1]);
       pid->out = pid->pout + pid->iout + pid->dout;
   }

   // 记录前两次偏差
   pid->error[2] = pid->error[1];
   pid->error[1] = pid->error[0];
}

void pidout_limit(pid_t *pid)
{
   // 输出限幅
   if(pid->out>=MAX_DUTY)
       pid->out=MAX_DUTY;
   if(pid->out<=0)
       pid->out=0;
}