Merge pull request #100 from liyixin135/standard1

Slow gyro's speed when shooting buff
rm-controls · Aug 4, 2024 · 0c69cc4 · 0c69cc4
2 parents ee7b016 + 4513b83
commit 0c69cc4
Show file tree

Hide file tree

Showing 4 changed files with 107 additions and 21 deletions.
diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
@@ -20,21 +20,29 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
 
 protected:
   void changeSpeedMode(SpeedMode speed_mode);
+  void changeGyroSpeedMode(SpeedMode speed_mode);
   void updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
   void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
   void checkReferee() override;
   void sendCommand(const ros::Time& time) override;
   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 wPressing() override;
+  void aPressing() override;
+  void sPressing() override;
+  void dPressing() override;
+  void wRelease() override;
+  void aRelease() override;
+  void sRelease() override;
+  void dRelease() override;
 
   virtual void ctrlZPress();
   virtual void ctrlZRelease()
@@ -43,6 +51,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   };
   double low_speed_scale_{}, normal_speed_scale_{};
   double exit_buff_mode_duration_{};
+  double gyro_speed_limit_{};
   rm_common::SwitchDetectionCaller* switch_buff_srv_{};
   rm_common::SwitchDetectionCaller* switch_buff_type_srv_{};
   rm_common::SwitchDetectionCaller* switch_exposure_srv_{};

diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/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);

diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/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("gyro_speed_limit", gyro_speed_limit_, 6.0);
 
   ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this),
                         boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this));
@@ -42,6 +43,24 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode)
   }
 }
 
+void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode)
+{
+  if (speed_mode == LOW)
+  {
+    if (x_scale_ != 0.0 || y_scale_ != 0.0)
+      vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, gyro_speed_limit_);
+    else
+      vel_cmd_sender_->setAngularZVel(1.0, gyro_speed_limit_);
+  }
+  else if (speed_mode == NORMAL)
+  {
+    if (x_scale_ != 0.0 || y_scale_ != 0.0)
+      vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
+    else
+      vel_cmd_sender_->setAngularZVel(1.0);
+  }
+}
+
 void ChassisGimbalShooterCoverManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data)
 {
   ChassisGimbalShooterManual::updatePc(dbus_data);
@@ -133,21 +152,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 +162,30 @@ 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)
+      changeGyroSpeedMode(LOW);
+    else
+      changeGyroSpeedMode(NORMAL);
+  }
+}
+
+void ChassisGimbalShooterCoverManual::cPress()
+{
+  if (is_gyro_)
+  {
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
+  }
+  else
+  {
+    setChassisMode(rm_msgs::ChassisCmd::RAW);
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+    if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+      changeGyroSpeedMode(LOW);
+    else
+      changeGyroSpeedMode(NORMAL);
+  }
 }
 
 void ChassisGimbalShooterCoverManual::zPress()
@@ -193,6 +221,57 @@ void ChassisGimbalShooterCoverManual::wPressing()
     switch_buff_type_srv_->callService();
     switch_exposure_srv_->callService();
   }
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, gyro_speed_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::aPressing()
+{
+  ChassisGimbalShooterManual::aPressing();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, gyro_speed_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::sPressing()
+{
+  ChassisGimbalShooterManual::sPressing();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, gyro_speed_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::dPressing()
+{
+  ChassisGimbalShooterManual::dPressing();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, gyro_speed_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::wRelease()
+{
+  ChassisGimbalShooterManual::wRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, gyro_speed_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::aRelease()
+{
+  ChassisGimbalShooterManual::aRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, gyro_speed_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::sRelease()
+{
+  ChassisGimbalShooterManual::sRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, gyro_speed_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::dRelease()
+{
+  ChassisGimbalShooterManual::dRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, gyro_speed_limit_);
 }
 
 void ChassisGimbalShooterCoverManual::ctrlZPress()

diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
@@ -425,8 +425,6 @@ void ChassisGimbalShooterManual::bRelease()
 
 void ChassisGimbalShooterManual::rPress()
 {
-  if (camera_switch_cmd_sender_)
-    camera_switch_cmd_sender_->switchCamera();
   if (scope_cmd_sender_)
   {
     use_scope_ = !scope_cmd_sender_->getState();