start

The omni-directional mobile chassis of smart car commonly includes omni-directional wheel chassis and mcnamu wheel chassis (hereinafter referred to as mcnamu wheel chassis). The following content only includes these two chassis.
Due to the special structure, the omni-directional wheel is divided into four wheels.
First, we decompose the omni-directional movement of the trolley into the speed in the direction of VX, vy and VZ. If you want to control the trolley, you only need to input the speed in the corresponding direction, synthesize it according to the output equation, and output the calculation results to the corresponding serial number motor.

definition

1. Define a xyx site coordinate system and an XYZ airframe coordinate system Initially, the centers of the two coordinate systems coincide and the included angle α Is 0

2. The counterclockwise rotation of the motor is positive.

3. The angle range of gyroscope is - 180-0-180

4. No matter what kind of chassis, the motor corresponding to the first wheel in the upper left corner is Motor1 by default, and the clockwise rotation is Motor1, Motor2, Motor3 and Motor4 in turn

Chassis layout

Fast calculation function

#define ONE_PI   (3.14159265)

float angle_to_radian = 0.01745f;//Angle to radian

double mx_sin(double rad)
{   
	double sine;
	if (rad < 0)
		sine = rad * (1.27323954f + 0.405284735f * rad);
	else
		sine = rad * (1.27323954f - 0.405284735f * rad);
	if (sine < 0)
		sine = sine*(-0.225f * (sine + 1) + 1);
	else
		sine = sine * (0.225f *( sine - 1) + 1);
	return sine;
}

double my_sin(double rad)
{
	s8 flag = 1;
	
	while(rad > 2*ONE_PI)
	{
		rad = rad -  2*ONE_PI;
	}
	if (rad >= ONE_PI)
	{
		rad -= ONE_PI;
		flag = -1;
	}

	return mx_sin(rad) * flag;
}

double my_cos(double rad)
{
	s8 flag = 1;
	rad += ONE_PI/2.0;
   
	while(rad > 2*ONE_PI)
	{
		rad = rad -  2*ONE_PI;
	}
	if (rad >= ONE_PI)
	{
		flag = -1;
		rad -= ONE_PI;
	}
	return my_sin(rad)*flag;
}

Main function

void Task_Auto_Run(unsigned short int motor_mode)
{
	float a,b,c,ang_motion=imu[z].Relative_Dist;
	unsigned short int moto_num,motor_num,axis_num;
		
	switch (motor_mode)
	{
		//Three wheel omnidirectional wheel
		case 1: a = (60 + ang_motion)*angle_to_radian; 
				b = (60 - ang_motion)*angle_to_radian; 
				c = (a - b)*0.5f;//c = (0 + ang_motion)*angle_to_radian;
				motor_num = 3;
				break;
		//Four wheel omnidirectional wheel
		case 2: a = (60 + ang_motion)*angle_to_radian; 
				b = (60 - ang_motion)*angle_to_radian; 
				c = (a - b)*0.5f;//c = (0 + ang_motion)*angle_to_radian;
				motor_num = 4;
				break;
		//Four wheel wheat wheel
		case 3: a = (45 - ang_motion)*angle_to_radian;
				b = (45 + ang_motion)*angle_to_radian;
				c = a;
				motor_num = 4;
				break;
	}
	
	//Synthetic formula
	speed[MOTOR1] = +my_cos(a)*motion[x].v + my_sin(a)*motion[y].v - motion[z].v;//1******2	 
	speed[MOTOR2] = +my_cos(b)*motion[x].v - my_sin(b)*motion[y].v - motion[z].v;//********
	speed[MOTOR3] = -my_cos(c)*motion[x].v - my_sin(c)*motion[y].v - motion[z].v;//********
	speed[MOTOR4] = -my_cos(b)*motion[x].v + my_sin(b)*motion[y].v - motion[z].v;//4******3	 	
}
//Detailed formula
//	/**************************************************Three wheel omnidirectional wheel*****************************************************/
//	speed[MOTOR1] = (+my_cos((60+ang_motion)*angle_to_radian)*motion[x].v + my_sin((60+ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//1********2	 /---------\
//	speed[MOTOR2] = (+my_cos((60-ang_motion)*angle_to_radian)*motion[x].v - my_sin((60-ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//**********
//	speed[MOTOR3] = (-my_cos((0 -ang_motion)*angle_to_radian)*motion[x].v - my_sin((0 -ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//****3****       -

//	/**************************************************Four wheel omnidirectional wheel*****************************************************/
//	speed[MOTOR1] = (+my_cos((60+ang_motion)*angle_to_radian)*motion[x].v + my_sin((60+ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//1******2	 /---------\
//	speed[MOTOR2] = (+my_cos((60-ang_motion)*angle_to_radian)*motion[x].v - my_sin((60-ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//********
//	speed[MOTOR3] = (-my_cos((60+ang_motion)*angle_to_radian)*motion[x].v - my_sin((60+ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//********
//	speed[MOTOR4] = (-my_cos((60-ang_motion)*angle_to_radian)*motion[x].v + my_sin((60-ang_motion)*angle_to_radian)*motion[y].v - motion[z].v);//4******3	 \---------/

//	/***************************************************Four wheel wheat wheel**********************************************************/
//	speed[MOTOR1] = (+my_cos((45-ang_motion)*angle_to_radian)*motion[x].v + my_sin((45-ang_motion)*angle_to_radian)*motion[y].v) - motion[z].v;//1******2	 \---------/
//	speed[MOTOR2] = (+my_cos((45+ang_motion)*angle_to_radian)*motion[x].v - my_sin((45+ang_motion)*angle_to_radian)*motion[y].v) - motion[z].v;//********
//	speed[MOTOR3] = (-my_cos((45-ang_motion)*angle_to_radian)*motion[x].v - my_sin((45-ang_motion)*angle_to_radian)*motion[y].v) - motion[z].v;//********
//	speed[MOTOR4] = (-my_cos((45+ang_motion)*angle_to_radian)*motion[x].v + my_sin((45+ang_motion)*angle_to_radian)*motion[y].v) - motion[z].v;//4******3	 /---------\	
	

/* end of motor_ctrl.c */

last

1.ang_motion=imu[z].Relative_Dist; // The z-axis angle of the gyroscope is assigned to participate in the solution of the equation of motion. (generally, the gyroscope can not be used, just assign ang_motion to 0)
2. When the z-axis data of gyroscope is assigned to participate in the operation, pay attention to the z-axis data direction, and the equation is positive clockwise by default. Use the gyroscope to unlock headless mode. The xy direction of the body coordinate system is only related to the initial direction when powered on. No matter how it rotates during the movement, the direction will not change. Refer to RM infantry small gyroscope, spinning and moving at the same time.

It is poorly written, there is time to make up the diagram, and the derivation process is cumbersome, and there are a lot of resources on the Internet. This paper will not elaborate. This is just a summary of the unintentional findings in the communication with the senior students.