diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 7ad9104a..f1a71b41 100644 --- 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 TimeStampCommandSenderBasegetSpeedLimit(); - msg_.hz = heat_limit_->getHz(); + msg_.hz = heat_limit_->getShootFrequency(); TimeStampCommandSenderBase::sendCommand(time); } double getSpeed() @@ -311,13 +311,13 @@ class ShooterCommandSender : public TimeStampCommandSenderBasesetMode(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 index 2ed8d962..a0252e32 100644 --- 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 index 4d0311dd..266e407f 100644 --- 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(); } }