Merge pull request #247 from chen-gr/new_cap_add_state

Update new capacitor control code.

rm_common/include/rm_common/decision/power_limit.h

@@ -86,7 +86,14 @@ class PowerLimit
   }
   void updateState(uint8_t state)
   {
-    expect_state_ = state;
+    if (!capacitor_is_on_)
+      expect_state_ = ALLOFF;
+    else
+      expect_state_ = state;
+  }
+  void updateCapSwitchState(bool state)
+  {
+    capacitor_is_on_ = state;
   }
   void setGameRobotData(const rm_msgs::GameRobotStatus data)
   {
@@ -122,24 +129,29 @@ class PowerLimit
       {
         if (capacity_is_online_)
         {
-          if (chassis_power_limit_ > burst_power_)
-            chassis_cmd.power_limit = burst_power_;
+          if (expect_state_ == ALLOFF)
+            normal(chassis_cmd);
           else
           {
-            switch (is_new_capacitor_ ? expect_state_ : cap_state_)
+            if (chassis_power_limit_ > burst_power_)
+              chassis_cmd.power_limit = burst_power_;
+            else
             {
-              case NORMAL:
-                normal(chassis_cmd);
-                break;
-              case BURST:
-                burst(chassis_cmd, is_gyro);
-                break;
-              case CHARGE:
-                charge(chassis_cmd);
-                break;
-              default:
-                zero(chassis_cmd);
-                break;
+              switch (is_new_capacitor_ ? expect_state_ : cap_state_)
+              {
+                case NORMAL:
+                  normal(chassis_cmd);
+                  break;
+                case BURST:
+                  burst(chassis_cmd, is_gyro);
+                  break;
+                case CHARGE:
+                  charge(chassis_cmd);
+                  break;
+                default:
+                  zero(chassis_cmd);
+                  break;
+              }
             }
           }
         }
@@ -169,7 +181,7 @@ class PowerLimit
   }
   void burst(rm_msgs::ChassisCmd& chassis_cmd, bool is_gyro)
   {
-    if (cap_energy_ > capacitor_threshold_)
+    if (cap_state_ != ALLOFF && cap_energy_ > capacitor_threshold_)
     {
       if (is_gyro)
         chassis_cmd.power_limit = chassis_power_limit_ + extra_power_;
@@ -188,8 +200,9 @@ class PowerLimit
   double charge_power_{}, extra_power_{}, burst_power_{};
   double buffer_threshold_{};
   double power_gain_{};
-  bool is_new_capacitor_{};
+  bool is_new_capacitor_{ false };
   uint8_t expect_state_{}, cap_state_{};
+  bool capacitor_is_on_{ true };
 
   bool referee_is_online_{ false };
   bool capacity_is_online_{ false };

rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg

@@ -2,7 +2,7 @@ float32 chassis_power
 float32 chassis_expect_power
 float32 capacity_recent_charge_power
 float32 capacity_remain_charge
-uint8 capacity_expect_charge_power
+uint8 capacity_discharge_power
 uint8 state_machine_running_state
 uint8 power_management_protection_info
 uint8 power_management_topology

rm_referee/include/rm_referee/ui/trigger_change_ui.h

@@ -70,7 +70,8 @@ class ChassisTriggerChangeUi : public TriggerChangeUi
 
 private:
   void update() override;
-  void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false) override;
+  void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false,
+                    bool extra_flag = false);
   void displayInCapacity();
   std::string getChassisState(uint8_t mode);
   uint8_t chassis_mode_, power_limit_state_, s_l_, s_r_, key_ctrl_, key_shift_, key_b_;

rm_referee/src/referee.cpp

@@ -551,7 +551,7 @@ int Referee::unpack(uint8_t* rx_data)
               (static_cast<uint16_t>((data[4] << 8) | data[5]) / 100.);
           sample_and_status_pub_data.capacity_remain_charge =
               (static_cast<uint16_t>((data[6] << 8) | data[7]) / 10000.);
-          sample_and_status_pub_data.capacity_expect_charge_power = static_cast<uint8_t>(data[8]);
+          sample_and_status_pub_data.capacity_discharge_power = static_cast<uint8_t>(data[8]);
           sample_and_status_pub_data.state_machine_running_state = base_.capacity_recent_mode_ =
               static_cast<uint8_t>(data[9] >> 4);
           sample_and_status_pub_data.power_management_protection_info = static_cast<uint8_t>((data[9] >> 2) & 0x03);

rm_referee/src/ui/trigger_change_ui.cpp

@@ -87,7 +87,8 @@ void ChassisTriggerChangeUi::update()
     updateConfig(chassis_mode_, false, 1, false);
   else
     updateConfig(chassis_mode_, power_limit_state_ == rm_common::PowerLimit::BURST, 0,
-                 power_limit_state_ == rm_common::PowerLimit::CHARGE);
+                 power_limit_state_ == rm_common::PowerLimit::CHARGE,
+                 power_limit_state_ == rm_common::PowerLimit::NORMAL);
   graph_->setOperation(rm_referee::GraphOperation::UPDATE);
   checkModeChange();
   updateTwiceForQueue(true);
@@ -128,7 +129,8 @@ void ChassisTriggerChangeUi::checkModeChange()
   }
 }
 
-void ChassisTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag)
+void ChassisTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag,
+                                          bool extra_flag)
 {
   static ros::Time trigger_time;
   static int expect;
@@ -144,9 +146,7 @@ void ChassisTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uin
     graph_->setColor(rm_referee::GraphColor::PINK);
   else
   {
-    if ((base_.capacity_recent_mode_ == rm_common::PowerLimit::NORMAL ||
-         power_limit_state_ == rm_common::PowerLimit::NORMAL) &&
-        !delay)
+    if (base_.capacity_recent_mode_ == rm_common::PowerLimit::ALLOFF && !delay)
     {
       trigger_time = ros::Time::now();
       expect = power_limit_state_;
@@ -165,8 +165,10 @@ void ChassisTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uin
           graph_->setColor(rm_referee::GraphColor::ORANGE);
         else if (sub_flag)
           graph_->setColor(rm_referee::GraphColor::GREEN);
-        else
+        else if (extra_flag)
           graph_->setColor(rm_referee::GraphColor::WHITE);
+        else
+          graph_->setColor(rm_referee::GraphColor::BLACK);
         delay = false;
       }
     }
@@ -176,8 +178,10 @@ void ChassisTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uin
         graph_->setColor(rm_referee::GraphColor::ORANGE);
       else if (sub_flag)
         graph_->setColor(rm_referee::GraphColor::GREEN);
-      else
+      else if (extra_flag)
         graph_->setColor(rm_referee::GraphColor::WHITE);
+      else
+        graph_->setColor(rm_referee::GraphColor::BLACK);
     }
   }
 }