Add key function to switch low or high shoot frequency and fix shift …

…key function bug.
rm-controls · Jun 11, 2022 · 98f40dd · 98f40dd
1 parent 2a3151b
commit 98f40dd
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 33 deletions.
diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h
@@ -291,7 +291,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   void sendCommand(const ros::Time& time) override
   {
     msg_.speed = heat_limit_->getSpeedLimit();
-    msg_.hz = heat_limit_->getHz();
+    msg_.hz = heat_limit_->getShootFrequency();
     TimeStampCommandSenderBase<rm_msgs::ShootCmd>::sendCommand(time);
   }
   double getSpeed()
@@ -311,13 +311,13 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
     }
     return 0.;
   }
-  void setBurstMode(bool burst_flag)
+  void setShootFrequency(uint8_t mode)
   {
-    heat_limit_->setMode(burst_flag);
+    heat_limit_->setShootFrequency(mode);
   }
-  bool getBurstMode()
+  uint8_t getShootFrequency()
   {
-    return heat_limit_->getMode();
+    return heat_limit_->getShootFrequencyMode();
   }
   void setZero() override{};
 

diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h
@@ -45,13 +45,9 @@ class HeatLimit
 public:
   HeatLimit(ros::NodeHandle& nh, const RefereeData& referee_data) : referee_data_(referee_data)
   {
-    if (!nh.getParam("expect_shoot_frequency_1", expect_shoot_frequency_1_))
+    if (!nh.getParam("low_shoot_frequency", low_shoot_frequency_))
       ROS_ERROR("Expect shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
-    if (!nh.getParam("expect_shoot_frequency_2", expect_shoot_frequency_2_))
-      ROS_ERROR("Expect shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
-    if (!nh.getParam("expect_shoot_frequency_3", expect_shoot_frequency_3_))
-      ROS_ERROR("Expect shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
-    if (!nh.getParam("burst_shoot_frequency", burst_shoot_frequency_))
+    if (!nh.getParam("high_shoot_frequency", high_shoot_frequency_))
       ROS_ERROR("Expect shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("safe_shoot_frequency", safe_shoot_frequency_))
       ROS_ERROR("Safe shoot frequency no defined (namespace: %s)", nh.getNamespace().c_str());
@@ -65,9 +61,16 @@ class HeatLimit
       bullet_heat_ = 10.;
   }
 
-  double getHz() const
+  typedef enum
+  {
+    LOW = 0,
+    HIGH = 1,
+    BURST = 2,
+  } ShootHz;
+
+  double getShootFrequency() const
   {
-    if (burst_flag_)
+    if (state_ == BURST)
       return shoot_frequency_;
     if (!referee_data_.is_online_)
       return safe_shoot_frequency_;
@@ -143,34 +146,34 @@ class HeatLimit
     return -1;  // TODO unsafe!
   }
 
-  void updateExpectShootFrequency()
+  void setShootFrequency(uint8_t mode)
   {
-    if (burst_flag_)
-      shoot_frequency_ = burst_shoot_frequency_;
-    else if (referee_data_.game_robot_status_.robot_level_ == 1)
-      shoot_frequency_ = expect_shoot_frequency_1_;
-    else if (referee_data_.game_robot_status_.robot_level_ == 2)
-      shoot_frequency_ = expect_shoot_frequency_2_;
-    else if (referee_data_.game_robot_status_.robot_level_ == 3)
-      shoot_frequency_ = expect_shoot_frequency_3_;
-    else
-      shoot_frequency_ = safe_shoot_frequency_;
+    state_ = mode;
   }
 
-  void setMode(bool burst_flag)
+  bool getShootFrequencyMode() const
   {
-    burst_flag_ = burst_flag;
+    return state_;
   }
-  bool getMode() const
+
+private:
+  void updateExpectShootFrequency()
   {
-    return burst_flag_;
+    if (state_ == HeatLimit::BURST)
+      shoot_frequency_ = high_shoot_frequency_;
+    else if (state_ == HeatLimit::LOW)
+      shoot_frequency_ = low_shoot_frequency_;
+    else if (state_ == HeatLimit::HIGH)
+      shoot_frequency_ = high_shoot_frequency_;
+    else
+      shoot_frequency_ = safe_shoot_frequency_;
   }
 
-private:
   std::string type_{};
   const RefereeData& referee_data_;
-  double bullet_heat_, safe_shoot_frequency_{}, heat_coeff_{}, shoot_frequency_{}, expect_shoot_frequency_1_{},
-      expect_shoot_frequency_2_{}, expect_shoot_frequency_3_{}, burst_shoot_frequency_{};
+  double bullet_heat_, safe_shoot_frequency_{}, heat_coeff_{}, shoot_frequency_{}, low_shoot_frequency_{},
+      high_shoot_frequency_{};
+  uint8_t state_{};
   bool burst_flag_ = false;
 };
 

diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h
@@ -111,8 +111,8 @@ class PowerLimit
                 charge();
                 break;
             }
-            if (!(state_ == Mode::BURST) && (abs(referee_data_.capacity_data.limit_power_ -
-                                                 referee_data_.game_robot_status_.chassis_power_limit_) < 0.05))
+            if (state_ != Mode::BURST && (abs(referee_data_.capacity_data.limit_power_ -
+                                              referee_data_.game_robot_status_.chassis_power_limit_) < 0.05))
               normal();
           }
         }