小车PID算法跑直线

时间：2014-08-03 20:40:05 阅读：845 评论：0 收藏：0 [点我收藏+]

标签：算法

#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

小车PID算法跑直线

标签：算法

原文地址：http://blog.csdn.net/u011046042/article/details/38359317

踩

(0)

评论一句话评论（0）

分享档案

更多>

2021年07月29日 (22)
2021年07月28日 (40)
2021年07月27日 (32)
2021年07月26日 (79)
2021年07月23日 (29)
2021年07月22日 (30)
2021年07月21日 (42)
2021年07月20日 (16)
2021年07月19日 (90)
2021年07月16日 (35)

周排行