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; // left
p[2] = 3.2; //right
p[3] = 10; //behind
PID_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,2000
MOVE.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; // left
p[2] = 3.2; //right
p[3] = 10; //behind
PID_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:感谢实验室的学长