F405版麦轮小车源码库函数版本控制流程

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);

1. 调用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;
    ...

2. 调用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);
}

3. 使用增量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;        //增量输出
}

4. 进行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;	
}