[其他ST产品] 小车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);   

}