创建命令

以speed_control为例，编写一个命令来控制速度的目标值。

注释下面代码，即之前写死的speed_setpoint删掉，以便我们在命令行中控制

/* 获取控制命令 暂时写死*/
speed_ctrl->speed_setpoint_ = 0.15f;

创建命令函数

mod_speed_control.hpp

在类内声明函数

注意一定要声明为静态，第一个参数为当前类的指针

static void SetSpeed(SpeedControl* ctrl, int argc, char* argv[]);

创建命令对象

使用当前类指针实例化一个命令模板，并作为SpeedControl的成员。

System::Term::Command<SpeedControl*> cmd_;

初始化命令

在SpeedControl的初始化列表里加入cmd_的初始化，即把SetSpeed函数映射到/dev/speed_control

cmd_(this, this->SetSpeed, "speed_control", System::Term::DevDir())

解析逻辑

argc为命令参数个数，argv为参数的字符串数组，第一个参数为命令名。

void SpeedControl::SetSpeed(SpeedControl* ctrl, int argc, char* argv[]) {
    /* 参数应当为一个 */
    if (argc != 2) {
        ms_printf("参数错误");
        return;
    }

    /* 获取参数并转为float */
    float speed_setpoint = std::stof(argv[1]);

    /* 写入目标速度 */
    ctrl->speed_setpoint_ = speed_setpoint;
    return;
}

运行

运行之后在命令行输入/dev/speed_control 0.2尝试更改速度，再使用/dev/motor show 1000 10查看电机反馈

示例代码

mod_speed_control.cpp

#include "mod_speed_control.hpp"

using namespace Module;

#define MOTOR_MAX_ROTATIONAL_SPEED (7000.0f)

SpeedControl::SpeedControl(Param& param)
    : param_(param),
      pid_(param.pid, 500),
      motor_(param.motor, "motor"),
      cmd_(this, this->SetSpeed, "speed_control", System::Term::DevDir()) {
  /* 使用lambda创建控制线程函数 */
  auto ctrl_thread = [](SpeedControl* speed_ctrl) {
    while (1) {
      /* 更新反馈 */
      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_);
}

void SpeedControl::SetSpeed(SpeedControl* ctrl, int argc, char* argv[]) {
  if (argc != 2) {
    ms_printf("参数错误");
    return;
  }

  float speed_setpoint = std::stof(argv[1]);

  ctrl->speed_setpoint_ = speed_setpoint;
  return;
}

mod_speed_control.hpp

#inclued <module.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();

  /* 设置目标速度 */
  static void SetSpeed(SpeedControl* ctrl, int argc, char* argv[]);

private:
  Param param_; /* 配置参数 */

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

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

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

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

  System::Term::Command<SpeedControl*> cmd_;
};
}  // namespace Module