在模拟器中控制麦轮底盘

新建一个robot，加入RMMotor模块和刚才编写的底盘模块，并编写配置文件。模拟器文件使用src/robot/sim_mecanum/Mecanum.wbt

robot.cpp

#include "robot.hpp"

#include <thread.hpp>

#include "system.hpp"

/* clang-format off */
Robot::Simulator::Param param = {
  .led = {
    .gpio = BSP_GPIO_LED,
    .timeout = 200,
  },

  .imu = {
    .tp_name_prefix = "chassis",
  },

  .motor = {
    Device::RMMotor::Param{
        .model = Device::RMMotor::MOTOR_M3508,
    },
    Device::RMMotor::Param{
        .model = Device::RMMotor::MOTOR_M3508,
    },
    Device::RMMotor::Param{
        .model = Device::RMMotor::MOTOR_M3508,
    },
    Device::RMMotor::Param{
        .model = Device::RMMotor::MOTOR_M3508,
    },
  },

  .chassis={
    .EVENT_MAP = {
      Component::CMD::EventMapItem{
        Device::TerminalController::STOP_CTRL,
        Module::Mecanum::SET_MODE_RELAX
      },
      Component::CMD::EventMapItem{
        Device::TerminalController::START_CTRL,
        Module::Mecanum::SET_MODE_INDENPENDENT
      },
    },

    .speed_actr = {
      Component::SpeedActuator::Param{
        .speed = {
          .k = 2.0f,
          .p = 1.0f,
          .i = 0.0f,
          .d = 0.0f,
          .i_limit = 0.02f,
          .out_limit = 1.0f,
          .d_cutoff_freq = -1.0f,
          .cycle = false,
        },

        .in_cutoff_freq = -1.0f,

        .out_cutoff_freq = -1.0f,

      },
      Component::SpeedActuator::Param{
        .speed = {
          .k = 2.0f,
          .p = 1.0f,
          .i = 0.0f,
          .d = 0.0f,
          .i_limit = 0.02f,
          .out_limit = 1.0f,
          .d_cutoff_freq = -1.0f,
          .cycle = false,
        },

        .in_cutoff_freq = -1.0f,

        .out_cutoff_freq = -1.0f,
      },
      Component::SpeedActuator::Param{
        .speed = {
          .k = 2.0f,
          .p = 1.0f,
          .i = 0.0f,
          .d = 0.0f,
          .i_limit = 0.02f,
          .out_limit = 1.0f,
          .d_cutoff_freq = -1.0f,
          .cycle = false,
        },

        .in_cutoff_freq = -1.0f,

        .out_cutoff_freq = -1.0f,
      },
      Component::SpeedActuator::Param{
        .speed = {
          .k = 2.0f,
          .p = 1.0f,
          .i = 0.0f,
          .d = 0.0f,
          .i_limit = 0.02f,
          .out_limit = 1.0f,
          .d_cutoff_freq = -1.0f,
          .cycle = false,
        },

        .in_cutoff_freq = -1.0f,

        .out_cutoff_freq = -1.0f,
      },
    },

    .follow = {
      .k = 0.5f,
      .p = 1.0f,
      .i = 0.0f,
      .d = 0.0f,
      .i_limit = 1.0f,
      .out_limit = 1.0f,
      .d_cutoff_freq = -1.0f,
      .cycle = true,
    },
  },
};
/* clang-format on */

void robot_init() { System::Start<Robot::Simulator, Robot::Simulator::Param>(param); }

robot.hpp

#include <array>

#include "comp_utils.hpp"
#include "dev_blink_led.hpp"
#include "dev_camera.hpp"
#include "dev_controller.hpp"
#include "dev_imu.hpp"
#include "dev_motor.hpp"
#include "dev_referee.hpp"
#include "dev_rm_motor.hpp"
#include "mod_mecanum.hpp"

void robot_init();

namespace Robot {
class Simulator {
 public:
  typedef struct {
    Device::BlinkLED::Param led;
    Device::IMU::Param imu;
    std::array<Device::RMMotor::Param, 4> motor;
    Module::Mecanum::Param chassis;
  } Param;

  Component::CMD cmd_;

  Device::BlinkLED led_;
  Device::Referee referee_;
  Device::TerminalController ctrl_;
  Device::Camera camera_;
  Device::IMU imu_;
  std::array<Device::RMMotor, 4> motor_;

  Module::Mecanum chassis_;

  Simulator(Param& param)
      : cmd_(Component::CMD::CMD_TERM_CTRL),
        led_(param.led),
        imu_(param.imu),
        motor_(std::array<Device::RMMotor, 4>{
            Device::RMMotor(param.motor[0], "Chassis_0"),
            Device::RMMotor(param.motor[1], "Chassis_1"),
            Device::RMMotor(param.motor[2], "Chassis_2"),
            Device::RMMotor(param.motor[3], "Chassis_3")}),
        chassis_(param.chassis, std::array<Device::BaseMotor*, 4>{
                                    &this->motor_[0], &this->motor_[1],
                                    &this->motor_[2], &this->motor_[3]}) {}
};
}  // namespace Robot

运行

启动模拟器和代码，在命令行中输入control start和control keyboard。在模拟器界面即可使用wasd控制底盘。