Add slow mode for gyro's different speed.

rm-controls · Aug 4, 2024 · 4a8c955 · 4a8c955
1 parent 2a16eea
commit 4a8c955
Show file tree

Hide file tree

Showing 4 changed files with 58 additions and 29 deletions.
diff --git a/include/rm_manual/chassis_gimbal_manual.h b/include/rm_manual/chassis_gimbal_manual.h
@@ -64,6 +64,7 @@ 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
@@ -20,6 +20,7 @@ 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;
@@ -38,13 +39,18 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   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_{};

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_);
+        vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_ * gyro_speed_change_scale_);
       else
-        vel_cmd_sender_->setAngularZVel(1.0);
+        vel_cmd_sender_->setAngularZVel(1.0 * gyro_speed_change_scale_);
       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,6 +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_gryo_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_, 6.0);
 
@@ -43,6 +45,18 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode)
   }
 }
 
+void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode)
+{
+  if (speed_mode == LOW)
+  {
+    gyro_speed_change_scale_ = low_gyro_speed_scale_;
+  }
+  else if (speed_mode == NORMAL)
+  {
+    gyro_speed_change_scale_ = normal_gyro_speed_scale_;
+  }
+}
+
 void ChassisGimbalShooterCoverManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data)
 {
   ChassisGimbalShooterManual::updatePc(dbus_data);
@@ -165,29 +179,9 @@ void ChassisGimbalShooterCoverManual::ePress()
 
 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 (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);
-    }
-  }
+  ChassisGimbalShooterManual::cPress();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
+    changeGyroSpeedMode(LOW);
 }
 
 void ChassisGimbalShooterCoverManual::zPress()
@@ -223,31 +217,59 @@ void ChassisGimbalShooterCoverManual::wPressing()
     switch_buff_type_srv_->callService();
     switch_exposure_srv_->callService();
     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_);
+      changeGyroSpeedMode(LOW);
   }
 }
 
 void ChassisGimbalShooterCoverManual::aPressing()
 {
   ChassisGimbalShooterManual::aPressing();
   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_);
+    changeGyroSpeedMode(LOW);
 }
 
 void ChassisGimbalShooterCoverManual::sPressing()
 {
   ChassisGimbalShooterManual::sPressing();
   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_);
+    changeGyroSpeedMode(LOW);
 }
 
 void ChassisGimbalShooterCoverManual::dPressing()
 {
   ChassisGimbalShooterManual::dPressing();
   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_);
+    changeGyroSpeedMode(LOW);
 }
 
+void ChassisGimbalShooterCoverManual::wRelease()
+{
+  ChassisGimbalShooterManual::wRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
+    changeGyroSpeedMode(LOW);
+};
+
+void ChassisGimbalShooterCoverManual::aRelease()
+{
+  ChassisGimbalShooterManual::aRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
+    changeGyroSpeedMode(LOW);
+};
+
+void ChassisGimbalShooterCoverManual::sRelease()
+{
+  ChassisGimbalShooterManual::sRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
+    changeGyroSpeedMode(LOW);
+};
+
+void ChassisGimbalShooterCoverManual::dRelease()
+{
+  ChassisGimbalShooterManual::dRelease();
+  if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_)
+    changeGyroSpeedMode(LOW);
+};
+
 void ChassisGimbalShooterCoverManual::ctrlZPress()
 {
   if (!cover_command_sender_->getState())