F405版麦轮小车源码库函数版本
本文档从小车APP控制小车移动的功能未切入口,分析了小车从接收控制信号,计算目标速度,增量式离散PID闭环控制,到PWM增量输出控制电机的流程。
代码架构
小车控制代码control.c
在计时器的PWM 模式,即脉冲宽度调制模式,可以生成一个信号,该信号频率由 TIMx_ARR 寄存器值决定,其占空比则由 TIMx_CCRx 寄存器值决定。
此项目中对小车四路电机的控制,最终归结于对TIM8_CCRx进行写入,
#define PWMD TIM8->CCR4
#define PWMC TIM8->CCR3
#define PWMB TIM8->CCR2
#define PWMA TIM8->CCR1
MPU6O5O的INT引脚中断引脚每5ms产生一次中断,小车控制电机的代码都在中断服务函数
int EXTI15_10_IRQHandler(void)
中,可以理解为每5ms控制一次小车。此函数步骤如下
读取四路电机解码器的计数
//为了保证M法测速的时间基准,5ms控制一次,读取编码器计数并清零
Encoder_A = (short)TIM2 -> CNT; TIM2 -> CNT=0;
Encoder_B = (short)TIM3 -> CNT; TIM3 -> CNT=0;
Encoder_C = -(short)TIM4 -> CNT; TIM4 -> CNT=0;
Encoder_D = -(short)TIM5 -> CNT; TIM5 -> CNT=0;
读取MPU6050内置DMP的姿态信息,更新姿态。
Read_DMP()
函数。与显示屏输出小车状态有关,不涉及运动控制。
验证电压不存在异常。
当电压过低,使能开关为OFF,关闭电机。
解算运动参数赋值给PWM寄存器 :
//获取三轴角度和XYZ轴目标速度Move_X,Y,Z
Get_RC();
//得到控制目标值Target_A,B,C,D,进行运动学分析
Kinematic_Analysis(Move_X,Move_Y,Move_Z);
//速度闭环控制计算电机A,B,C,D最终PWM
Motor_A=Incremental_PI(Encoder_A,Target_A);
Motor_B=Incremental_PI(Encoder_B,Target_B);
Motor_C=Incremental_PI(Encoder_C,Target_C);
Motor_D=Incremental_PI(Encoder_D,Target_D);
//PWM限幅
Xianfu_Pwm(6900);
//赋值给PWM寄存器
Set_Pwm(Motor_A,Motor_B,Motor_C,Motor_D);
- 调用
Get_RC()
获取三轴角度和XYZ轴目标速度Move_X,Move_Y,Move_Z
。全局变量Flag_Direction
在usart.c
依据串口接收到的信号确定。
void Get_RC(void)
{
...
float step=0.5; //设置速度控制步进值。
static float Bias,Last_Bias; //偏差和历史值
u8 Flag_Move=1;
...
switch(Flag_Direction) //方向控制
{
case 1: Move_X=0; Move_Y+=step; break;
case 2: Move_X+=step; Move_Y+=step; break;
case 3: Move_X+=step; Move_Y=0; break;
case 4: Move_X+=step; Move_Y-=step; break;
case 5: Move_X=0; Move_Y-=step; break;
case 6: Move_X-=step; Move_Y-=step; break;
case 7: Move_X-=step; Move_Y=0; break;
case 8: Move_X-=step; Move_Y+=step; break;
default:
Flag_Move=0; Move_X=Move_X/1.05f; Move_Y=Move_Y/1.05f;
break;
}
if(Flag_Move==0) //如果无方向控制指令,检查转向控制状态
{
if(Flag_Left==1) Move_Z-=step,Gyro_K=0; //左自旋
else if(Flag_Right==1) Move_Z+=step,Gyro_K=0; //右自旋
else Move_Z=0,Gyro_K=0.004; //停止
}
else Flag_Left=0,Flag_Right=0,Move_Z=0;
...
- 调用
Kinematic_Analysis()
,进行运动学分析,得到控制目标值Target_A,B,C,D
void Kinematic_Analysis(float Vx,float Vy,float Vz)
{
Target_A = -Vx+Vy-Vz*(a_PARAMETER+b_PARAMETER);
Target_B = +Vx+Vy-Vz*(a_PARAMETER+b_PARAMETER);
Target_C = -Vx+Vy+Vz*(a_PARAMETER+b_PARAMETER);
Target_D = +Vx+Vy+Vz*(a_PARAMETER+b_PARAMETER);
}
- 使用增量PI控制器,根据编码器测量值,目标速度解算电机PWM。
int Incremental_PI_A(int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Encoder-Target; //计算偏差
//增量式PI控制器
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
- 进行PWM限幅,然后赋值给PWM寄存器。
void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d)
{
int siqu=0;
if(motor_a>0) PWMA=motor_a+siqu, INA=0;
else PWMA=7200+motor_a-siqu,INA=1;
if(motor_b>0) PWMB=7200-motor_b-siqu,INB=1;
else PWMB=-motor_b+siqu,INB=0;
if(motor_c>0) PWMC=7200-motor_c-siqu,INC=1;
else PWMC=-motor_c+siqu,INC=0;
if(motor_d>0) PWMD=motor_d+siqu,IND=0;
else PWMD=7200+motor_d-siqu,IND=1;
}
APP串口通信控制小车usart.c
该小车的APP控制用的是串口2,波特率设置为9600,其控制指令在串口 2接收中断服务函数中。
int USART2_IRQHandler(void)
{
//未接收到数据
if(USART_GetITStatus(USART2, USART_IT_RXNE) == RESET)
return 0;
static u8 Flag_PID,i,j,Receive[50];
static float Data;
Usart2_Receive=USART2->DR;
if(Usart2_Receive==0x4B)
Turn_Flag=1; //进入转向控制界面,实现原地旋转
else if(Usart2_Receive==0x49||Usart2_Receive==0x4A)
Turn_Flag=0; //进入方向控制界面,实现8方向的运动
if(Run_Flag==0)//Run_Flag初始化为0,未出现修改的代码
{
if(Turn_Flag==0)//如果处于速度方向控制模式
{
if(Usart2_Receive>=0x41&&Usart2_Receive<=0x48)
Flag_Direction=Usart2_Receive-0x40;
else if(Usart2_Receive<=8)
Flag_Direction=Usart2_Receive;
else Flag_Direction=0;
}
else if(Turn_Flag==1)//如果处于转向控制界面
{
if(Usart2_Receive==0x43) Flag_Left=0,Flag_Right=1;
else if(Usart2_Receive==0x47) Flag_Left=1,Flag_Right=0;
else Flag_Left=0,Flag_Right=0;
if(Usart2_Receive==0x41||Usart2_Receive==0x45)
Flag_Direction=Usart2_Receive-0x40;
else Flag_Direction=0;
}
}
if(Usart2_Receive==0x58)
RC_Velocity=RC_Velocity+10;//加速按键,加速10
if(Usart2_Receive==0x59)
RC_Velocity=RC_Velocity-10;//减速按键,减速10
if(RC_Velocity>70) RC_Velocity=70;
if(RC_Velocity<10) RC_Velocity=10; //设定速度阈值
...
return 0;
}