PID exercise: Increment PID (1), adjust the speed of the motor

1. The principle of incremental PID, the final formula
of incremental PID 2. The code of incremental PID;
3. stm32F4 realizes the adjustment of the motor speed.

1. Incremental PID

code

//定义PID结构体
typedef struct 
{
    
    
   __IO int      SetPoint;                                 //设定目标 Desired Value
   __IO float    Proportion;                               //比例常数 Proportional Const
   __IO float    Integral;                                 //积分常数 Integral Const
   __IO float    Derivative;                               //微分常数 Derivative Const
   __IO int      LastError;                                //Error[-1]
   __IO int      PrevError;                                //Error[-2]
}PID;

/* 私有宏定义 ----------------------------------------------------------------*/
/*************************************/
// 定义PID相关宏
// 这三个参数设定对电机运行影响非常大
// PID参数跟采样时间息息相关
/*************************************/
#define  P_DATA      0.4                              // P参数
#define  I_DATA      0.3                              // I参数
#define  D_DATA      0.1                              // D参数

static PID sPID;
static PID *sptr = &sPID;
/* 扩展变量 ------------------------------------------------------------------*/
/* 私有函数原形 --------------------------------------------------------------*/
void IncPIDInit(void);
int32_t IncPIDCalc(int16_t NextPoint);


 if(KEY1_StateRead()==KEY_DOWN)  // 增速
    {
    
    
      /* 设置目标速度 */
      sptr->SetPoint +=50;    //50 pulse
    }
    if(KEY2_StateRead()==KEY_DOWN)  // 减速
    {
    
    
      /* 设置目标速度 */
      sptr->SetPoint -=50;
    }    

  if(start_flag) // 等待脉冲输出后才开始计时
  {
    
    
    time_count++;         // 每1ms自动增一
    if(time_count==200)
    {
    
    
      __IO int16_t count;
      __IO int para;
      
      /* 得到编码器计数值，200ms内的编码器数值，数值越大说明速度越大 */
      count = (int16_t)__HAL_TIM_GET_COUNTER(&htimx_Encoder);

      /* 计数得到增量式PID的增量数值 */
      para=IncPIDCalc(count);
      
      /* 根据增量数值调整当前电机速度 */
      PWM_Duty +=para;  
      
      if(PWM_Duty>999)PWM_Duty=1000;  
      if(PWM_Duty<0)PWM_Duty=0;

      /* 对速度值进行简单的处理， */
      cal += count*(1000/time_count); // 单位转换成秒//并累加
      i++;
      if(i==(1000/time_count))
      {
    
    
        i=0;
        cal /= (1000/time_count);     // 求平均，1s内采样5次
        // 11：编码器线数(转速一圈输出脉冲数),编码器4倍频
        // 34：电机减数比，内部电机转动圈数与电机输出轴转动圈数比，即减速齿轮比      
        printf("Speed %.2f r/s\n",cal/(11*4)/34);
        cal = 0;
      }
      __HAL_TIM_SET_COUNTER(&htimx_Encoder,0);
      L298N_DCMOTOR_Contrl(1,PWM_Duty); 
      time_count=0;      
    }
  }

/*********************PID参数初始化********************************/
void IncPIDInit(void) 
{
    
    
    sptr->LastError=0;            //Error[-1]
    sptr->PrevError=0;            //Error[-2]
    sptr->Proportion=P_DATA;      //比例常数 Proportional Const
    sptr->Integral=I_DATA;        //积分常数  Integral Const
    sptr->Derivative=D_DATA;      //微分常数 Derivative Const
    sptr->SetPoint=300;           //设定目标Desired Value
}
/********************增量式PID控制设计************************************/
/** 
  * 函数名称：增量式PID控制设计
  * 输入参数：当前控制量
  * 返 回 值：目标控制量
  * 说    明：无
  */
int32_t IncPIDCalc(int16_t NextPoint) 
{
    
    
  int iError,iIncpid = 0;                             //当前误差
  iError=sptr->SetPoint-NextPoint;                    //增量计算
  if( iError<3 && iError>-3 )                         //误差在正负3以内，则不做处理
    return iIncpid;
  iIncpid=(sptr->Proportion * iError)                 //E[k]项
              -(sptr->Integral * sptr->LastError)     //E[k-1]项
              +(sptr->Derivative * sptr->PrevError);  //E[k-2]项
  
  sptr->PrevError=sptr->LastError;                    //存储误差，用于下次计算
  sptr->LastError=iError;
  return(iIncpid);                                    //返回增量值
}