# 1. 正解

## 解算

• 前轮只受线速度y和角速度控制影响，可得到 `Vf = y+z*L/2`
• 左轮收到x、y和角速度控制影响
Left Wheel

`Vl = -x*cos(30°)-y*sin(30°)+z*L/2`
• 右轮同样ru左轮可以得到：
`Vr = x*cos(30°)-y*sin(30°)+z*L/2`

# 2.逆解

## 解算

``````Vf = y + z*L/2
Vl = -x*cos(30°) - y*sin(30°) + z*L/2
Vr = x*cos(30°) - y*sin(30°) + z*L/2
``````

``````x = (-Vl + Vr) / (2 * cos(30°))
y = (2Vf - Vl - Vr) / 3
z = (Vf + Vl + Vr) * 2 / (3L)
``````

# 4.代码

`model.h` 模型接口类

``````#ifndef PIBOT_MODLE_H_
#define PIBOT_MODLE_H_

struct Odom{
float x;
float y;
float z;
float vel_x;
float vel_y;
float vel_z;
};

struct Model{
Model(){}

}

~Model(){}

//robot speed ------------> motor speed
virtual void motion_solver(float* robot_speed, float* motor_speed) = 0;

//motor speed-------------> robot speed
//interval  ms
virtual void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval) = 0;

protected:
};

#endif
``````

`differential.h` 差分轮解算实现

``````#ifndef PIBOT_DIFFERENTIAL_H_
#define PIBOT_DIFFERENTIAL_H_

#include "model.h"
#include "math.h"

#define MOTOR_COUNT 2

class Differential :    public Model{
public:
Differential() {}

void motion_solver(float* robot_speed, float* motor_speed){
//  robot_speed[0] x    robot_speed[1] y    robot_speed[2] z
}

void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval)
{
float dxy_ave = (-motor_dis[0] + motor_dis[1]) / 2.0;
float dth = (motor_dis[0] + motor_dis[1]) / (2* body_radius);
float vxy = 1000 * dxy_ave / interval;
float vth = 1000 * dth / interval;

odom->vel_x = vxy;
odom->vel_y = 0;
odom->vel_z = vth;
float dx = 0, dy = 0;
if (motor_dis[0] != motor_dis[1])
{
dx = cos(dth) * dxy_ave;
dy = -sin(dth) * dxy_ave;
odom->x += (cos(odom->z) * dx - sin(odom->z) * dy);
odom->y += (sin(odom->z) * dx + cos(odom->z) * dy);;
}

if (motor_dis[0] + motor_dis[1] != 0)
odom->z += dth;
}
};

#endif
``````

`omni3.h`三轮全向轮解算

``````#ifndef PIBOT_OMNI3_H_
#define PIBOT_OMNI3_H_

#include "model.h"
#include "math.h"

#define MOTOR_COUNT 3

#define _sqrt_of_3 1.73205f

class Omni3:public Model{
public:
Omni3() {}

void motion_solver(float* robot_speed, float* motor_speed){
//  robot_speed[0] x    robot_speed[1] y    robot_speed[2] z
}

void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval){
if (motor_dis[0]!=0 || motor_dis[1]!=0 || motor_dis[2]!=0){
//speed
double dvx = (-motor_dis[1]+motor_dis[2])*_sqrt_of_3/3.0f;
double dvy = (motor_dis[0]*2-motor_dis[1]-motor_dis[2])/3.0f;
double dvth = (motor_dis[0]+motor_dis[1]+motor_dis[2])/ (3 * body_radius);
odom->vel_x = dvx / dt;
odom->vel_y = dvy / dt;
odom->vel_z = dvth / dt;

//odometry
double dx = (-sin(th)*motor_dis[0]*2+(-_sqrt_of_3*cos(th)+sin(th))*motor_dis[1]+(_sqrt_of_3*cos(th)+sin(th))*motor_dis[2])/3.0f;
double dy = (cos(th)*motor_dis[0]*2+(-_sqrt_of_3*sin(th)-cos(th))*motor_dis[1]+(_sqrt_of_3*sin(th)-cos(th))*motor_dis[2])/3.0f;
double dth = (motor_dis[0]+motor_dis[1]+motor_dis[2])/ (3 * body_radius);

odom->x += dx;
odom->y += dy;
odom->z += dth;
}
}
};

#endif
``````

## 4.实现

``````    static Differential diff(wheel_diameter*0.5, wheel_track*0.5);
static Omni3 omni3(wheel_diameter*0.5, wheel_track*0.5);
Model* model = &omni3;
``````