STM32三轮全向底盘

最近在实验室培训，玩过麦克纳姆四轮底盘后，玩三轮全向底盘也有一段时间了，于是来分享一下自己的心得。

附图
这是去年参加robocon的底盘，三个大疆3508电机，一个全场定位模块，一个实验室焊的遥控器[^1]: （怕出问题所以没用DEVO10）。

1.底盘公式

三轮全向底盘论坛上已经有很多的前辈写了很多好的文章，自己就不再重复论述了。

附上一篇前辈的博文：https://blog.csdn.net/qq_40696002/article/details/107856908

同样附上视频链接：https://v.youku.com/v_show/id_XMjY2NTIxMjUyNA==.html?qq-pf-to=pcqq.c2c

链接的视频我个人觉得是非常好的，我当初看很多博客都看不懂，是看了这个视频才懂的。

关于三轮底盘的功能，特殊性在上视频中都有详细的阐述，唯一注意的是电机的序号。

2.程序实现手动模式

1.总体思路：遥控拨杆发送底盘整体的速度——通过底盘公式分解出个个电机的速度——各电机的速度通过PID计算发送各电机。
2.遥控器部分：通过CAN发送拨杆的通道值各底盘公式。

#define  w1                 (3.14159f/3.0f)               /*60度弧度*/
#define  w2                 (IMU.REAL.A*3.1415926f/180.0f)/*全场定位输出弧度*/ for (motor_num=1;motor_num<4;motor_num++) //清除程序之前的PID参数{PID_SPEED[motor_num].I_OUT = 0;PID_SPEED[motor_num].OUT = 0;}speed[1]   = (-my_cos(w1 + w2)*RC.CH3*1.5f + my_sin(w1 + w2)*RC.CH4*2.5f - RC.CH1 )*2.0f; //*1***********2*/speed[2]   = (-my_cos(w1 - w2)*RC.CH3*1.5f - my_sin(w1 - w2)*RC.CH4*2.5f - RC.CH1 )*2.0f;speed[3]   = (+my_cos(w2     )*RC.CH3*1.5f - my_sin(w2     )*RC.CH4*2.5f - RC.CH1 )*2.0f;       //3}

如没有传感器计算角度，则 w2 = 0。

3.PID计算电机速度

  p[1] = 3.4;       // leftp[2] = 3.2; //rightp[3] = 10;  //behindPID_Cal( &PID_SPEED[1], MOTOR_FEEDBACK[1].speed_rpm, speed[1], p[1], 5000);PID_Cal( &PID_SPEED[2], MOTOR_FEEDBACK[2].speed_rpm, speed[2], p[2], 5000);PID_Cal( &PID_SPEED[3], MOTOR_FEEDBACK[3].speed_rpm, speed[3], p[3], 9000); //3号速度>1号2号Motor_Set_Current(PID_SPEED[1].OUT,PID_SPEED[2].OUT,PID_SPEED[3].OUT,0);

p[ ] 是PID的比例系数，我自己微调了一下P目的是让电机跑的更直。
关于大疆电机的使用方法可以看我之前的博客。

3.程序实现自动模式

1.总体思路：位置式PID通过距离先计算出底盘整体的移动速度——通过底盘公式分解出个个电机的速度——各电机的速度通过PID计算发送各电机。

位置式PID计算底盘的移动速度
关于位置式PID的程序在论坛上同样有很多，我自己弄的也不是明明白白，就不献丑了。

void MOVE_SET(float goal_X,float goal_Y,float goal_A,unsigned short int mode) //(x,y,angle.mode)
{i = mode;MOVE.GOAL_A = goal_A;MOVE.GOAL_Y = goal_Y;MOVE.GOAL_X = goal_X;
}
MOVE_SET(0,8000,0,0); //通过自己写的MOVE——SET程序发送坐标给全场定位。if(MOVE.Mode==0)//Y是主轴,X是辅助轴:X,Y都是位置式PID; 在通过位置PID计算出底盘在X.Y轴的速度
{MOVE.VX = PID_Cal(&VXPID,X_LONG,MOVE.GOAL_X,3,3000);//4,2000MOVE.VY = PID_Cal(&VYPID,Y_LONG,MOVE.GOAL_Y,3,5000);//（8.2000）MOVE.VZ = PID_Cal(&VAPID,IMU.REAL.A,MOVE.GOAL_A,65,1500);//110,2500
}

3.底盘公式计算电机速度

speed[1]   = ( my_cos((60+IMU.REAL.A)*Angle_To_Rad)*MOVE.VX + my_sin((60+IMU.REAL.A)*Angle_To_Rad)*MOVE.VY - MOVE.VZ );speed[2]   = ( my_cos((60-IMU.REAL.A)*Angle_To_Rad)*MOVE.VX - my_sin((60-IMU.REAL.A)*Angle_To_Rad)*MOVE.VY - MOVE.VZ );speed[3]   = (-my_cos(    IMU.REAL.A *Angle_To_Rad)*MOVE.VX - my_sin(    IMU.REAL.A *Angle_To_Rad)*MOVE.VY - MOVE.VZ );

4.PID速度环计算各电机速度

 V_Limit();      /* 限  速 */p[1] = 3.4;      // leftp[2] = 3.2; //rightp[3] = 10;  //behindPID_Cal( &PID_SPEED[1], MOTOR_FEEDBACK[1].speed_rpm, speed[1], p[1], 5000);PID_Cal( &PID_SPEED[2], MOTOR_FEEDBACK[2].speed_rpm, speed[2], p[2], 5000);PID_Cal( &PID_SPEED[3], MOTOR_FEEDBACK[3].speed_rpm, speed[3], p[3], 9000); //3号速度>1号2号Motor_Set_Current(PID_SPEED[1].OUT,PID_SPEED[2].OUT,PID_SPEED[3].OUT,0);

4.底盘自动跑点

1.思路：通过位置式PID定坐标，再通过全场定位模块反馈底盘实际坐标来实现自动跑点。

if(step == 1){MOVE_SET(0,8000,0,0);//      MOVE_SET(Profile_SET(R0.X0,0),R1.Y0,0,2);       //跑曲线       if(Y_LONG >= 7975){step = 2;}}else if( step == 2){       MOVE_SET(-4000,8000,0,10);if( X_LONG <= -3950){step = 3;}}

上文若有所错误，敬请指出，共同学习，共同进步。
ps：感谢实验室的学长