AGV小车PID算法跑直线-AGV吧

#include
#include”sys.h”
extern unsigned char Rec_Dat_U1;
extern unsigned char Rec_String_U1[200];
extern int conut_flag_1;
#define in_1(x) x?GPIO_ResetBits(GPIOC , GPIO_Pin_0): GPIO_SetBits(GPIOC , GPIO_Pin_0)
#define in_2(x) x?GPIO_ResetBits(GPIOC , GPIO_Pin_1): GPIO_SetBits(GPIOC , GPIO_Pin_1)
#define in_3(x) x?GPIO_ResetBits(GPIOB , GPIO_Pin_10): GPIO_SetBits(GPIOB , GPIO_Pin_10)
#define in_4(x) x?GPIO_ResetBits(GPIOB , GPIO_Pin_11): GPIO_SetBits(GPIOB , GPIO_Pin_11)
#define key_1 GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_12)
#define key_2 GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_13)
#define key_3 GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_14)
#define key_4 GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_15)
float Kp_l = 0.82 ; //比例常数
float Ti_l = 0.09 ; //微分时间常数
float Td_l = 0.015 ; //采样周期
float Kp_r = 0.20 ;
float Ti_r = 0.09 ;
float Td_r = 0.015 ;
#define T 0.02
#define Ki_l Kp_l*(T/Ti_l) // Kp Ki Kd
#define Kd_l Kp_l*(Td_l/T)
#define Ki_r Kp_l*(T/Ti_l) // Kp Ki Kd
#define Kd_r Kp_l*(Td_l/T)
#define left_b in_1(1); in_2(0);
#define left_f in_1(0); in_2(1);
#define right_f in_3(1); in_4(0);
#define right_b in_1(0); in_2(1);
extern float pwm_l;
extern float pwm_r;
void CAR_GPIO_INIT(void);
int main()
{
// int speed=50;
SYS_CONFIG();
CAR_GPIO_INIT();
pwm_init(TIM3_1);
pwm_init(TIM3_2);
TIM4_PWMINPUT_INIT(0xffff,31); //1M速度采样率
TIM5_PWMINPUT_INIT(0xffff,31);
incPIDinit();
PID_set(Kp_l,Ki_l,Kd_l,Kp_r,Ki_r,Kd_r);
//void PID_set(float pp_1,float ii_1,float dd_1,float pp_2,float ii_2,float dd_2)
right_f
left_f
// set_pwm_percent(TIM3_1,50) ;
// set_pwm_percent(TIM3_2,50) ;
PID_setpoint(&PID_l, 160);
PID_setpoint(&PID_r, 160);
while(1)
{
printf(“%d\r\n”,(int)pwm_l);
delay_ms(200);
}
}
void CAR_GPIO_INIT()
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOC|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOD, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOD, &GPIO_InitStructure);
}