Merge pull request #99 from 1moule/switch-shoot-freq

Switch shoot frequency
rm-controls · Jul 17, 2024 · b62ee1e · b62ee1e
2 parents fd616fe + cc985c9
commit b62ee1e
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 5 deletions.
diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
@@ -29,7 +29,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   void rightSwitchUpRise() override;
   void rPress() override;
   void ePress() override;
-  void zPressing();
+  void zPress();
   void zRelease();
   void wPress() override;
   void wPressing() override;

diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -52,6 +52,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override;
   void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
   void leftSwitchUpOn(ros::Duration duration);
+  void leftSwitchUpFall();
   void mouseLeftPress();
   void mouseLeftRelease()
   {
@@ -119,6 +120,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   rm_common::CalibrationQueue* gimbal_calibration_;
 
   geometry_msgs::PointStamped point_out_;
+  uint8_t last_shoot_freq_{};
 
   bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false,
        adjust_image_transmission_ = false;

diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp
@@ -25,8 +25,8 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle
 
   ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this),
                         boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this));
-  z_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterCoverManual::zPressing, this));
-  z_event_.setFalling(boost::bind(&ChassisGimbalShooterCoverManual::zRelease, this));
+  z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::zPress, this),
+                   boost::bind(&ChassisGimbalShooterCoverManual::zRelease, this));
 }
 
 void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode)
@@ -159,14 +159,15 @@ void ChassisGimbalShooterCoverManual::ePress()
   switch_exposure_srv_->callService();
 }
 
-void ChassisGimbalShooterCoverManual::zPressing()
+void ChassisGimbalShooterCoverManual::zPress()
 {
+  last_shoot_freq_ = shooter_cmd_sender_->getShootFrequency();
   shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::MINIMAL);
 }
 
 void ChassisGimbalShooterCoverManual::zRelease()
 {
-  shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::LOW);
+  shooter_cmd_sender_->setShootFrequency(last_shoot_freq_);
 }
 
 void ChassisGimbalShooterCoverManual::wPress()

diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
@@ -40,6 +40,7 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros:
   self_inspection_event_.setRising(boost::bind(&ChassisGimbalShooterManual::selfInspectionStart, this));
   game_start_event_.setRising(boost::bind(&ChassisGimbalShooterManual::gameStart, this));
   left_switch_up_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterManual::leftSwitchUpOn, this, _1));
+  left_switch_up_event_.setFalling(boost::bind(&ChassisGimbalShooterManual::leftSwitchUpFall, this));
   left_switch_mid_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterManual::leftSwitchMidOn, this, _1));
   e_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::ePress, this),
                    boost::bind(&ChassisGimbalShooterManual::eRelease, this));
@@ -313,6 +314,12 @@ void ChassisGimbalShooterManual::leftSwitchUpRise()
 {
   ChassisGimbalManual::leftSwitchUpRise();
   gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
+  last_shoot_freq_ = shooter_cmd_sender_->getShootFrequency();
+}
+
+void ChassisGimbalShooterManual::leftSwitchUpFall()
+{
+  shooter_cmd_sender_->setShootFrequency(last_shoot_freq_);
 }
 
 void ChassisGimbalShooterManual::leftSwitchUpOn(ros::Duration duration)
@@ -323,11 +330,13 @@ void ChassisGimbalShooterManual::leftSwitchUpOn(ros::Duration duration)
     gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
   if (duration > ros::Duration(1.))
   {
+    shooter_cmd_sender_->setShootFrequency(last_shoot_freq_);
     shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
     shooter_cmd_sender_->checkError(ros::Time::now());
   }
   else if (duration < ros::Duration(0.02))
   {
+    shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::MINIMAL);
     shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
     shooter_cmd_sender_->checkError(ros::Time::now());
   }