forked from logzhan/RobotHardware-UESTC
49 lines
1.4 KiB
C++
49 lines
1.4 KiB
C++
#ifndef PIBOT_DIFFERENTIAL_H_
|
|
#define PIBOT_DIFFERENTIAL_H_
|
|
|
|
// 2轮差分模型
|
|
#include "model.h"
|
|
#include "math.h"
|
|
|
|
//定义Motor数目
|
|
#define MOTOR_COUNT 2
|
|
|
|
// 2轮差分模型接口实现
|
|
class Differential : public Model
|
|
{
|
|
public:
|
|
Differential() {}
|
|
Differential(float _wheel_radius, float _body_radius) : Model(_wheel_radius, _body_radius) {}
|
|
|
|
// 运动解算 把给到机器人的速度分解为各个轮子速度
|
|
virtual void motionSolver(float* robotSpeed, float* motorSpeed) {
|
|
// robot_speed[0] x robot_speed[1] y robot_speed[2] z
|
|
motorSpeed[0] = (-robotSpeed[0] + robotSpeed[2] * bodyRadius) / wheelRadius;
|
|
motorSpeed[1] = ( robotSpeed[0] + robotSpeed[2] * bodyRadius) / wheelRadius;
|
|
}
|
|
|
|
//反解算， 把各个轮子的速度转为机器人的速度 ，这里通过固定时间间隔转为里程
|
|
virtual 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* bodyRadius);
|
|
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
|