小车PID算法跑直线

#include<stm32f10x.h>
#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);   
}