2020年04月06日 | MSP432E401Y单片机智能小车PID调速代码

 * MSP432E401Y

 * Description:PID处理函数

 * 引脚：

 *       

 * Author: Robin.J

***************************************************************************/

#include

#include "ti/devices/msp432e4/driverlib/driverlib.h"

float iError_l , iIncpid_l; //当前误差

float iError_r , iIncpid_r; //当前误差

float variable_l;

float variable_r;

extern int ch_l,ch_r;

//extern float ch;

//PID初始化

void IncPIDInit(void)

{

    sptr->SetPoint_l    = 30.0; 

        sptr->SetPoint_r    = 30.0;         //设定值

    sptr->LastError_l = 0;      //前1次误差值

    sptr->PrevError_l = 0;      //前2次误差值

        sptr->LastError_r = 0;      //前1次误差值

    sptr->PrevError_r = 0;      //前2次误差值

    sptr->Proportion_r = 0.4;//0.5;//0.8;       //比例

    sptr->Integral_r     = 0.024;//0.026;//0.15;    //积分

    sptr->Derivative_r = 0.0;       //微分

        sptr->Proportion_l = 0.8;       //比例

    sptr->Integral_l     = 0.15;    //积分

    sptr->Derivative_l = 0.0;       //微分

        sptr->A_r = sptr->Proportion_r + sptr->Integral_r + sptr->Derivative_r;

        sptr->B_r = -(2*sptr->Derivative_r + sptr->Proportion_r);

        sptr->C_r = sptr->Derivative_r;

        sptr->A_l = sptr->Proportion_l + sptr->Integral_l + sptr->Derivative_l;

        sptr->B_l= -(2*sptr->Derivative_l + sptr->Proportion_l);

        sptr->C_l = sptr->Derivative_l;

    sptr->iError_l = 0;     //当前误差

        sptr->iError_r = 0;

    sptr->iIncpid_l=0;          //增量误差

        sptr->iIncpid_r=0;

        sptr->limit = 49;

}

/* 左轮PID控制*/

float IncPIDCalc_l(float NextPoint)

{

  if(ch_l != 0){

    sptr->SetPoint_l = ch_l;

    iError_l = sptr->SetPoint_l - NextPoint; //增量计算

  }else{

    return 0;

  }

//    iError_l = sptr->SetPoint_l - NextPoint; //增量计算

   sptr->iIncpid_l =(sptr->A_l * iError_l //E[k]项

             + sptr->B_l * sptr->LastError_l //E[k－1]项

             + sptr->C_l * sptr->PrevError_l); //E[k－2]项

   variable_l =iError_l -  sptr->LastError_l;

    sptr->PrevError_l = sptr->LastError_l;   //存储误差，用于下次计算

    sptr->LastError_l = iError_l;

/*PWM limiting function, PWM value is 80, minimum is 0

*/

//    if(sptr->iIncpid  >= 0)

//    {

//      if(sptr->iIncpid <= sptr->limit){

//        

//        sptr->iIncpid += (pwm_set*100);

//      }

//      else{

//        sptr->iIncpid = (int)(sptr->limit + pwm_set * 100);

//      }

//    }

//    else if(sptr->iIncpid < 0){

//      

//            if(sptr->iIncpid >= -sptr->limit){

//        

//               sptr->iIncpid += (pwm_set*100);

//              }

//            else{

//            

//              sptr->iIncpid =(int)(pwm_set-sptr->limit);

//            }

//      

//    }

        if(sptr->iIncpid_l >= sptr->limit)

    {

      sptr->iIncpid_l = sptr->limit;

      }

      else if(sptr->iIncpid_l < -sptr->limit){

        sptr->iIncpid_l = -sptr->limit;

      }

  return(sptr->iIncpid_l);                         //返回增量值

}

/* 右轮PID控制*/

float IncPIDCalc_r(float NextPoint)

{

    if(ch_r != 0){

    sptr->SetPoint_r = ch_r;

    iError_r = sptr->SetPoint_r - NextPoint; //增量计算

  }else{

    return 0;

  }

    //iError_r = sptr->SetPoint_r - NextPoint; //增量计算

   sptr->iIncpid_r =(sptr->A_r * iError_r //E[k]项

             + sptr->B_r * sptr->LastError_r //E[k－1]项

             + sptr->C_r * sptr->PrevError_r); //E[k－2]项

   variable_r =iError_r -  sptr->LastError_r;

    sptr->PrevError_r = sptr->LastError_r;   //存储误差，用于下次计算

    sptr->LastError_r = iError_r;

    if(sptr->iIncpid_r >= sptr->limit)

    {

      sptr->iIncpid_r = sptr->limit;

      }

      else if(sptr->iIncpid_r < -sptr->limit){

        sptr->iIncpid_r = -sptr->limit;

      }

  return(sptr->iIncpid_r);                         //返回增量值

}

/******************* (C) COPYRIGHT 2018 DY EleTe *****END OF FILE************/