Slow gyro's speed when shooting buff

include/rm_manual/chassis_gimbal_shooter_cover_manual.h

@@ -27,14 +27,17 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   void rightSwitchDownRise() override;
   void rightSwitchMidRise() override;
   void rightSwitchUpRise() override;
-  void rPress() override;
   void ePress() override;
+  void cPress() override;
   void zPress();
   void zRelease();
-  void wPress() override;
-  void wPressing() override;
   void ctrlRPressing();
   void ctrlRRelease() override;
+  void wPress() override;
+  void aPress() override;
+  void sPress() override;
+  void dPress() override;
+  void wPressing() override;
 
   virtual void ctrlZPress();
   virtual void ctrlZRelease()
@@ -43,6 +46,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   };
   double low_speed_scale_{}, normal_speed_scale_{};
   double exit_buff_mode_duration_{};
+  double buff_gyro_rotate_limit_{};
   rm_common::SwitchDetectionCaller* switch_buff_srv_{};
   rm_common::SwitchDetectionCaller* switch_buff_type_srv_{};
   rm_common::SwitchDetectionCaller* switch_exposure_srv_{};

include/rm_manual/chassis_gimbal_shooter_manual.h

@@ -88,10 +88,10 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   virtual void cPress();
   virtual void bPress();
   virtual void bRelease();
-  virtual void rPress();
   virtual void xRelease();
   virtual void shiftPress();
   virtual void shiftRelease();
+  void rPress();
   void qPress()
   {
     shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::BURST);

src/chassis_gimbal_shooter_cover_manual.cpp

@@ -22,6 +22,7 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle
   normal_speed_scale_ = chassis_nh.param("normal_speed_scale", 1);
   low_speed_scale_ = chassis_nh.param("low_speed_scale", 0.30);
   nh.param("exit_buff_mode_duration", exit_buff_mode_duration_, 0.5);
+  nh.param("buff_gyro_rotate_limit", buff_gyro_rotate_limit_, 6.0);
 
   ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this),
                         boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this));
@@ -133,21 +134,6 @@ void ChassisGimbalShooterCoverManual::rightSwitchUpRise()
   supply_ = false;
 }
 
-void ChassisGimbalShooterCoverManual::rPress()
-{
-  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
-  {
-    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
-    if (switch_buff_type_srv_->getTarget() == rm_msgs::StatusChangeRequest::SMALL_BUFF)
-      switch_buff_type_srv_->setTargetType(rm_msgs::StatusChangeRequest::BIG_BUFF);
-    else
-      switch_buff_type_srv_->setTargetType(rm_msgs::StatusChangeRequest::SMALL_BUFF);
-    switch_buff_type_srv_->callService();
-  }
-  else
-    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
-}
-
 void ChassisGimbalShooterCoverManual::ePress()
 {
   switch_buff_srv_->switchTargetType();
@@ -158,6 +144,53 @@ void ChassisGimbalShooterCoverManual::ePress()
   switch_detection_srv_->callService();
   switch_buff_type_srv_->callService();
   switch_exposure_srv_->callService();
+  if (is_gyro_)
+  {
+    if (switch_buff_srv_->getTarget() == rm_msgs::StatusChangeRequest::ARMOR)
+    {
+      if (x_scale_ != 0.0 || y_scale_ != 0.0)
+        vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
+      else
+        vel_cmd_sender_->setAngularZVel(1.0);
+    }
+    else
+    {
+      if (x_scale_ != 0.0 || y_scale_ != 0.0)
+        vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, buff_gyro_rotate_limit_);
+      else
+        vel_cmd_sender_->setAngularZVel(1.0, buff_gyro_rotate_limit_);
+    }
+  }
+}
+
+void ChassisGimbalShooterCoverManual::cPress()
+{
+  if (is_gyro_)
+  {
+    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
+    vel_cmd_sender_->setAngularZVel(0.0);
+    is_gyro_ = false;
+  }
+  else
+  {
+    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
+    is_gyro_ = true;
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+    if (x_scale_ != 0.0 || y_scale_ != 0.0)
+    {
+      if (switch_buff_srv_->getTarget() == rm_msgs::StatusChangeRequest::SMALL_BUFF)
+        vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, buff_gyro_rotate_limit_);
+      else
+        vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
+    }
+    else
+    {
+      if (switch_buff_srv_->getTarget() == rm_msgs::StatusChangeRequest::SMALL_BUFF)
+        vel_cmd_sender_->setAngularZVel(1.0, buff_gyro_rotate_limit_);
+      else
+        vel_cmd_sender_->setAngularZVel(1.0);
+    }
+  }
 }
 
 void ChassisGimbalShooterCoverManual::zPress()
@@ -174,8 +207,32 @@ void ChassisGimbalShooterCoverManual::zRelease()
 void ChassisGimbalShooterCoverManual::wPress()
 {
   ChassisGimbalShooterManual::wPress();
-  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
+  {
     last_switch_time_ = ros::Time::now();
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_);
+  }
+}
+
+void ChassisGimbalShooterCoverManual::aPress()
+{
+  ChassisGimbalShooterManual::aPress();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::sPress()
+{
+  ChassisGimbalShooterManual::sPress();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::dPress()
+{
+  ChassisGimbalShooterManual::dPress();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_);
 }
 
 void ChassisGimbalShooterCoverManual::wPressing()

src/chassis_gimbal_shooter_manual.cpp

@@ -425,17 +425,18 @@ void ChassisGimbalShooterManual::bRelease()
 
 void ChassisGimbalShooterManual::rPress()
 {
-  if (camera_switch_cmd_sender_)
-    camera_switch_cmd_sender_->switchCamera();
-  if (scope_cmd_sender_)
+  if (robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO)
   {
-    use_scope_ = !scope_cmd_sender_->getState();
-    if (use_scope_)
-      gimbal_cmd_sender_->setEject(true);
-    else
+    if (scope_cmd_sender_)
     {
-      gimbal_cmd_sender_->setEject(false);
-      adjust_image_transmission_ = false;
+      use_scope_ = !scope_cmd_sender_->getState();
+      if (use_scope_)
+        gimbal_cmd_sender_->setEject(true);
+      else
+      {
+        gimbal_cmd_sender_->setEject(false);
+        adjust_image_transmission_ = false;
+      }
     }
   }
 }