89 lines
2.8 KiB
C++
89 lines
2.8 KiB
C++
#include "Kinematics.h"
|
|
|
|
// 设置轮子的间距
|
|
void Kinematics::set_wheel_distance(float distance)
|
|
{
|
|
wheel_distance = distance;
|
|
}
|
|
|
|
void Kinematics::set_motor_param(uint8_t id, float per_pluse_distance)
|
|
{
|
|
motor_param[id].per_pulse_distance = per_pluse_distance;
|
|
}
|
|
|
|
|
|
// 运动学正解,将左右轮的速度转换成线速度和角速度
|
|
void Kinematics::kinematics_forward(float left_speed, float right_speed, float *out_linear_speed, float *out_angular_speed)
|
|
{
|
|
*out_linear_speed = (left_speed + right_speed) / 2;
|
|
*out_angular_speed = (right_speed - left_speed) / wheel_distance;
|
|
}
|
|
|
|
|
|
// 运动学逆解，将线速度和角速度转换成左右轮的速度
|
|
void Kinematics::kinematics_inverse(float linear_speed, float angular_speed, float *out_left_speed, float *out_right_speed)
|
|
{
|
|
*out_left_speed = linear_speed - angular_speed * wheel_distance / 2;
|
|
*out_right_speed = linear_speed + angular_speed * wheel_distance / 2;
|
|
}
|
|
|
|
// 输入：左右轮脉冲数，当前时间，输出更新电机速度和编码器数据
|
|
void Kinematics::update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick)
|
|
{
|
|
int16_t dt = current_time - last_update_time; // ms
|
|
delta_ticks[0] = left_tick - motor_param[0].last_encoder_ticks;
|
|
delta_ticks[1] = right_tick - motor_param[1].last_encoder_ticks;
|
|
|
|
motor_param[0].motor_speed = (delta_ticks[0] * 105.805) / dt;
|
|
motor_param[1].motor_speed = (delta_ticks[1] * 105.805) / dt;
|
|
|
|
// 为了下次还可以正常的计算速度
|
|
motor_param[0].last_encoder_ticks = left_tick;
|
|
motor_param[1].last_encoder_ticks = right_tick;
|
|
last_update_time = current_time;
|
|
|
|
update_odom(dt);
|
|
}
|
|
// 获取电机速度,返回值是速度
|
|
int16_t Kinematics::get_motor_speed(uint8_t id)
|
|
{
|
|
if (id < 0 || id > 1)
|
|
{
|
|
return -1;
|
|
}
|
|
return motor_param[id].motor_speed;
|
|
}
|
|
|
|
odom_t& Kinematics::get_odom()
|
|
{
|
|
return odom;
|
|
}
|
|
|
|
void Kinematics::TransAngleInPI(float angle,float& out_angle)
|
|
{
|
|
if(angle>PI)
|
|
{
|
|
out_angle -= 2*PI;
|
|
}else if (angle<-PI)
|
|
{
|
|
out_angle += 2*PI;
|
|
}
|
|
}
|
|
|
|
void Kinematics::update_odom(uint16_t dt)
|
|
{
|
|
float dt_s = float(dt)/1000.0; // ms -> s
|
|
// 获取实时的角速度和线速度呢？我们拿左右轮实时的速度，进行运动学正解
|
|
this->kinematics_forward(motor_param[0].motor_speed,motor_param[1].motor_speed,&odom.linear_speed,&odom.angular_speed);
|
|
// 计算里程计信息
|
|
odom.linear_speed = odom.linear_speed/1000.0; // 转换成米每秒
|
|
|
|
// 角度积分
|
|
odom.angle += odom.angular_speed*dt_s;
|
|
TransAngleInPI(odom.angle,odom.angle);
|
|
// 计算机器人行走的距离（沿自身前进方向的）
|
|
float delta_distance = odom.linear_speed * dt_s;
|
|
// 分解到X轴和Y轴
|
|
odom.x += delta_distance * std::cos(odom.angle);
|
|
odom.y += delta_distance * std::sin(odom.angle);
|
|
} |