slamugv使用说明--6.电机pid调试下

4. 关键代码详解

/*************************************/
// 定义PID相关宏
// 这三个参数设定对电机运行影响非常大
// PID参数跟采样时间息息相关
/*************************************/
#define  SPD_P_DATA      5.5f        // P参数
#define  SPD_I_DATA      1.56f       // I参数
#define  SPD_D_DATA      0.0f        // D参数
#define  TARGET_SPEED    100.0f      // 目标速度    15r/m
   
    typedef struct
{
  __IO int32_t  SetPoint;    //设定目标 Desired Value
	
  __IO float    SumError;    //误差累计
  __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_TypeDef;
 
 /* PID结构体 */
PID_TypeDef  sPID;               // PID参数结构体
PID_TypeDef  *ptr =  &sPID;

/**
  * 函数功能: PID参数初始化
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明: 无
  */
void PID_ParamInit(void)
{
	   sPID.SumError  = 0;
    sPID.LastError = 0;               // Error[-1]
    sPID.PrevError = 0;               // Error[-2]
	
    sPID.Proportion = SPD_P_DATA; // 比例常数 Proportional Const
    sPID.Integral 	= SPD_I_DATA;   // 积分常数  Integral Const
    sPID.Derivative = SPD_D_DATA; // 微分常数 Derivative Const
	
    sPID.SetPoint = TARGET_SPEED;     // 设定目标Desired Value
}
/**
  * 函数名称：速度闭环PID控制设计
  * 输入参数：当前控制量
  * 返 回 值：目标控制量
  * 说    明：根据增量式离散PID公式 
	pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差
  */
int32_t SpdPIDCalc_A(float NextPoint)
{
  float iError,iIncpid;
  iError = (float)sPID.SetPoint - NextPoint; //偏差

  /* 给恒定的占空比的时候，0.5r/m 的跳动范围是正常的 */
  if((iError<0.5f )&& (iError>-0.5f))
    iError = 0.0f;

  iIncpid=(sPID.Proportion * iError)                //E[k]项
              -(sPID.Integral * sPID.LastError)     //E[k-1]项
              +(sPID.Derivative * sPID.PrevError);  //E[k-2]项

  sPID.PrevError = sPID.LastError;                  //存储误差，用于下次计算
  sPID.LastError = iError;
  return(iIncpid);                  //返回增量值

}

/**
  * 函数名称：位置闭环PID控制设计
  * 输入参数：当前控制量
  * 返 回 值：目标控制量
  * 说    明：无
  */
int32_t LocPIDCalc_A(int32_t NextPoint)
{
  int32_t iError,iIncpid;
  iError = sPID.SetPoint - NextPoint; //偏差

  iIncpid=(int32_t)( (sPID.Proportion * (float)iError)          //E[k]项
                  - (sPID.Integral * (float)sPID.LastError)     //E[k-1]项
                  + (sPID.Derivative * (float)sPID.PrevError) );//E[k-2]项

  sPID.PrevError = sPIDLastError;   //存储误差，用于下次计算
  sPID.LastError = iError;
  return(iIncpid);                   //返回增量值
}

然后把这个值放在pwm调控 MotorPwm(int a, int iIncpid) ；（电机号，pid反馈值）