Merge pull request #89 from YoujianWu/fix_bug

Avoid capacity keep in charge mode when chassis rotate.
rm-controls · Mar 25, 2024 · a7f7ab6 · a7f7ab6
2 parents f39aad9 + 44d6df4
commit a7f7ab6
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 3 deletions.
diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -83,6 +83,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   virtual void eRelease();
   virtual void cPress();
   virtual void bPress();
+  virtual void bRelease();
   virtual void rPress();
   virtual void xReleasing();
   virtual void shiftPress();

diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
@@ -47,7 +47,8 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros:
   q_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::qPress, this),
                    boost::bind(&ChassisGimbalShooterManual::qRelease, this));
   f_event_.setRising(boost::bind(&ChassisGimbalShooterManual::fPress, this));
-  b_event_.setRising(boost::bind(&ChassisGimbalShooterManual::bPress, this));
+  b_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::bPress, this),
+                   boost::bind(&ChassisGimbalShooterManual::bRelease, this));
   x_event_.setRising(boost::bind(&ChassisGimbalShooterManual::xPress, this));
   x_event_.setActiveLow(boost::bind(&ChassisGimbalShooterManual::xReleasing, this));
   r_event_.setRising(boost::bind(&ChassisGimbalShooterManual::rPress, this));
@@ -399,6 +400,11 @@ void ChassisGimbalShooterManual::bPress()
   chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
 }
 
+void ChassisGimbalShooterManual::bRelease()
+{
+  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+}
+
 void ChassisGimbalShooterManual::rPress()
 {
   if (camera_switch_cmd_sender_)
@@ -561,10 +567,11 @@ void ChassisGimbalShooterManual::vPress()
 
 void ChassisGimbalShooterManual::shiftPress()
 {
-  if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::FOLLOW)
+  if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::FOLLOW && is_gyro_)
   {
     chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
-    vel_cmd_sender_->setAngularZVel(0.);
+    vel_cmd_sender_->setAngularZVel(1.0);
+    is_gyro_ = false;
   }
   chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
 }