小车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);

}

小车PID算法跑直线,布布扣,bubuko.com