Car PID algorithm running straight line

# Include <stm32f10x. h>
# Include "SYS. H"


Extern unsigned char rec_dat_u1;
Extern unsigned char rec_string_u1 [2, 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; // proportional constant
Float ti_l = 0.09; // differential time constant
Float td_l = 0.015; // sampling period


Float kp_r= 0.20;
Float ti_r = 0.09;
Float fig = 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 (0 xFFFF, 31); // 1 Mbit/s speed Sampling Rate
Tim5_pwminput_init (0 xFFFF, 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 (& fig, 160 );
Pid_setpoint (& fig, 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 );
}