STM32智能小車路徑閉環(huán)PID算法源碼省賽一等獎作品

ID:379889 · 發(fā)表于 2019-1-18 23:45

希望給做智能小車的朋友一點點幫助，里面包含工程文件模板基于STM32F103ZET6芯片、encoder編碼器、串口數(shù)據(jù)讀取、PID閉環(huán)控制、乃至一點尋跡PID思路，該尋跡PID為本作者原創(chuàng)，算法完成以后，不需要任何費時的調(diào)試，模塊化完成之后即可在95%穩(wěn)定性（測試之后）尋跡，因此上屆省賽我以第三的成績出線（一等獎）。

單片機源程序如下:

#include "delay.h"
#include "sys.h"
#include "usart.h"
#include "encoder.h"
#include "stm32f10x.h"
#include "pid.h"
#include "PWM.h"
#include "key.h"
int u=2;
int main(void)
{
delay_init(); //延時函數(shù)初始化
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //設置NVIC中斷分組2:2位搶占優(yōu)先級，2位響應優(yōu)先級
uart_init(115200); //串口初始化為115200
TIM8_Configuration();//pwm
KEY_Init();
while(1)
{
int p=1;
if(p!=GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_4))
{delay_ms(10);
if(p!=GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_4))
{u++;
delay_ms(500);
}
printf("%d\r\n",0);
// while(1)
// { jisuan_2();
// jisuan_();}
}
}
}

復制代碼

pid.c

#include "pid.h"
#include "encoder.h"
#include "stm32f10x.h"
int V_encoder_TIM3(void)//1電機速度計算轉/秒
{
int cnt1;
cnt1=((int16_t)TIM3->CNT);
TIM3->CNT=0;
return cnt1;
}
int User_PidSpeedControl1(float SpeedTag)
{
float control1=0;
float kp=450;
float ki=1;
float kd=1;
float errILim=999;
float errNow;
float errOld=0;
float errP=0;
float errI=0;
float errD=0;
float spdNow1;
float s;
spdNow1=s;
errNow =SpeedTag*1.1 - spdNow1;
errP=errNow;
errI+=errNow;
if(errILim != 0)
{
if(errI >= errILim) errI = errILim;
else if(errI <= -errILim) errI = -errILim;
}
errD= errNow - errOld;
errOld = errNow;
control1= kp * errP + ki * errI + kd * errD;
if(control1 >= 1000) control1 = 1000-1 ;//上限 CCR的值必須小于或等于ARR的值
if(control1 <=-1000) control1 = -(1000-1);//下限
if(control1>=0.0) { GPIO_SetBits(GPIOC,GPIO_Pin_4); GPIO_ResetBits(GPIOC,GPIO_Pin_5); }
else {GPIO_SetBits(GPIOC,GPIO_Pin_5); GPIO_ResetBits(GPIOC,GPIO_Pin_4);}
if(control1 <0) control1 =-control1;//下限
if(s<=0.1&&s>=-0.1&&SpeedTag==0)TIM_SetCompare1(TIM8,0);
else TIM_SetCompare1(TIM8,control1);//放入PWM
return (int)control1;
}