电机PID补偿程序

只看该作者 · 2017-3-31 17:41

电机的反馈补偿，应用了PID算法，本人不是太懂，但是最近急需弄明白32程序的PID语言意义。
#include "led.h"
#include "delay.h"
#include "sys.h"
#include "pwm.h"
#include "timer.h"
#include "exti.h"
#include "usart.h"

u16 counter;
u16 PWM_MOTO=3445;
u16 PWM_IN=70;
PID_AbsoluteType PID_Control;//定义PID算法的结构体
void PID_Init(void);
int main(void)
{
u16 led0pwmval=0;
u8 dir=1;

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
delay_init();            //延时函数初始化
uart_init(115200);
LED_Init();       //初始化与LED连接的硬件接口
TIM5_Int_Init(99,71); //0.1ms中断，用于计算输入信号触发与电机转一圈触发信号的时间差
TIM4_PWM_Init(9999,0);////pwm控制电机转速
TIM2_PWM_Init(1666,719); //输出一个接近60 Hz的的信号
TIM_SetCompare1(TIM4,PWM_MOTO);    //通道1
//TIM_SetCompare1(TIM2,7999);
TIM3_Int_Init(999,7199); //100ms秒中断
EXTIX_Init();
PID_Init();
   while(1)
{
// delay_ms(4);
if(USART_RX_STA & 0x8000)
{
USART_RX_STA=0;
if(USART_RX_BUF[0]==1)  PWM_MOTO+=USART_RX_BUF[1]; //串口调试转速
else PWM_MOTO-=USART_RX_BUF[1];
// if(USART_RX_BUF[0]==4) TIM2_PWM_Init(100000/USART_RX_BUF[1],719);//串口改变输入信号
if(USART_RX_BUF[0]==2) //调节PID参数
{
PID_Control.kp = USART_RX_BUF[1] * 0.1f;
PID_Control.ki = USART_RX_BUF[2] * 0.1f;
PID_Control.kd = USART_RX_BUF[3] * 0.1f;
printf("P=%f I=%f D=%f\r\n",PID_Control.kp,PID_Control.ki,PID_Control.kd);
}
if(USART_RX_BUF[0]==3) //调节PID参数
{
PID_Control.kp1 = USART_RX_BUF[1] * 0.001f;
PID_Control.ki1 = USART_RX_BUF[2] * 0.001f;
PID_Control.kd1 = USART_RX_BUF[3] * 0.001f;
printf("P=%f I=%f D=%f\r\n",PID_Control.kp,PID_Control.ki,PID_Control.kd);
}
printf("%d\r\n",PWM_MOTO);
printf("%d\r\n",USART_RX_BUF[1]);
TIM_SetCompare1(TIM4,PWM_MOTO);
}

//printf("%d\r\n",InCounter);
}
}

////////////////////////////////////////////////////////////////////////
//绝对式PID算法
void PID_AbsoluteMode(PID_AbsoluteType* PID) //速度环控制
{
// if(PID->kp    < 0) PID->kp    = -PID->kp;
// if(PID->ki    < 0) PID->ki    = -PID->ki;
// if(PID->kd    < 0) PID->kd    = -PID->kd;
// if(PID->errILim < 0) PID->errILim = -PID->errILim;

PID->errP = PID->errNow;  //读取现在的误差，用于kp控制

PID->errI += PID->errNow; //误差积分，用于ki控制

if(PID->errILim != 0)    //微分上限和下限
{
  if(    PID->errI >  PID->errILim) PID->errI =  PID->errILim;
  else if(PID->errI < -PID->errILim) PID->errI = -PID->errILim;
}

PID->errD = PID->errNow - PID->errOld;//误差微分，用于kd控制

PID->errOld = PID->errNow; //保存现在的误差

PID->ctrOut = PID->kp * PID->errP + PID->ki * PID->errI + PID->kd * PID->errD;//计算绝对式PID输出

}
void PID_Trig(PID_AbsoluteType* PID) //触发控制
{
//u16 temp;
//temp = (100.0f / Incounter1)
// if(PID->errNow1 > 500) PID->errNow1 = -(1000 - PID->errNow1); //计算在输入信号的前还是后触发，100ms计算一次，数值大于50ms则算作输入信号之后触发
//    PID->errNow1 =  -PID->errNow1; //根据触发先后顺序，0 - 500ms 是电机触发慢于输入信号触发
PID->errP1 = PID->errNow1;  //读取现在的误差，用于kp控制

PID->errI1 += PID->errNow1; //误差积分，用于ki控制
if(PID->errILim1 != 0)    //微分上限和下限
{
  if(    PID->errI1 >  PID->errILim1) PID->errI1 =  PID->errILim1;
  else if(PID->errI1 < -PID->errILim1) PID->errI1 = -PID->errILim1;
}
PID->errD1 = PID->errNow1 - PID->errOld1;//误差微分，用于kd控制

PID->errOld1 = PID->errNow1; //保存现在的误差

PID->ctrOut1 = PID->kp1 * PID->errP1 + PID->ki1 * PID->errI + PID->kd1 * PID->errD1;//计算绝对式PID输出
//if(PID->ctrOut1>55) PID->ctrOut1 = 55; //xianzhi
// if(PID->ctrOut1<-55) PID->ctrOut1 = -55; //xianzhi
}

/*****************************************电机速度环伺服***********************************************/

s32 spdTag, spdNow, control;//定义一个目标速度，采样速度，控制量
u16 CC,CC1;
u8 C_Flag=1;
double k=1; //转速误差系数

void User_PidSpeedControl(s32 SpeedTag)
{
spdNow = InCounter; //电机转一圈计数18个脉冲
   spdTag = SpeedTag;

PID_Control.errNow =spdNow - spdTag+7.5; //计算并写入速度误差
PID_Control.errNow1 = Time_2; //计算并写入速度误差

// PID_AbsoluteMode(&PID_Control); PID_Trig(&PID_Control);control = PID_Control.ctrOut + PID_Control.ctrOut1
//control = PID_Control.ctrOut ;
   if(PID_Control.errNow == 0) {CC++;if(CC>13) {CC=0; C_Flag=2;}CC1=0;} //Trig PID 转速等于输入信号，开始调节触发
else {CC1++; if(CC1>20){CC1 = 0;C_Flag=1;}CC=0;}
//else {  control = PID_Control.ctrOut ;}       //读取控制值/ Speed PID算法
   if(C_Flag==2 && (Time_2>10 || Time_2<-10)) {       PID_Trig(&PID_Control);control =  PID_Control.ctrOut + PID_Control.ctrOut1;}
   if(C_Flag==1) { PID_AbsoluteMode(&PID_Control); control = PID_Control.ctrOut ; }
   if(CC1>40) {CC=0;}
//    if(CC>40 && C_Flag==0) {C_Flag=1;CC=0;}
//    if(CC>40 && C_Flag==1) {C_Flag=0;CC=0;}

   //if(control>4999) control=4999; //限幅
   //if(control<0) control=0;
TIM_SetCompare1(TIM4,(control+3500)*k); //占空比在60%以上电机才开始转动
   //#ifdef _TEST_
   printf("Motor=%d Out=%.1f Out1=%.1f C_Flag=%d errNow=%f \r\n",control,PID_Control.ctrOut,PID_Control.ctrOut1,C_Flag,PID_Control.errNow);
   //#endif
   //PID_Control.ctrOut1=0; //清零
}

void PID_Init(void)
{
PID_Control.kp    = 2.2f;          //写入比例系数
PID_Control.ki    = 3.0f;             //写入积分系数
PID_Control.kd    = 1.5f;             //写入微分系数
PID_Control.errILim = 3000;          //写入误差积分上下限
PID_Control.kp1    = 0.2f;          //写入比例系数
PID_Control.ki1    = 0.00f;             //写入积分系数
PID_Control.kd1    = 1.1f;             //写入微分系数
PID_Control.errILim1 = 500;          //写入误差积分
}