麦克纳姆轮控制

HeDouet

于 2024-08-13 20:18:34 发布

阅读量325

点赞数 9

文章标签：硬件架构

本文链接：https://blog.csdn.net/HeDouet/article/details/141172614

版权

如下是麦克纳姆轮工作机制，使用全力矩控制，附pid

把每一个轮子拿出来都是一个承受三个方向上的力的单一物体，根据力矩平衡，四个不同方向上的力会形成四轮平衡，以移动

不懂的话没关系

后附控制代码

void Chassis_Ctrl(void)
{



   f_ang=DOUBLE_ECHO.GM_UP_ECHO.yaw_curangle-ORIN_ANGLE;

cha_goal_angle=f_ang;

   if(RC_CtrlData.rc.s2==2)cha_goal_angle+=800;

           wv=(cha_goal_angle)*0.3;

   r_vx=RC_CtrlData.rc.ch0;
   r_vy=RC_CtrlData.rc.ch1;

   vx=(r_vx*cos(f_ang*PI/180.f)-r_vy*sin(f_ang*PI/180.f)) / 220;
           vy=(r_vy*cos(f_ang*PI/180.f)+r_vx*sin(f_ang*PI/180.f)) / 220;

           vw2 = -(+vx - vy - wv * MOTOR_TO_CENTER) / (WHEEL_RADIUS * 2 * PI) * REDUCE_RATIO * 60;
           vw3 = -(+vx + vy - wv * MOTOR_TO_CENTER) / (WHEEL_RADIUS * 2 * PI) * REDUCE_RATIO * 60;
           vw4 = (+vx - vy + wv * MOTOR_TO_CENTER) / (WHEEL_RADIUS * 2 * PI) * REDUCE_RATIO * 60;
           vw1 = (+vx + vy + wv * MOTOR_TO_CENTER) / (WHEEL_RADIUS * 2 * PI) * REDUCE_RATIO * 60;

   if(vw1>=1800)vw1=1800;
           if(vw2>=1800)vw2=1800;
           if(vw3>=1800)vw3=1800;
           if(vw4>=1800)vw4=1800;

           if(vw1<=-1800)vw1=-1800;
           if(vw2<=-1800)vw2=-1800;
           if(vw3<=-1800)vw3=-1800;
           if(vw4<=-1800)vw4=-1800;

GUARD_CHASSIS_MOTOR.data.output_current1= pidProcess(&hpid3508, vw1, m3508.m1.speed);
GUARD_CHASSIS_MOTOR.data.output_current2 = pidProcess(&hpid3508, vw2, m3508.m2.speed);
GUARD_CHASSIS_MOTOR.data.output_current3 = pidProcess(&hpid3508, vw3, m3508.m3.speed);
GUARD_CHASSIS_MOTOR.data.output_current4 = pidProcess(&hpid3508, vw4, m3508.m4.speed);

   //通过can来控制电机
ctrl.pack = M3508_PACK_1_4;
ctrl.setcur[0] = (int16_t)GUARD_CHASSIS_MOTOR.data.output_current1;
ctrl.setcur[1] = (int16_t)GUARD_CHASSIS_MOTOR.data.output_current2;
ctrl.setcur[2] = (int16_t)GUARD_CHASSIS_MOTOR.data.output_current3;
ctrl.setcur[3] = (int16_t)GUARD_CHASSIS_MOTOR.data.output_current4;
m3508SetCurrent(&ctrl, &CAN1Handle);

       vw1=vw2=vw3=vw4=0;
       control_on++;

}