全向轮：
参考视频：https://www.bilibili.com/video/BV1rX4y1p7n7

舵轮：
注意：解算图中各个轮子的w反了。
V1、V2、V3、V4为行进轮的速度，θ1、θ2、θ3、θ4为舵轮的转向角。知道了这八个参数，就可以确定车体的运动。

ifdef OMNI_3 //全向等边三轮

chassis.wheel[0].v = (speed_wantset.vx - speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
chassis.wheel[1].v = (-cos_60 * speed_wantset.vx + cos_30 * speed_wantset.vy - speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
chassis.wheel[2].v = (-cos_60 * speed_wantset.vx - cos_30 * speed_wantset.vy - speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);

endif

ifdef OMNI_4a //全向正方形四轮

chassis.wheel[0].v = (-cos_45 * speed_wantset.vx + cos_45 * speed_wantset.vy + speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
chassis.wheel[1].v = (-cos_45 * speed_wantset.vx - cos_45 * speed_wantset.vy + speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
chassis.wheel[2].v = (cos_45 * speed_wantset.vx - cos_45 * speed_wantset.vy + speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
chassis.wheel[3].v = (cos_45 * speed_wantset.vx + cos_45 * speed_wantset.vy + speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);

endif

ifdef OMNI_4b //全向长方形四轮

float m = 30, n = 20;
WheelSpeed[0] = (-cos_45 * speed_wantset.vx + cos_45 * speed_wantset.vy + (m + n) * speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
WheelSpeed[1] = (-cos_45 * speed_wantset.vx - cos_45 * speed_wantset.vy + (-m + n) * speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
WheelSpeed[2] = (cos_45 * speed_wantset.vx - cos_45 * speed_wantset.vy + (-m + -n) * speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
WheelSpeed[3] = (cos_45 * speed_wantset.vx + cos_45 * speed_wantset.vy + (m + -n) * speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);

endif

ifdef MECANUM //麦克纳姆轮

float m = 30, n = 20;
WheelSpeed[0] = (-1 * speed_wantset.vx + (-1) * speed_wantset.vy + (m + n) * speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
WheelSpeed[1] = (-1 * speed_wantset.vx + (-1) * speed_wantset.vy + (m + n) * speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
WheelSpeed[2] = (1 * speed_wantset.vx + (1) * speed_wantset.vy + (m + n) * speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);
WheelSpeed[3] = (1 * speed_wantset.vx + (1) * speed_wantset.vy + (m + n) * speed_wantset.vw * chassis.omni_wheel2center * PI / 180.f);

endif

ifdef STEER_3

//应该都是正号，廖叔没有改电机正方向
int i = 0;
float add_angle;
chassis.wheel[0].vx = -speed_wantset.vx - speed_wantset.vw * chassis.R_1 * PI / 180.f;
chassis.wheel[0].vy = -speed_wantset.vy;
chassis.wheel[0].v = sqrt(chassis.wheel[0].vx * chassis.wheel[0].vx + chassis.wheel[0].vy * chassis.wheel[0].vy);

chassis.wheel[1].vx = -speed_wantset.vx + speed_wantset.vw * chassis.width_23 * PI / 180.f;
chassis.wheel[1].vy = -speed_wantset.vy - speed_wantset.vw * chassis.length_23 * PI / 180.f;
chassis.wheel[1].v = sqrt(chassis.wheel[1].vx * chassis.wheel[1].vx + chassis.wheel[1].vy * chassis.wheel[1].vy);

chassis.wheel[2].vx = -speed_wantset.vx + speed_wantset.vw * chassis.width_23 * PI / 180.f;
chassis.wheel[2].vy = -speed_wantset.vy + speed_wantset.vw * chassis.length_23 * PI / 180.f;
chassis.wheel[2].v = sqrt(chassis.wheel[2].vx * chassis.wheel[2].vx + chassis.wheel[2].vy * chassis.wheel[2].vy);