feat: 更新第九章

chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.cpp

@@ -0,0 +1,89 @@
+#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);
+}

chapt9/fishbot_motion_control/lib/Kinematics/Kinematics.h

@@ -0,0 +1,57 @@
+#ifndef __KINEMATICS_H__
+#define __KINEMATICS_H__
+
+#include "Arduino.h"
+
+typedef struct
+{
+    float per_pulse_distance;   // 每个脉冲的前进的距离
+    int16_t motor_speed;        // 单位用mm/s
+    int64_t last_encoder_ticks; // 上一次点击的编码器读数
+} motor_param_t;
+
+
+typedef struct {
+    float x;
+    float y;
+    float angle;
+    float linear_speed;
+    float angular_speed;
+}odom_t;
+
+/**
+ * 1. 运动学正逆解（两个轮子的实时速度->当前实时的角速度和线速度 / 当前目标的角速度和线速度->两个轮子的目标速度）
+ *
+ */
+class Kinematics
+{
+private:
+    /* data */
+    motor_param_t motor_param[2];
+    int16_t delta_ticks[2] = {0, 0}; // 用于存储上一次读取的编码器数值
+    uint64_t last_update_time = 0; // 用于存储上一次更新电机速度的时间,计算速度的时候使用
+    float wheel_distance = 0.0;    // 两个轮子之间的距离
+    odom_t odom; // 用于存储里程计信息
+public:
+    Kinematics(/* args */) = default;
+    ~Kinematics() = default;
+
+    odom_t& get_odom();
+    void update_odom(uint16_t dt);
+    void TransAngleInPI(float angle,float& out_angle);
+
+    void set_wheel_distance(float distance); // 设置轮子的间距
+
+    void set_motor_param(uint8_t id,float per_pluse_distance);
+    // 运动学正解,将左右轮的速度转换成线速度和角速度
+    void kinematics_forward(float left_speed,float right_speed,float* out_linear_speed,float* out_angular_speed);
+    // 运动学逆解，将线速度和角速度转换成左右轮的速度
+    void kinematics_inverse(float linear_speed,float angular_speed,float* out_left_speed,float* out_right_speed);
+    // 更新点击速度和编码器数据
+    void update_motor_speed(uint64_t current_time,int32_t left_tick,int32_t right_tick);
+    // 获取电机速度,返回值是速度
+    int16_t get_motor_speed(uint8_t id);
+
+};
+
+#endif // __KINEMATICS_H__