Modify method to achieve gyro speed slower when shooting buff.

rm-controls · Aug 4, 2024 · 15c6831 · 15c6831
1 parent bd59fb9
commit 15c6831
Show file tree

Hide file tree

Showing 5 changed files with 98 additions and 25 deletions.
diff --git a/include/rm_manual/chassis_gimbal_manual.h b/include/rm_manual/chassis_gimbal_manual.h
@@ -64,7 +64,6 @@ class ChassisGimbalManual : public ManualBase
   double x_scale_{}, y_scale_{};
   bool is_gyro_{ 0 };
   double speed_change_scale_{ 1. };
-  double gyro_speed_change_scale_{ 1. };
   double gimbal_scale_{ 1. };
   double gyro_move_reduction_{ 1. };
   double gyro_rotate_reduction_{ 1. };

diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
@@ -36,15 +36,22 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   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()
   {
     gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
   };
   double low_speed_scale_{}, normal_speed_scale_{};
-  double low_gyro_speed_scale_{}, normal_gyro_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_{};

diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp
@@ -230,9 +230,9 @@ void ChassisGimbalManual::setChassisMode(int mode)
       chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
       is_gyro_ = true;
       if (x_scale_ != 0.0 || y_scale_ != 0.0)
-        vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_ * gyro_speed_change_scale_);
+        vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
       else
-        vel_cmd_sender_->setAngularZVel(1.0 * gyro_speed_change_scale_);
+        vel_cmd_sender_->setAngularZVel(1.0);
       break;
     case rm_msgs::ChassisCmd::FOLLOW:
       chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);

diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp
@@ -21,9 +21,8 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle
   ros::NodeHandle chassis_nh(nh, "chassis");
   normal_speed_scale_ = chassis_nh.param("normal_speed_scale", 1);
   low_speed_scale_ = chassis_nh.param("low_speed_scale", 0.30);
-  normal_gyro_speed_scale_ = chassis_nh.param("normal_gyro_speed_scale", 1);
-  low_gyro_speed_scale_ = chassis_nh.param("low_gyro_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_, 9.0);
 
   ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this),
                         boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this));
@@ -44,15 +43,21 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode)
   }
 }
 
-void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode gyro_speed_mode)
+void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode)
 {
-  if (gyro_speed_mode == LOW)
+  if (speed_mode == LOW)
   {
-    gyro_speed_change_scale_ = low_gyro_speed_scale_;
+    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_);
   }
-  else if (gyro_speed_mode == NORMAL)
+  else if (speed_mode == NORMAL)
   {
-    gyro_speed_change_scale_ = normal_gyro_speed_scale_;
+    if (x_scale_ != 0.0 || y_scale_ != 0.0)
+      vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
+    else
+      vel_cmd_sender_->setAngularZVel(1.0);
   }
 }
 
@@ -157,23 +162,38 @@ void ChassisGimbalShooterCoverManual::ePress()
   switch_detection_srv_->callService();
   switch_buff_type_srv_->callService();
   switch_exposure_srv_->callService();
-  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
-    changeGyroSpeedMode(LOW);
+  if (is_gyro_)
+  {
+    if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+      changeGyroSpeedMode(LOW);
+    else
+      changeGyroSpeedMode(NORMAL);
+  }
 }
 
 void ChassisGimbalShooterCoverManual::cPress()
 {
-  ChassisGimbalShooterManual::cPress();
-  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
-    changeGyroSpeedMode(LOW);
+  if (is_gyro_)
+  {
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
+  }
   else
-    changeGyroSpeedMode(NORMAL);
+  {
+    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()
 {
   last_shoot_freq_ = shooter_cmd_sender_->getShootFrequency();
   shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::MINIMAL);
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_);
 }
 
 void ChassisGimbalShooterCoverManual::zRelease()
@@ -203,8 +223,55 @@ 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, buff_gyro_rotate_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, buff_gyro_rotate_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, buff_gyro_rotate_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::dPressing()
+{
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_);
+}
+
+void ChassisGimbalShooterCoverManual::wRelease()
+{
+  ChassisGimbalShooterManual::wRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, buff_gyro_rotate_limit_);
+};
+void ChassisGimbalShooterCoverManual::aRelease()
+{
+  ChassisGimbalShooterManual::aRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, buff_gyro_rotate_limit_);
+};
+void ChassisGimbalShooterCoverManual::sRelease()
+{
+  ChassisGimbalShooterManual::sRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, buff_gyro_rotate_limit_);
+};
+void ChassisGimbalShooterCoverManual::dRelease()
+{
+  ChassisGimbalShooterManual::dRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+    vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, buff_gyro_rotate_limit_);
+};
+
 void ChassisGimbalShooterCoverManual::ctrlZPress()
 {
   if (!cover_command_sender_->getState())

diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
@@ -501,45 +501,45 @@ void ChassisGimbalShooterManual::dPress()
 void ChassisGimbalShooterManual::wRelease()
 {
   ChassisGimbalManual::wRelease();
-  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_speed_change_scale_ : 0);
+  vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0);
 }
 void ChassisGimbalShooterManual::aRelease()
 {
   ChassisGimbalManual::aRelease();
-  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_speed_change_scale_ : 0);
+  vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0);
 }
 void ChassisGimbalShooterManual::sRelease()
 {
   ChassisGimbalManual::sRelease();
-  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_speed_change_scale_ : 0);
+  vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0);
 }
 void ChassisGimbalShooterManual::dRelease()
 {
   ChassisGimbalManual::dRelease();
-  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_speed_change_scale_ : 0);
+  vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0);
 }
 void ChassisGimbalShooterManual::wPressing()
 {
   ChassisGimbalManual::wPressing();
-  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ * gyro_speed_change_scale_ : 0);
+  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0);
 }
 
 void ChassisGimbalShooterManual::aPressing()
 {
   ChassisGimbalManual::aPressing();
-  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ * gyro_speed_change_scale_ : 0);
+  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0);
 }
 
 void ChassisGimbalShooterManual::sPressing()
 {
   ChassisGimbalManual::sPressing();
-  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ * gyro_speed_change_scale_ : 0);
+  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0);
 }
 
 void ChassisGimbalShooterManual::dPressing()
 {
   ChassisGimbalManual::dPressing();
-  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ * gyro_speed_change_scale_ : 0);
+  vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0);
 }
 
 void ChassisGimbalShooterManual::xPress()