mirror of
https://github.com/ycHepth/KalmanFilter.git
synced 2026-03-13 10:23:28 +08:00
pid.c
This commit is contained in:
YcChan
327
pid.c
Normal file
327
pid.c
Normal file
@@ -0,0 +1,327 @@
|
||||
#include "stm32f10x.h"
|
||||
#include "pid.h"
|
||||
|
||||
s16 speed_l = 0;
|
||||
s16 speed_r = 0;
|
||||
s16 speed_need = 0;
|
||||
s16 turn_need_r = 0;
|
||||
s16 turn_need_l = 0;
|
||||
s16 speed_out = 0;
|
||||
|
||||
float rout;
|
||||
float set_point = 0;
|
||||
float now_error;
|
||||
float proportion = 300;
|
||||
float integral2 = 0.01;
|
||||
float derivative = 1;
|
||||
float derivative2 = 1;
|
||||
double position ;
|
||||
double speed;
|
||||
|
||||
#define FILTER_COUNT 20
|
||||
#define MAX_SPEED 5500
|
||||
s16 speed_buf{FILTER_COUNT} = {0};
|
||||
|
||||
void motor_init(void)
|
||||
{
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
|
||||
TIM_OCInitTypeDef TIM_OCInitStructure; //Output Compare
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB , ENABLE);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
|
||||
GPIO_Init(GPIOB,&GPIO_InitStructure);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_6 | GPIO_Pin_7;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
|
||||
GPIO_Init(GPIOA,&GPIO_InitStructure);
|
||||
|
||||
GPIO_ResetBits(GPIOB, GPIO_Pin_1); //AT1 50% current
|
||||
GPIO_SetBits(GPIOB, GPIO_Pin_0); //AT2
|
||||
GPIO_SetBits(GPIOB, GPIO_Pin_6); //ST
|
||||
GPIO_ResetBits(GPIOB, GPIO_Pin_7); //OE
|
||||
GPIO_SetBits(GPIOB, GPIO_Pin_8); //FR left side
|
||||
|
||||
GPIO_ResetBits(GPIOA, GPIO_Pin_1); //AT1 50% current
|
||||
GPIO_SetBits(GPIOA, GPIO_Pin_0); //AT2
|
||||
GPIO_SetBits(GPIOA, GPIO_Pin_7); //ST
|
||||
GPIO_ResetBits(GPIOA, GPIO_Pin_6); //OE
|
||||
GPIO_ResetBits(GPIOA, GPIO_Pin_3); //FR right side
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //left
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
|
||||
GPIO_Init(GPIOB,&GPIO_InitStructure);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; //right
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
|
||||
GPIO_Init(GPIOA,&GPIO_InitStructure);
|
||||
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
|
||||
|
||||
TIM_TimeBaseStructure.TIM_Period = 7200 - 1;
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = 10 - 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_Pluse = 3600;
|
||||
|
||||
TIM_OC4Init(TIM4,&TIM_OCInitStructure);
|
||||
|
||||
TIM_OC4PreloadConfig(TIM4,TIM_OCPerload_Enable);
|
||||
TIM_ARRPreloadConfig(TIM4,ENABLE);
|
||||
|
||||
TIM_Cmd(TIM4,ENABLE);
|
||||
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
|
||||
|
||||
TIM_TimeBaseStructure.TIM_Period = 7200 - 1;
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = 10 - 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 = 3600;
|
||||
|
||||
TIM_OC3Init(TIM2, &TIM_OCInitStructure); //PA2 right
|
||||
|
||||
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
|
||||
TIM_ARRPreloadConfig(TIM2, ENABLE);
|
||||
|
||||
TIM_Cmd(TIM2,ENABLE);
|
||||
}
|
||||
|
||||
void receive_parameter(u8 cmd)
|
||||
{
|
||||
switch(cmd)
|
||||
{
|
||||
case 'u': // forward
|
||||
integral2 = 0.03;
|
||||
derivative2 = 1.5;
|
||||
speed_need = 1000;
|
||||
turn_need_r = 0;
|
||||
turn_need_l = 0;
|
||||
break;
|
||||
case 1: // forward
|
||||
integral2 = 0.03;
|
||||
derivative2 = 1.5;
|
||||
speed_need = 1000;
|
||||
turn_need_r = 0;
|
||||
turn_need_l = 0;
|
||||
break;
|
||||
|
||||
case 'd': // back
|
||||
integral2 = 0.03;
|
||||
derivative2 = 1.5;
|
||||
speed_need = -1000;
|
||||
turn_need_r = 0;
|
||||
turn_need_l = 0;
|
||||
break;
|
||||
case 2: // back
|
||||
integral2 = 0.03;
|
||||
derivative2 = 1.5;
|
||||
speed_need = -1000;
|
||||
turn_need_r = 0;
|
||||
turn_need_l = 0;
|
||||
break;
|
||||
|
||||
case 'l': // turn left
|
||||
integral2 = 0.03;
|
||||
derivative2 = 1.5;
|
||||
speed_need = 0;
|
||||
turn_need_r = -500; //中心为轴 半径左转
|
||||
turn_need_l = 500;
|
||||
break;
|
||||
case 3: // turn left
|
||||
integral2 = 0.03;
|
||||
derivative2 = 1.5;
|
||||
speed_need = 0;
|
||||
turn_need_r = -500;
|
||||
turn_need_l = 500;
|
||||
break;
|
||||
|
||||
case 'r': // turn right
|
||||
integral2 = 0.03;
|
||||
derivative2 = 1.5;
|
||||
speed_need = 0;
|
||||
turn_need_r = 500;
|
||||
turn_need_l = -500;//中心为轴 半径右转
|
||||
break;
|
||||
case 4: // turn right
|
||||
integral2 = 0.03;
|
||||
derivative2 = 1.5;
|
||||
speed_need = 0;
|
||||
turn_need_r = 500;
|
||||
turn_need_l = -500;
|
||||
break;
|
||||
|
||||
case 's': // stop
|
||||
integral2 = 0.01;
|
||||
derivative2 = 1;
|
||||
speed_need = 0;
|
||||
turn_need_r = 0;
|
||||
turn_need_l = 0;
|
||||
break;
|
||||
case 5: // stop
|
||||
integral2 = 0.01;
|
||||
derivative2 = 1;
|
||||
speed_need = 0;
|
||||
turn_need_r = 0;
|
||||
turn_need_l = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void speed_filter(void)
|
||||
{
|
||||
u8 i;
|
||||
s32 speed_sum = 0;
|
||||
|
||||
for(i = 1 ; i < FILTER_COUNT ; i++)
|
||||
{
|
||||
speed_buf[i-1] = speed_buf[i]
|
||||
}
|
||||
|
||||
speed_buf[FILTER_COUNT - 1] = ((speed_l + speed_r) /2);
|
||||
|
||||
for(i = 0 ; i < FILTER_COUNT ; i++)
|
||||
{
|
||||
speed_sum += speed_buf[i];
|
||||
}
|
||||
|
||||
speed_out = (s16)(speed_sum / FILTER_COUNT);
|
||||
}
|
||||
|
||||
void pid(float angle , float angle_dot)
|
||||
{
|
||||
u32 temp;
|
||||
u16 sl,sr;
|
||||
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
|
||||
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;
|
||||
if(position > 60000) position = 60000;
|
||||
|
||||
rout = proportion * new_error + derivative * angle_dot - position * integral - derivarive2 * 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
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user