- PID
首先想要驱动电机按照我们所前往的方向、所需要的速度旋转,少不得的就是PID算法。
我在这次编写代码的过程中所运动到的是串级PID:外环为位置环、内环则是速度环。
首先是PID的初始化,外环的位置环,因为运用到的是位置PID,位置PID在Integral项运动的是误差的积分,所以需要抗饱和,在此处加上了一个最值限幅,两种方式的PID在最后的输出处我都用上了限幅,防止炸电机。(为了代码的美观性,我也是在开头define了一个比例系数[狗头])
typedef struct
{
float kp;
float ki;
float kd;
float error;
float last_error;
float pre_error;
float out;
float max_integral;//积分限幅(抗饱和)
float max_out; //输出限幅
}pid;
typedef struct
{
pid inner;
pid outer;
}cascade;
#define coefficient 2.0f
//内环PID初始化
void PID_Speed_Init(cascade *cascade,float p,float i,float d,float maxout)
{
cascade->inner.kp = p;
cascade->inner.ki = i;
cascade->inner.kd = d;
cascade->inner.max_out = maxout;
cascade->inner.pre_error = 0;
cascade->inner.last_error = 0;
cascade->inner.error = 0;
}
//外环PID初始化
void PID_Location_Init(cascade *cascade,float p,float i,float d,float maxintegral,float maxout)
{
cascade->outer.kp = p;
cascade->outer.ki = i;
cascade->outer.kd = d;
cascade->outer.max_integral = maxintegral;
cascade->outer.max_out = maxout;
cascade->outer.last_error = 0;
cascade->outer.error = 0;
}
下面就是PID的一个计算过程,也是非常的常见,PID最艰难的并不是什么编写代码什么的,而是和卡尔曼滤波等一样的调参[哭泣]
//位置式pid (外环--位置环)
void Outer_PID(cascade *cascade,float target,float feedback)
{
float proportional,integral,differential;
cascade->outer.last_error = cascade->outer.error;
cascade->outer.error = target - feedback;
proportional = cascade->outer.kp * cascade->outer.error;
integral += cascade->outer.ki * cascade->outer.error;
differential = cascade->outer.kd * (cascade->outer.error - cascade->outer.last_error);
if(integral > cascade->outer.max_integral)
{
integral = cascade->outer.max_integral;
}else if(integral < -cascade->outer.max_integral)
{
integral = -cascade->outer.max_integral;
}
cascade->outer.out = proportional + integral + differential;
if(cascade->outer.out > cascade->outer.max_out)
{
cascade->outer.out = cascade->outer.max_out;
}else if(cascade->outer.out < -cascade->outer.max_out)
{
cascade->outer.out = -cascade->outer.max_out;
}
}
//增量式PID (内环--速度环)
void Inner_PID(cascade *cascade,float target,float feedback)
{
float proportional,integral,differential;
cascade->inner.pre_error = cascade->inner.last_error;
cascade->inner.last_error = cascade->inner.error;
cascade->inner.error = target - feedback;
proportional = cascade->inner.kp * (cascade->inner.error - cascade->inner.last_error);
integral = cascade->inner.ki * cascade->inner.error;
differential = cascade->inner.kd * (cascade->inner.kd - coefficient * cascade->inner.last_error + cascade->inner.pre_error);
cascade->inner.out += proportional + integral + differential;
if(cascade->inner.out > cascade->inner.max_out)
{
cascade->inner.out = cascade->inner.max_out;
}else if(cascade->inner.out < -cascade->inner.max_out)
{
cascade->inner.out = -cascade->inner.max_out;
}
}
- 编码器
下面就是通过电机的编码器,来读出电机的一些基本属性。博主在这里并没有直接运用编码器,而是通过捕获电机的高低电平变化来计算出高电平时间,然后通过IO口读取编码器B相的电平状态来判断毛刺,避免误差,因为博主编写的是小车四个轮子即四个电机的代码,所以在此处就放上其中一个电机的代码以作参考,首先是一些参数的定义
uint8_t count[4] = {0,0,0,0}; //判断条件
uint32_t Cap_Buf_Before[4]; //开始时间
uint32_t Cap_Buf_Later[4]; //结束时间
uint8_t Cap_Lev_Before[4]; //捕获上升沿时B相电平
uint8_t Cap_Lev_Later[4]; //捕获下降沿时B相电平
int direction[4]; //判断车轮正转还是反转
uint32_t High_Level_Time[4]; //高电平时间
uint8_t Filter_Flag[4] = {0,0,0,0}; //平均值标志位
uint32_t Real_Time[4]; //平均高电平时间
int Forward_Round[4] = {0,0,0,0}; //正转圈数
uint16_t All_Round[4] = {0,0,0,0}; //总圈数
int Sum_F_direction[4] = {0,0,0,0}; //正转总和
uint16_t Sum_A_direction[4] = {0,0,0,0}; //总和
#define W_lu 0
#define rise_edag 0
#define fall_edge 1
#define forward 1
#define reverse -1
#define Per_Round 390
#define ARR 65535
#define filter 8 - 1 //均值滤波采样次数
typedef struct
{
TIM_HandleTypeDef TIM; //定时器句柄
GPIO_TypeDef* R_GPIO_B; //B相
uint16_t Read_GPIO; //B相IO口
uint32_t Channel; //捕获通道
uint32_t ActiveChannel; //占线通道
}wheel;
然后是电机的初始化
//左上(前)轮
wheel_lu.TIM = htim5;
wheel_lu.R_GPIO_B = GPIOC;
wheel_lu.Read_GPIO = GPIO_PIN_6;
wheel_lu.Channel = TIM_CHANNEL_1;
wheel_lu.ActiveChannel = HAL_TIM_ACTIVE_CHANNEL_1;
最后就是关于电机的代码,博主在最后计算出来的高电平时间采用了一个平均值的计算,所以不可避免地导致最后在VOFA上数值波形的滞后性,大家可以按照自己的实际情况进行取舍
//左上(前)轮
if((htim->Instance == wheel_lu.TIM.Instance) && (htim->Channel == wheel_lu.ActiveChannel))
{
switch(count[W_lu])
{
case rise_edag: //捕获到上升沿
Cap_Buf_Before[W_lu] = HAL_TIM_ReadCapturedValue(&(wheel_lu.TIM),wheel_lu.Channel);
__HAL_TIM_SET_CAPTUREPOLARITY(&(wheel_lu.TIM),wheel_lu.Channel ,TIM_INPUTCHANNELPOLARITY_FALLING); //设置为下降沿捕获
Cap_Lev_Before[W_lu] = HAL_GPIO_ReadPin(wheel_lu.R_GPIO_B,wheel_lu.Read_GPIO); //判断电平高低
if(Cap_Lev_Before[W_lu] == RESET) //判断正转or反转
{
direction[W_lu] = reverse;
}
else
{
direction[W_lu] = forward;
}
count[W_lu] = fall_edge;
break;
case fall_edge: //捕获到下降沿,关闭捕获功能
Cap_Buf_Later[W_lu] = HAL_TIM_ReadCapturedValue(&(wheel_lu.TIM),wheel_lu.Channel);
HAL_TIM_IC_Stop_IT(&(wheel_lu.TIM),wheel_lu.Channel);
Cap_Lev_Later[W_lu] = HAL_GPIO_ReadPin(wheel_lu.R_GPIO_B,wheel_lu.Read_GPIO);
if(Cap_Lev_Later[W_lu] != Cap_Lev_Before[W_lu]) //判断毛刺
{
if(Cap_Buf_Later[W_lu] > Cap_Buf_Before[W_lu]) //溢出计算
{
High_Level_Time[W_lu] = Cap_Buf_Later[W_lu] - Cap_Buf_Before[W_lu];
}
else
{
High_Level_Time[W_lu] = ARR + 1 - Cap_Buf_Before[W_lu] + Cap_Buf_Later[W_lu];
}
}
if(Filter_Flag[W_lu] != filter) //取八次求平均值(导致数据滞后)
{
if(Filter_Flag[W_lu] == 0)
{
Real_Time[W_lu] = 0;
}
Filter_Flag[W_lu] ++;
Real_Time[W_lu] += High_Level_Time[W_lu];
}
else
{
Real_Time[W_lu] += High_Level_Time[W_lu];
Real_Time[W_lu] /= filter + 1;
Filter_Flag[W_lu] = 0;
}
Sum_F_direction[W_lu] += direction[W_lu];
if(Sum_F_direction[W_lu] >= Per_Round) //计算圈数
{
Forward_Round[W_lu]++;
}
else if(Sum_F_direction[W_lu] <= -Per_Round)
{
Forward_Round[W_lu]--;
}
if(direction[W_lu] == reverse)
{
direction[W_lu] = -direction[W_lu];
}
Sum_F_direction[W_lu] += direction[W_lu];
if(Sum_F_direction[W_lu] >= Per_Round)
{
All_Round[W_lu]++;
}
HAL_TIM_IC_Start_IT(&(wheel_lu.TIM),wheel_lu.Channel); //重新开启捕获并设置为上升沿
__HAL_TIM_SET_CAPTUREPOLARITY(&(wheel_lu.TIM),wheel_lu.Channel,TIM_INPUTCHANNELPOLARITY_RISING);
count[W_lu] = rise_edag;
}
}
这篇文章也存在着许多的不足,希望读者可以多加改正。