Move sentryMode in ChassisGimbalShooterCoverManual.

rm-controls · Jul 19, 2024 · f2df116 · f2df116
1 parent 617b8c3
commit f2df116
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 37 deletions.
diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
@@ -33,12 +33,15 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   void zRelease();
   void wPress() override;
   void wPressing() override;
+  void mouseRightPress() override;
 
   virtual void ctrlZPress();
   virtual void ctrlZRelease()
   {
     gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
   };
+  void sentryMode();
+
   double low_speed_scale_{}, normal_speed_scale_{};
   double exit_buff_mode_duration_{};
   rm_common::SwitchDetectionCaller* switch_buff_srv_{};

diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -59,7 +59,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
     shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
     prepare_shoot_ = true;
   }
-  void mouseRightPress();
+  virtual void mouseRightPress();
   void mouseRightRelease()
   {
     gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
@@ -106,7 +106,6 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void ctrlBPress();
   void ctrlRPress();
   void judgeIsAuto();
-  void sentryMode();
   virtual void ctrlQPress();
   void eventDartCallback(const rm_msgs::EventData ::ConstPtr& data) override;
 

diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp
@@ -211,4 +211,42 @@ void ChassisGimbalShooterCoverManual::ctrlZPress()
   }
 }
 
+void ChassisGimbalShooterCoverManual::mouseRightPress()
+{
+  if (is_auto_)
+    sentryMode();
+}
+
+void ChassisGimbalShooterCoverManual::sentryMode()
+{
+  if (!is_gyro_)
+  {
+    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)
+      vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
+    else
+      vel_cmd_sender_->setAngularZVel(1.0);
+  }
+  if (track_data_.id == 0)
+  {
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ);
+    double traj_yaw, traj_pitch;
+    traj_yaw = M_PI * count_ / 900;
+    traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2;
+    count_ = (count_ + 1) % 900;
+    gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch);
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
+    count_++;
+  }
+  else
+  {
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
+    gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed());
+    if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP)
+      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
+  }
+}
 }  // namespace rm_manual
diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
@@ -367,9 +367,7 @@ void ChassisGimbalShooterManual::mouseLeftPress()
 
 void ChassisGimbalShooterManual::mouseRightPress()
 {
-  if (is_auto_ && robot_id_ != rm_msgs::GameRobotStatus::BLUE_HERO && robot_id_ != rm_msgs::GameRobotStatus::RED_HERO)
-    sentryMode();
-  else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH)
+  if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH)
     shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
   else
   {
@@ -671,36 +669,4 @@ void ChassisGimbalShooterManual::judgeIsAuto()
     is_auto_ = false;
 }
 
-void ChassisGimbalShooterManual::sentryMode()
-{
-  if (!is_gyro_)
-  {
-    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)
-      vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
-    else
-      vel_cmd_sender_->setAngularZVel(1.0);
-  }
-  if (track_data_.id == 0)
-  {
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ);
-    double traj_yaw, traj_pitch;
-    traj_yaw = M_PI * count_ / 900;
-    traj_pitch = 0.15 * sin(2 * M_PI * count_ / 900) + 0.2;
-    count_ = (count_ + 1) % 900;
-    gimbal_cmd_sender_->setYawAndPitchTraj(traj_yaw, traj_pitch);
-    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
-    count_++;
-  }
-  else
-  {
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
-    gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed());
-    if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP)
-      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
-    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
-  }
-}
 }  // namespace rm_manual