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

	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
}

标签：rotor,remote,函数,pid,chassis,motor,output,RM,运动
来源： https://blog.csdn.net/yunddun/article/details/120412493