Merge pull request #72 from xrobot-org/dev

Dev

Author: Jiu-xiao

hw/bsp/rm-c/config/engineer.config

@@ -9,10 +9,11 @@
  *
  */
 
+#include "mod_engineer_chassis.hpp"
+
 #include <random>
 
 #include "bsp_time.h"
-#include "mod_chassis.hpp"
 
 #define ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */
 #define ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
@@ -21,31 +22,13 @@
 
 #define MOTOR_MAX_ROTATIONAL_SPEED 7000.0f /* 电机的最大转速 */
 
-#if POWER_LIMIT_WITH_CAP
-/* 保证电容电量宏定义在正确范围内 */
-#if ((CAP_PERCENT_NO_LIM < 0) || (CAP_PERCENT_NO_LIM > 100) || \
-     (CAP_PERCENT_WORK < 0) || (CAP_PERCENT_WORK > 100))
-#error "Cap percentage should be in the range from 0 to 100."
-#endif
-
-/* 保证电容功率宏定义在正确范围内 */
-#if ((CAP_MAX_LOAD < 60) || (CAP_MAX_LOAD > 200))
-#error "The capacitor power should be in in the range from 60 to 200."
-#endif
-
-static const float kCAP_PERCENTAGE_NO_LIM = (float)CAP_PERCENT_NO_LIM / 100.0f;
-static const float kCAP_PERCENTAGE_WORK = (float)CAP_PERCENT_WORK / 100.0f;
-
-#endif
-
 using namespace Module;
 
 template <typename Motor, typename MotorParam>
 Chassis<Motor, MotorParam>::Chassis(Param& param, float control_freq)
     : param_(param),
       mode_(Chassis::RELAX),
       mixer_(param.type),
-      follow_pid_(param.follow_pid_param, control_freq),
       ctrl_lock_(true) {
   memset(&(this->cmd_), 0, sizeof(this->cmd_));
 
@@ -70,12 +53,6 @@ Chassis<Motor, MotorParam>::Chassis(Param& param, float control_freq)
       case SET_MODE_RELAX:
         chassis->SetMode(RELAX);
         break;
-      case SET_MODE_FOLLOW:
-        chassis->SetMode(FOLLOW_GIMBAL);
-        break;
-      case SET_MODE_ROTOR:
-        chassis->SetMode(ROTOR);
-        break;
       case SET_MODE_INDENPENDENT:
         chassis->SetMode(INDENPENDENT);
         break;
@@ -95,26 +72,24 @@ Chassis<Motor, MotorParam>::Chassis(Param& param, float control_freq)
   auto chassis_thread = [](Chassis* chassis) {
     auto cmd_sub =
         Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
-
+    uint32_t last_online_time = bsp_time_get_ms();
     while (1) {
       /* 读取控制指令、电容、裁判系统、电机反馈 */
       cmd_sub.DumpData(chassis->cmd_);
 
       /* 更新反馈值 */
-      chassis->PraseRef();
-
       chassis->ctrl_lock_.Wait(UINT32_MAX);
       chassis->UpdateFeedback();
       chassis->Control();
       chassis->ctrl_lock_.Post();
 
       /* 运行结束，等待下一次唤醒 */
-      chassis->thread_.SleepUntil(2);
+      chassis->thread_.SleepUntil(2, last_online_time);
     }
   };
 
-  this->thread_.Create(chassis_thread, this, "chassis_thread",
-                       MODULE_CHASSIS_TASK_STACK_DEPTH, System::Thread::MEDIUM);
+  this->thread_.Create(chassis_thread, this, "chassis_thread", 512,
+                       System::Thread::MEDIUM);
 
   System::Timer::Create(this->DrawUIStatic, this, 2100);
 
@@ -152,16 +127,6 @@ void Chassis<Motor, MotorParam>::Control() {
       break;
 
     case Chassis::RELAX:
-    case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量
-                                  */
-    case Chassis::ROTOR: {
-      float beta = this->yaw_;
-      float cos_beta = cosf(beta);
-      float sin_beta = sinf(beta);
-      this->move_vec_.vx = cos_beta * this->cmd_.x - sin_beta * this->cmd_.y;
-      this->move_vec_.vy = sin_beta * this->cmd_.x + cos_beta * this->cmd_.y;
-      break;
-    }
     case Chassis::REVERSE: {
       this->move_vec_.vx = -this->cmd_.x;
       this->move_vec_.vy = -this->cmd_.y;
@@ -177,19 +142,6 @@ void Chassis<Motor, MotorParam>::Control() {
     case Chassis::INDENPENDENT: /* 独立模式wz为0 */
       this->move_vec_.wz = this->cmd_.z;
       break;
-
-    case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台
-                                  */
-      this->move_vec_.wz =
-          this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_);
-      break;
-
-    case Chassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转
-                            */
-      this->move_vec_.wz =
-          this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX);
-      break;
-    }
     case Chassis::REVERSE: {
       this->move_vec_.wz = this->cmd_.z;
       break;
@@ -207,21 +159,8 @@ void Chassis<Motor, MotorParam>::Control() {
   /* 根据底盘模式计算输出值 */
   switch (this->mode_) {
     case Chassis::BREAK:
-    case Chassis::FOLLOW_GIMBAL:
-    case Chassis::ROTOR:
     case Chassis::REVERSE:
     case Chassis::INDENPENDENT: /* 独立模式,受PID控制 */ {
-      float percentage = 0.0f;
-      if (cap_.online_) {
-        percentage = cap_.percentage_;
-      } else if (ref_.status == Device::Referee::RUNNING) {
-        percentage = this->ref_.chassis_pwr_buff / 30.0f;
-      } else {
-        percentage = 1.0f;
-      }
-
-      clampf(&percentage, 0.0f, 1.0f);
-
       for (unsigned i = 0; i < this->mixer_.len_; i++) {
         float out = this->actuator_[i]->Calculate(
             this->setpoint_.motor_rotational_speed[i] *
@@ -243,15 +182,6 @@ void Chassis<Motor, MotorParam>::Control() {
   }
 }
 
-template <typename Motor, typename MotorParam>
-void Chassis<Motor, MotorParam>::PraseRef() {
-  this->ref_.chassis_power_limit =
-      this->raw_ref_.robot_status.chassis_power_limit;
-  this->ref_.chassis_pwr_buff = this->raw_ref_.power_heat.chassis_pwr_buff;
-  this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
-  this->ref_.status = this->raw_ref_.status;
-}
-
 template <typename Motor, typename MotorParam>
 float Chassis<Motor, MotorParam>::CalcWz(const float LO, const float HI) {
   float wz_vary = fabsf(0.2f * sinf(ROTOR_OMEGA * this->now_)) + LO;
@@ -265,10 +195,6 @@ void Chassis<Motor, MotorParam>::SetMode(Chassis::Mode mode) {
     return; /* 模式未改变直接返回 */
   }
 
-  if (mode == Chassis::ROTOR && this->mode_ != Chassis::ROTOR) {
-    std::srand(this->now_);
-    this->wz_dir_mult_ = (std::rand() % 2) ? -1 : 1;
-  }
   /* 切换模式后重置PID和滤波器 */
   for (size_t i = 0; i < this->mixer_.len_; i++) {
     this->actuator_[i]->Reset();
@@ -294,14 +220,6 @@ void Chassis<Motor, MotorParam>::DrawUIStatic(
 
   /* 更新底盘模式选择框 */
   switch (chassis->mode_) {
-    case FOLLOW_GIMBAL:
-      box_pos_left = REF_UI_MODE_OFFSET_2_LEFT;
-      box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT;
-      break;
-    case ROTOR:
-      box_pos_left = REF_UI_MODE_OFFSET_4_LEFT;
-      box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT;
-      break;
     case INDENPENDENT:
       box_pos_left = REF_UI_MODE_OFFSET_3_LEFT;
       box_pos_right = REF_UI_MODE_OFFSET_3_RIGHT;
@@ -342,14 +260,6 @@ void Chassis<Motor, MotorParam>::DrawUIDynamic(
 
   /* 更新底盘模式选择框 */
   switch (chassis->mode_) {
-    case FOLLOW_GIMBAL:
-      box_pos_left = REF_UI_MODE_OFFSET_2_LEFT;
-      box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT;
-      break;
-    case ROTOR:
-      box_pos_left = REF_UI_MODE_OFFSET_4_LEFT;
-      box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT;
-      break;
     case RELAX:
 
     case BREAK: