feat: 更新第九章

chapt9/fishbot_motion_control_9.3/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_9.3/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__

chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.cpp

@@ -0,0 +1,64 @@
+#include "Arduino.h"
+#include "PidController.h"
+
+// 构造函数，传入三个PID参数
+PidController::PidController(float kp, float ki, float kd)
+{
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+}
+
+float PidController::update(float current)
+{
+    error_ = target_ - current; //  计算error
+
+    error_sum_ += error_; //  计算error_sum,同时限制积分上下限
+    if (error_sum_ > intergral_up_)
+        error_sum_ = intergral_up_;
+    if (error_sum_ < -1 * intergral_up_)
+        error_sum_ = -1 * intergral_up_;
+
+    derror_ = prev_error_ - error_; // 计算误差变化率
+    prev_error_ = error_;           // 方便下次计算使用
+
+    float output = kp_ * error_ + ki_ * error_sum_ + kd_ * derror_;
+
+    if (output > out_max_)
+        output = out_max_;
+    if (output < out_min_)
+        output = out_min_;
+
+    return output;
+}
+
+void PidController::update_target(float target)
+{
+    target_ = target;
+}
+
+void PidController::update_pid(float kp, float ki, float kd)
+{
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+}
+
+void PidController::reset()
+{
+    error_sum_ = 0;
+    prev_error_ = 0;
+    error_ = 0;
+    derror_ = 0;
+    kp_ = 0;
+    ki_ = 0;
+    kd_ = 0;
+    intergral_up_ = 2500;
+    out_min_ = 0;
+    out_max_ = 0;
+}
+void PidController::out_limit(float min, float max)
+{
+    out_min_ = min;
+    out_max_ = max;
+}

chapt9/fishbot_motion_control_9.3/lib/PidController/PidController.h

@@ -0,0 +1,33 @@
+#ifndef __PID_CONTROLLER_H__
+#define __PID_CONTROLLER_H__
+
+class PidController
+{
+public:
+    PidController() = default;
+    PidController(float kp, float ki, float kd);
+
+private:
+    // PID 参数，可以调节的
+    float target_;
+    float out_min_;
+    float out_max_;
+    float kp_;
+    float ki_;
+    float kd_;
+    float intergral_up_ = 2500; // 积分上限
+    // pid 中间过程值
+    float error_;
+    float error_sum_;
+    float derror_;
+    float prev_error_;
+
+public:
+    float update(float current);                   // 提供当前值，返回下次输出值，也就是PID的结果
+    void update_target(float target);              // 更新目标值
+    void update_pid(float kp, float ki, float kd); // 更新PID参数
+    void reset();                                  // 重置PID
+    void out_limit(float min, float max);          // 设置输出限制
+};
+
+#endif // __PID_CONTROLLER_H__

chapt9/fishbot_motion_control_9.3/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html