平衡小车PID控制程序，有看懂的大神，可以给我详细解释下吗？谢谢

cauccy · 发表于 2015-5-30 11:51:21

名称：void pid(float angle, float angle_dot)
功能：PID运算
输入参数：
float angle 倾斜角度
float angle_dot 倾斜角速度
输出参数：无
返回值：无
**************************************************/
void pid(float angle, float angle_dot)
{
u32 temp;
u16 sl, sr;

TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;

now_error = set_point - angle;

speed_filter();

speed *= 0.7;
speed += speed_out * 0.3;
position += speed;
position -= speed_need;

if(position < -60000)
{
    position = -60000;
}
else if(position > 60000)
{
    position = 60000;
}

rout = proportion * now_error + derivative * angle_dot - position * integral2 - derivative2 * speed;

speed_l = -rout + turn_need_l;
speed_r = -rout + turn_need_r;

if(speed_l > MAX_SPEED)
{
    speed_l = MAX_SPEED;
}
else if(speed_l < -MAX_SPEED)
{
speed_l = -MAX_SPEED;
}

if(speed_r > MAX_SPEED)
{
    speed_r = MAX_SPEED;
}
else if(speed_r < -MAX_SPEED)
{
speed_r = -MAX_SPEED;
}

if(speed_l > 0)
{
    GPIO_ResetBits(GPIOB, GPIO_Pin_8); //left fr
sl = speed_l;
}
else
{
    GPIO_SetBits(GPIOB, GPIO_Pin_8);
sl = speed_l * (-1);
}

if(speed_r > 0)
{
    GPIO_SetBits(GPIOA, GPIO_Pin_3); //right fr
sr = speed_r;
}
else
{
    GPIO_ResetBits(GPIOA, GPIO_Pin_3);
sr = speed_r * (-1);
}

temp = 1000000 / sl;
if(temp > 65535)
{
    sl = 65535;
}
else
{
    sl = (u16)temp;
}

temp = 1000000 / sr;
if(temp > 65535)
{
    sr = 65535;
}
else
{
    sr = (u16)temp;
}

/* Time base configuration */
TIM_TimeBaseStructure.TIM_Period = sl - 1; // (Period + 1) * (Prescaler + 1) / 72M = 1ms
TIM_TimeBaseStructure.TIM_Prescaler = 30 - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_Pulse = sl >> 1;

TIM_OC4Init(TIM4, &TIM_OCInitStructure); //PB9 left

/* Time base configuration */
TIM_TimeBaseStructure.TIM_Period = sr - 1;
TIM_TimeBaseStructure.TIM_Prescaler = 30 - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_Pulse = sr >> 1;

TIM_OC3Init(TIM2, &TIM_OCInitStructure); //PA2 right
}