设计一个速度控制器

设计一个module，使用PID闭环来控制电机速度。实践时请对照 开始 章节如何编写新的模块和设备相关内容。

创建一个新的module

使用./project.py new module speed_control来创建。

此模块应当有以下文件：

speed_control
├── info.cmake
├── Kconfig
├── mod_speed_control.cpp
└── mod_speed_control.hpp

注意：语法检查和自动补全需要在config(使用命令'./project.py config'选择)中选择对应模块，然后刷新CMake缓存(使用命令'./project.py refresh')后才能使用

加入头文件

需要pid和电机类。

mod_speed_control.hpp

#include "comp_pid.hpp"
#include "dev_rm_motor.hpp"

声明speed_control类

namespace Module {
class SpeedControl {
    ...
};
}  // namespace Module

定义配置文件结构体

speed_control的配置文件应当包含pid和motor的配置

typedef struct {
  Component::PID::Param pid;
  Device::RMMotor::Param motor;
} Param;

成员函数

/* 构造函数 */
SpeedControl(Param& param);

/* 控制函数 */
void Control();

/* 更新电机反馈 */
void UpdateFeedback();

其他变量

float now_;         /* 当前时间 */
float dt_;          /* 距离上次控制的时间 */
float lask_wakeup_; /* 上次控制的时间 */

float speed_setpoint_; /* 速度目标值 */
float output_;         /* 电机输出 */

Component::PID pid_;    /* pid对象 */
Device::RMMotor motor_; /* motor对象 */

System::Thread thread_; /* 控制线程 */

运行逻辑

mod_speed_control.cpp 先包含对应头文件和命名空间

#include "mod_speed_control.hpp"

using namespace Module;

声明电机最大速度

#define MOTOR_MAX_ROTATIONAL_SPEED (7000.0f)

构造函数

用初始化列表初始化成员变量，再创建控制线程。

SpeedControl::SpeedControl(Param& param)
    :pid_(param.pid, 500), motor_(param.motor, "motor") {
  /* 使用lambda创建控制线程函数 */
  auto ctrl_thread = [](SpeedControl* speed_ctrl) {
    while (1) {
      /* 获取控制命令 暂时写死*/
      speed_ctrl->speed_setpoint_ = 0.15f;

      /* 更新反馈 */
      speed_ctrl->UpdateFeedback();

      /* 控制电机 */
      speed_ctrl->Control();

      /* 等待下一次控制 */
      speed_ctrl->thread_.SleepUntil(2);
    }
  };

  /* 创建线程 */
  this->thread_.Create(ctrl_thread, this, "speed_control", 384,
                      System::Thread::MEDIUM);
}

获取反馈信息

void SpeedControl::UpdateFeedback() { this->motor_.Update(); }

PID控制

void SpeedControl::Control() {
  /* 更新时间 */
  this->now_ = bsp_time_get();
  this->dt_ = this->now_ - this->lask_wakeup_;
  this->lask_wakeup_ = this->now_;

  /* 计算PID */
  this->output_ = this->pid_.Calculate(
      this->speed_setpoint_,
      this->motor_.GetSpeed() / MOTOR_MAX_ROTATIONAL_SPEED, this->dt_);

  /* 电机输出 */
  this->motor_.Control(this->output_);
}

完整代码示例

mod_speed_control.cpp

#include "mod_speed_control.hpp"

using namespace Module;

#define MOTOR_MAX_ROTATIONAL_SPEED (7000.0f)

SpeedControl::SpeedControl(Param& param)
    :pid_(param.pid, 500), motor_(param.motor, "motor") {
  /* 使用lambda创建控制线程函数 */
  auto ctrl_thread = [](SpeedControl* speed_ctrl) {
    while (1) {
      /* 获取控制命令 暂时写死*/
      speed_ctrl->speed_setpoint_ = 0.15f;

      /* 更新反馈 */
      speed_ctrl->UpdateFeedback();

      /* 控制电机 */
      speed_ctrl->Control();

      /* 等待下一次控制 */
      speed_ctrl->thread_.SleepUntil(2);
    }
  };

  /* 创建线程 */
  this->thread_.Create(ctrl_thread, this, "speed_control", 384,
                      System::Thread::MEDIUM);
}

void SpeedControl::UpdateFeedback() { this->motor_.Update(); }

void SpeedControl::Control() {
  /* 更新时间 */
  this->now_ = bsp_time_get();
  this->dt_ = this->now_ - this->lask_wakeup_;
  this->lask_wakeup_ = this->now_;

  /* 计算PID */
  this->output_ = this->pid_.Calculate(
      this->speed_setpoint_,
      this->motor_.GetSpeed() / MOTOR_MAX_ROTATIONAL_SPEED, this->dt_);

  /* 电机输出 */
  this->motor_.Control(this->output_);
}

mod_speed_control.hpp

#include "comp_pid.hpp"
#include "dev_rm_motor.hpp"

namespace Module {
class SpeedControl {
public:
  typedef struct {
    Component::PID::Param pid;
    Device::RMMotor::Param motor;
  } Param;

  /* 构造函数 */
  SpeedControl(Param& param);

  /* 控制函数 */
  void Control();

  /* 更新电机反馈 */
  void UpdateFeedback();

private:
  float now_;         /* 当前时间 */
  float dt_;          /* 距离上次控制的时间 */
  float lask_wakeup_; /* 上次控制的时间 */

  float speed_setpoint_; /* 速度目标值 */
  float output_;         /* 电机输出 */

  Component::PID pid_;    /* pid对象 */
  Device::RMMotor motor_; /* motor对象 */

  System::Thread thread_; /* 控制线程 */
};
}  // namespace Module