基于STM32的PID算法实现

#include"pid.h" #include"fuzzy.h" #include"pwm.h" #include"dac.h" PIDtypedef PID1; //PID结构体 PIDtypedef PID2; PIDtypedef PID3; PIDtypedef PID4; extern u8 start_flag; extern u16 pwm1,pwm2,pwm3,pwm4; void PIDperiodinit(u16 arr,u16 psc) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; NVIC_InitTypeDef NVIC_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); //时钟使能 //定时器TIM6初始化 TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值 TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //设置时钟分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式 TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStructure); //根据指定的参数初始化TIMx的时间基数单位 TIM_ITConfig(TIM6,TIM_IT_Update,ENABLE ); //使能指定的TIM3中断,允许更新中断 //中断优先级NVIC设置 NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn; //TIM3中断 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; //先占优先级1级 NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //从优先级1级 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能 NVIC_Init(&NVIC_InitStructure); //初始化NVIC寄存器 // TIM_Cmd(TIM6, ENABLE); //放在主程序中使能 } void TIM6_IRQHandler(void) // 采样时间到，中断处理函数 { if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)//更新中断 { frequency1=1000000/period_TIM4 ; //通过捕获的波形的周期算出频率 frequency2=1000000/period_TIM1 ; frequency3=1000000/period_TIM2 ; frequency4=1000000/period_TIM5 ; /********PID1处理**********/ PID1.sum_error+=(incPIDcalc(&PID1,frequency1)); //计算增量并累加 pwm1=PID1.sum_error*4.6875 ; //pwm1 代表将要输出PWM的占空比 frequency1=0; //清零 period_TIM4=0; /********PID2处理**********/ PID2.sum_error+=(incPIDcalc(&PID2,frequency2)); //计算增量并累加 Y=Y+Y' pwm2=PID2.sum_error*4.6875 ; //将要输出PWM的占空比 frequency2=0; period_TIM1=0; /********PID3处理**********/ PID3.sum_error+=(incPIDcalc(&PID3,frequency3)); //常规PID控制 pwm3=PID3.sum_error*4.6875 ; //将要输出PWM的占空比 frequency3=0; period_TIM2=0; /********PID4处理**********/ PID4.sum_error+=(incPIDcalc(&PID4,frequency4)); //计算增量并累加 pwm4=PID4.sum_error*4.6875 ; //将要输出PWM的占空比 frequency4=0; period_TIM5=0; } TIM_SetCompare(pwm1,pwm2,pwm3,pwm4); //重新设定PWM值 TIM_ClearITPendingBit(TIM6, TIM_IT_Update); //清除中断标志位 } void incPIDinit(void) { //PID1参数初始化 PID1.sum_error=0; PID1.last_error=0; PID1.prev_error=0; PID1.proportion=0; PID1.integral=0; PID1.derivative=0; PID1.setpoint=0; //PID2参数初始化 PID2.sum_error=0; PID2.last_error=0; PID2.prev_error=0; PID2.proportion=0; PID2.integral=0; PID2.derivative=0; PID2.setpoint=0; //PID3参数初始化 PID3.sum_error=0; PID3.last_error=0; PID3.prev_error=0; PID3.proportion=0; PID3.integral=0; PID3.derivative=0; PID3.setpoint=0; //PID4参数初始化 PID4.sum_error=0; PID4.last_error=0; PID4.prev_error=0; PID4.proportion=0; PID4.integral=0; PID4.derivative=0; PID4.setpoint=0; } void PID_setpoint(PIDtypedef*PIDx,u16 setvalue) { PIDx->setpoint=setvalue; } int incPIDcalc(PIDtypedef *PIDx,u16 nextpoint) { int iError,iincpid; iError=PIDx->setpoint-nextpoint; //当前误差 /*iincpid= //增量计算 PIDx->proportion*iError //e[k]项 -PIDx->integral*PIDx->last_error //e[k-1] +PIDx->derivative*PIDx->prev_error;//e[k-2] */ iincpid= //增量计算 PIDx->proportion*(iError-PIDx->last_error) +PIDx->integral*iError +PIDx->derivative*(iError-2*PIDx->last_error+PIDx->prev_error); PIDx->prev_error=PIDx->last_error; //存储误差，便于下次计算 PIDx->last_error=iError; return(iincpid) ; } void PID_set(float pp,float ii,float dd) { PID1.proportion=pp; PID1.integral=ii; PID1.derivative=dd; PID2.proportion=pp; PID2.integral=ii; PID2.derivative=dd; PID3.proportion=pp; PID3.integral=ii; PID3.derivative=dd; PID4.proportion=pp; PID4.integral=ii; PID4.derivative=dd; } /*最后设定四个轮子的转速，转速 1rad/s等价于122.23个脉冲每秒的转速*/ void set_speed(float W1,float W2,float W3,float W4) { float temp; if(W1>0) //判断W 正负，正数处理 { motor1_out0=0; motor1_out1=1; temp=W1*122.23; PID_setpoint(&PID1,temp); } else if(W1==0) //零值 { motor1_out0=0; motor1_out1=0; PID_setpoint(&PID1,0); } else //负数处理 { motor1_out0=1; motor1_out1=0; temp=-W1*122.23; PID_setpoint(&PID1,temp); } if(W2>0) { motor2_out0=0; motor2_out1=1; temp=W2*122.23; PID_setpoint(&PID2,temp); } else if(W2==0) { motor2_out0=0; motor2_out1=0; PID_setpoint(&PID2,0); } else { motor2_out0=1; motor2_out1=0; temp=-W2*122.23; PID_setpoint(&PID2,temp); } if(W3>0) { motor3_out0=0; motor3_out1=1; temp=W3*122.23; PID_setpoint(&PID3,temp); } else if(W3==0) { motor3_out0=0; motor3_out1=0; PID_setpoint(&PID3,0); } else { motor3_out0=1; motor3_out1=0; temp=-W3*122.23; PID_setpoint(&PID3,temp); } if(W4>0) { motor4_out0=0; motor4_out1=1; temp=W4*122.23; PID_setpoint(&PID4,temp); } else if(W4==0) { motor4_out0=0; motor4_out1=0; PID_setpoint(&PID4,0); } else { motor4_out0=1; motor4_out1=0; temp=-W4*122.23; PID_setpoint(&PID4,temp); } } void stop() { PID_setpoint(&PID1,0); PID1.sum_error=0; PID1.last_error=0; PID1.prev_error=0; PID_setpoint(&PID2,0); PID2.sum_error=0; PID2.last_error=0; PID2.prev_error=0; PID_setpoint(&PID3,0); PID3.sum_error=0; PID3.last_error=0; PID3.prev_error=0; PID_setpoint(&PID4,0); PID4.sum_error=0; PID4.last_error=0; PID4.prev_error=0; }