RM-2个函数实现全向运动

最新推荐文章于 2025-06-01 18:46:25 发布

Xuan-ZY

最新推荐文章于 2025-06-01 18:46:25 发布

阅读量293

点赞数

CC 4.0 BY-SA版权

文章标签： stm32

本文链接：https://blog.youkuaiyun.com/yunddun/article/details/120412493

本文介绍了一种用于底盘电机的全向运动控制算法。该算法通过计算电机角度及速度来实现底盘精确的方位调整与移动。核心部分包括角度补偿、速度矢量分解以及PID控制等关键技术。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

	chassis_enable();//底盘电机使能	
	chassis_move_remote();//电机全向运动算法

void chassis_move_remote(void)
{
rotor_Angle=(-moto_chassis[4].angle+Bchang)*360/8192;//开机时由于装配导致的补偿角度就可以通过减去Bchang值得到0，所以此时底盘朝前	
Vx=cos(rotor_Angle/180*My_Pi)*(remote_control.ch3*rotor_V/660)+sin(rotor_Angle/180*My_Pi)*(remote_control.ch4*rotor_V/660);
Vy=cos(rotor_Angle/180*My_Pi)*(remote_control.ch4*rotor_V/660)-sin(rotor_Angle/180*My_Pi)*(remote_control.ch3*rotor_V/660); 

}

void  chassis_enable(void)
{
			ch[0]= -Vy+Vx+Vz;
			ch[1]= -Vy-Vx+Vz;//vz小于0， 车子逆时针转动
			ch[2]=  Vy-Vx+Vz;
			ch[3]=  Vy+Vx+Vz;
			ch[4]=(float)(remote_control.ch1)*4096/660;


				//四个电机3506
	   for(int i=0; i<4; i++)
    {
      motor_pid[i].target = ch[i]; 																							
      motor_pid[i].f_cal_pid(&motor_pid[i],moto_chassis[i].speed_rpm);    //3506 moto_chassis[0].speed_rpm
    }
		//201 202 203 204
set_moto_current(&hcan1,motor_pid[0].output,motor_pid[1].output,motor_pid[2].output,motor_pid[3].output);//3506
 set_motor_voltage(&hcan1,0, motor_pid[4].output,motor_pid[5].output,motor_pid[6].output,0);//6020   205
}