#include "stm32f10x.h"
#include "PID.h"
int Speed_PID (int16_t target ,int16_t change) //差速PID控制器
{
float KP=0.405,KI=0.66,KD=0.62;
float Bias,Pwm,jifen;
static float Last_Bias;
Bias=target-change; //計算偏差
jifen+=Bias; //求出偏差的積分
Pwm=KP*Bias+KI*jifen+KD*(Bias-Last_Bias); //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
if(Pwm>2000) Pwm=2000;
if(Pwm<-2000) Pwm=-2000;
//if(target==0&&change==0)Pwm=0;
return Pwm; //返回PWM值
}
pid.h文件- #ifndef __PID_H
- #define __PID_H
- #include "stm32f10x.h"
- int Speed_PID (int16_t target ,int16_t change) ;
- #endif
注意PID的調(diào)用一定不能放在主循環(huán)里面，因為主循環(huán)的周期會隨著代碼量增加而變
我是放在中斷里面。
int main(void)
{
OLED_Init();
Motor_Init();
//hongwai_Init();
TIM1_Init(1000,72); //定時中斷初始化
Encoder_Init_TIM4( ); //編碼器初始化
Encoder_Init_TIM3( );
/*顯示靜態(tài)字符串*/
OLED_ShowString(1, 1, "Time:"); //1行1列顯示字符串Num:
OLED_ShowString(1, 8, ":");
OLED_ShowString(1, 11, ":");
OLED_ShowString(2, 1, "Speed:"); //1行1列顯示字符串Speed:
OLED_ShowString(3, 1, "path:"); //1行1列顯示字符串Speed:
OLED_ShowString(3, 11, "."); //1行1列顯示字符串Speed:
//Car_go();
while (1)
{
OLED_ShowNum(1,12, miao, 2); //不斷刷新顯示Num變量
OLED_ShowNum(1, 9, fen, 2);
OLED_ShowNum(1, 6, shi, 2);
OLED_ShowSignedNum(2, 7, Speed2, 4); //不斷刷新顯示編碼器測得的最新速度
OLED_ShowSignedNum(2, 12, Speed1,4); //不斷刷新顯示編碼器測得的最新速度
Car_go();
OLED_ShowNum(3, 6, M, 5); //不斷刷新顯示編碼器測得的最新速度
OLED_ShowNum(3, 12, CM, 2); //不斷刷新顯示編碼器測得的最新速度
OLED_ShowNum(4,1,sensor[0],1);
OLED_ShowNum(4,2,sensor[1],1);
OLED_ShowNum(4,3,sensor[2],1);
OLED_ShowNum(4,4,sensor[3],1);
OLED_ShowSignedNum(4, 6, OUTPUT2,5);
Speed1 = Encoder_Get_TIM4( );
Speed2= Encoder_Get_TIM3( );
//read_sensor();
if(yan == 0)
{
yan = Control();
}
//Car_leftstop();
//Car_rightstop( );
}
}
void TIM1_UP_IRQHandler(void)
{
static int8_t cnt = 0;
if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) //檢查TIM3更新中斷發(fā)生與否
{ //判斷是否是TIM1的更新事件觸發(fā)的中斷
{
//if(yan == 0)
cnt++;
if(cnt == 100)
{
cnt = 0;
miao++; //Num變量自增，用于測試定時中斷
if(miao>=60){miao=0;fen++;}
if(fen>=60){fen=0;shi++;}
}
OUTPUT2=Speed_PID (Speed2,Speed1); //將差速PID控制器輸出的PWM值賦值給
TIM_ClearITPendingBit(TIM1, TIM_IT_Update ); //清除TIMx更新中斷標志
/*寫入執(zhí)行的操作*/
}
}
}
|
