# Detailed inverse kinematics solution of omni-directional wheel and mcnamu wheel chassis (including motor output equation)

## 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

Figure 1 three wheel omnidirectional wheel layout

Fig. 2 four wheel omnidirectional wheel layout

Figure 3 layout of four wheel wheat wheel

## Fast calculation function

```#define ONE_PI   (3.14159265)

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

{
s8 flag = 1;

{
}
{
flag = -1;
}

}

{
s8 flag = 1;

{
}
{
flag = -1;
}
}
```

## 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;
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;
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;
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*****************************************************/

//	/**************************************************Four wheel omnidirectional wheel*****************************************************/

//	/***************************************************Four wheel wheat wheel**********************************************************/