From 355531a6f1c8f419d5342913235e31b2fb9b103b Mon Sep 17 00:00:00 2001
From: BlanchardLj <1714148437@qq.com>
Date: Mon, 8 Jul 2024 17:48:54 +0800
Subject: [PATCH 1/4] Add a function named modeFSM.

---
 .../rm_manual/chassis_gimbal_shooter_manual.h |  15 ++
 src/chassis_gimbal_shooter_manual.cpp         | 167 +++++++++++-------
 2 files changed, 117 insertions(+), 65 deletions(-)

diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
index bd147d4b..e5b207fb 100644
--- a/include/rm_manual/chassis_gimbal_shooter_manual.h
+++ b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -51,6 +51,21 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) override;
   void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override;
   void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
+  void modeFSM(int mode);
+  typedef enum
+  {
+    RAW = 0,
+    FOLLOW = 1,
+    RATE = 2,
+    TRACK = 3,
+    DIRECT = 4,
+    STOP = 5,
+    READY = 6,
+    PUSH = 7,
+    CHARGE = 8,
+    BURST = 9,
+    NORMAL = 10
+  } Mode;
   void leftSwitchUpOn(ros::Duration duration);
   void mouseLeftPress();
   void mouseLeftRelease()
diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
index 2fcffb68..12c8a319 100644
--- a/src/chassis_gimbal_shooter_manual.cpp
+++ b/src/chassis_gimbal_shooter_manual.cpp
@@ -182,6 +182,55 @@ void ChassisGimbalShooterManual::sendCommand(const ros::Time& time)
   }
 }
 
+void ChassisGimbalShooterManual::modeFSM(int mode)
+{
+  switch (mode)
+  {
+    case RAW:
+      chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
+      is_gyro_ = true;
+      break;
+    case FOLLOW:
+      chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
+      is_gyro_ = false;
+      break;
+    case RATE:
+      gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
+      break;
+    case TRACK:
+      gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
+      gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed());
+      break;
+    case DIRECT:
+      gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::DIRECT);
+      gimbal_cmd_sender_->setPoint(point_out_);
+      break;
+    case STOP:
+      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
+      break;
+    case READY:
+      if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP)
+        prepare_shoot_ = false;
+      else
+        prepare_shoot_ = true;
+      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
+      break;
+    case PUSH:
+      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
+      shooter_cmd_sender_->checkError(ros::Time::now());
+      break;
+    case CHARGE:
+      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
+      break;
+    case BURST:
+      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
+      break;
+    case NORMAL:
+      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+      break;
+  }
+}
+
 void ChassisGimbalShooterManual::remoteControlTurnOff()
 {
   ChassisGimbalManual::remoteControlTurnOff();
@@ -205,7 +254,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOn()
 void ChassisGimbalShooterManual::robotDie()
 {
   ManualBase::robotDie();
-  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
+  modeFSM(STOP);
   turn_flag_ = false;
   use_scope_ = false;
   adjust_image_transmission_ = false;
@@ -214,13 +263,13 @@ void ChassisGimbalShooterManual::robotDie()
 void ChassisGimbalShooterManual::chassisOutputOn()
 {
   ChassisGimbalManual::chassisOutputOn();
-  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
+  modeFSM(CHARGE);
 }
 
 void ChassisGimbalShooterManual::shooterOutputOn()
 {
   ChassisGimbalManual::shooterOutputOn();
-  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
+  modeFSM(STOP);
   shooter_calibration_->reset();
 }
 
@@ -235,22 +284,20 @@ void ChassisGimbalShooterManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbu
   ChassisGimbalManual::updateRc(dbus_data);
   if (std::abs(dbus_data->wheel) > 0.01)
   {
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
-    is_gyro_ = true;
+    modeFSM(RAW);
   }
   else
   {
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
-    is_gyro_ = false;
+    modeFSM(FOLLOW);
   }
   vel_cmd_sender_->setAngularZVel((std::abs(dbus_data->ch_r_y) > 0.01 || std::abs(dbus_data->ch_r_x) > 0.01) ?
                                       dbus_data->wheel * gyro_rotate_reduction_ :
                                       dbus_data->wheel);
   vel_cmd_sender_->setLinearXVel(is_gyro_ ? dbus_data->ch_r_y * gyro_move_reduction_ : dbus_data->ch_r_y);
   vel_cmd_sender_->setLinearYVel(is_gyro_ ? -dbus_data->ch_r_x * gyro_move_reduction_ : -dbus_data->ch_r_x);
-
+  // RC下，shooter不是STOP就setBulletSpeed
   if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP)
-    gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed());
+    modeFSM(TRACK);
 }
 
 void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data)
@@ -258,112 +305,104 @@ void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbu
   ChassisGimbalManual::updatePc(dbus_data);
   if (chassis_cmd_sender_->power_limit_->getState() != rm_common::PowerLimit::BURST && !is_gyro_ && !is_balance_)
   {  // Capacitor enter fast charge when chassis stop.
+    // 底盘不动才充电, PC
     if (!dbus_data->key_shift && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW &&
         std::sqrt(std::pow(vel_cmd_sender_->getMsg()->linear.x, 2) + std::pow(vel_cmd_sender_->getMsg()->linear.y, 2)) >
             0.0)
-      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+      modeFSM(NORMAL);
     else if (chassis_power_ < 6.0 && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW)
-      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
+      modeFSM(CHARGE);
   }
 }
 
 void ChassisGimbalShooterManual::rightSwitchDownRise()
 {
   ChassisGimbalManual::rightSwitchDownRise();
-  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
-  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
+  modeFSM(NORMAL);
+  modeFSM(STOP);
 }
 
 void ChassisGimbalShooterManual::rightSwitchMidRise()
 {
   ChassisGimbalManual::rightSwitchMidRise();
-  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
-  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
+  modeFSM(BURST);
+  modeFSM(STOP);
 }
 
 void ChassisGimbalShooterManual::rightSwitchUpRise()
 {
   ChassisGimbalManual::rightSwitchUpRise();
-  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
-  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
+  modeFSM(CHARGE);
+  modeFSM(STOP);
 }
 
 void ChassisGimbalShooterManual::leftSwitchDownRise()
 {
   ChassisGimbalManual::leftSwitchDownRise();
-  gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
-  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
+  modeFSM(RATE);
+  modeFSM(STOP);
 }
 
 void ChassisGimbalShooterManual::leftSwitchMidRise()
 {
   ChassisGimbalManual::leftSwitchMidRise();
-  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
+  modeFSM(READY);
 }
 
 void ChassisGimbalShooterManual::leftSwitchMidOn(ros::Duration duration)
 {
   if (track_data_.id == 0)
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
+    modeFSM(RATE);
   else
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
+    modeFSM(TRACK);
 }
 
 void ChassisGimbalShooterManual::leftSwitchUpRise()
 {
   ChassisGimbalManual::leftSwitchUpRise();
-  gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
+  modeFSM(TRACK);
 }
 
 void ChassisGimbalShooterManual::leftSwitchUpOn(ros::Duration duration)
 {
   if (track_data_.id == 0)
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
+    modeFSM(RATE);
   else
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
+    modeFSM(TRACK);
   if (duration > ros::Duration(1.))
   {
-    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
-    shooter_cmd_sender_->checkError(ros::Time::now());
+    modeFSM(PUSH);
   }
   else if (duration < ros::Duration(0.02))
   {
-    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
-    shooter_cmd_sender_->checkError(ros::Time::now());
+    modeFSM(PUSH);
   }
   else
-    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
+    modeFSM(READY);
 }
 
 void ChassisGimbalShooterManual::mouseLeftPress()
 {
-  if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP)
-  {
-    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
-    prepare_shoot_ = false;
-  }
+  modeFSM(READY);
   if (prepare_shoot_)
   {
-    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
-    shooter_cmd_sender_->checkError(ros::Time::now());
+    modeFSM(PUSH);
   }
 }
 
 void ChassisGimbalShooterManual::mouseRightPress()
 {
   if (track_data_.id == 0)
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
+    modeFSM(RATE);
   else
   {
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
-    gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed());
+    modeFSM(TRACK);
   }
   if (switch_armor_target_srv_->getArmorTarget() == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)
   {
     if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP)
     {
-      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
-      shooter_cmd_sender_->checkError(ros::Time::now());
+      modeFSM(PUSH);
     }
   }
 }
@@ -386,15 +425,13 @@ void ChassisGimbalShooterManual::cPress()
 {
   if (is_gyro_)
   {
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
+    modeFSM(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);
+    modeFSM(RAW);
+    modeFSM(NORMAL);
     if (x_scale_ != 0.0 || y_scale_ != 0.0)
       vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
     else
@@ -404,12 +441,12 @@ void ChassisGimbalShooterManual::cPress()
 
 void ChassisGimbalShooterManual::bPress()
 {
-  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
+  modeFSM(CHARGE);
 }
 
 void ChassisGimbalShooterManual::bRelease()
 {
-  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+  modeFSM(NORMAL);
 }
 
 void ChassisGimbalShooterManual::rPress()
@@ -440,10 +477,11 @@ void ChassisGimbalShooterManual::wPress()
   if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) &&
       gimbal_cmd_sender_->getEject())
   {
+    // TODO 开倍镜才能进，这里getEject()只能是false
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
-    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+    modeFSM(FOLLOW);
+    modeFSM(NORMAL);
   }
 }
 
@@ -455,8 +493,8 @@ void ChassisGimbalShooterManual::aPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
-    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+    modeFSM(FOLLOW);
+    modeFSM(NORMAL);
   }
 }
 
@@ -468,8 +506,8 @@ void ChassisGimbalShooterManual::sPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
-    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+    modeFSM(FOLLOW);
+    modeFSM(NORMAL);
   }
 }
 
@@ -481,8 +519,8 @@ void ChassisGimbalShooterManual::dPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
-    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+    modeFSM(FOLLOW);
+    modeFSM(NORMAL);
   }
 }
 
@@ -555,13 +593,12 @@ void ChassisGimbalShooterManual::xReleasing()
 {
   if (turn_flag_)
   {
-    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::DIRECT);
-    gimbal_cmd_sender_->setPoint(point_out_);
+    modeFSM(DIRECT);
     double roll{}, pitch{}, yaw{};
     quatToRPY(tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw);
     if (std::abs(angles::shortest_angular_distance(yaw, yaw_current_)) > finish_turning_threshold_)
     {
-      gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
+      modeFSM(RATE);
       turn_flag_ = false;
     }
   }
@@ -576,21 +613,21 @@ void ChassisGimbalShooterManual::shiftPress()
 {
   if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::FOLLOW && is_gyro_)
   {
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
+    modeFSM(FOLLOW);
     vel_cmd_sender_->setAngularZVel(1.0);
-    is_gyro_ = false;
   }
-  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
+  modeFSM(BURST);
 }
 
 void ChassisGimbalShooterManual::shiftRelease()
 {
+  // 松shift，底盘不动的时候CHARGE
   if (chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::RAW ||
       std::sqrt(std::pow(vel_cmd_sender_->getMsg()->linear.x, 2) + std::pow(vel_cmd_sender_->getMsg()->linear.y, 2)) >
           0.0)
-    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+    modeFSM(NORMAL);
   else
-    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
+    modeFSM(CHARGE);
 }
 
 void ChassisGimbalShooterManual::ctrlVPress()

From ad2a89df1aa8800c6d52c5d4eecd5c5f39b0aeb1 Mon Sep 17 00:00:00 2001
From: BlanchardLj <1714148437@qq.com>
Date: Wed, 10 Jul 2024 20:36:53 +0800
Subject: [PATCH 2/4] Update: only package chassis mode.

---
 .../rm_manual/chassis_gimbal_shooter_manual.h |  16 +-
 src/chassis_gimbal_shooter_manual.cpp         | 153 ++++++++----------
 2 files changed, 64 insertions(+), 105 deletions(-)

diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
index e5b207fb..9f51be4f 100644
--- a/include/rm_manual/chassis_gimbal_shooter_manual.h
+++ b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -51,21 +51,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) override;
   void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override;
   void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
-  void modeFSM(int mode);
-  typedef enum
-  {
-    RAW = 0,
-    FOLLOW = 1,
-    RATE = 2,
-    TRACK = 3,
-    DIRECT = 4,
-    STOP = 5,
-    READY = 6,
-    PUSH = 7,
-    CHARGE = 8,
-    BURST = 9,
-    NORMAL = 10
-  } Mode;
+  void chassisMode(int mode);
   void leftSwitchUpOn(ros::Duration duration);
   void mouseLeftPress();
   void mouseLeftRelease()
diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
index 12c8a319..2c35481d 100644
--- a/src/chassis_gimbal_shooter_manual.cpp
+++ b/src/chassis_gimbal_shooter_manual.cpp
@@ -182,52 +182,18 @@ void ChassisGimbalShooterManual::sendCommand(const ros::Time& time)
   }
 }
 
-void ChassisGimbalShooterManual::modeFSM(int mode)
+void ChassisGimbalShooterManual::chassisMode(int mode)
 {
   switch (mode)
   {
-    case RAW:
+    case rm_msgs::ChassisCmd::RAW:
       chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
       is_gyro_ = true;
       break;
-    case FOLLOW:
+    case rm_msgs::ChassisCmd::FOLLOW:
       chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
       is_gyro_ = false;
       break;
-    case RATE:
-      gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
-      break;
-    case TRACK:
-      gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
-      gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed());
-      break;
-    case DIRECT:
-      gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::DIRECT);
-      gimbal_cmd_sender_->setPoint(point_out_);
-      break;
-    case STOP:
-      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
-      break;
-    case READY:
-      if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP)
-        prepare_shoot_ = false;
-      else
-        prepare_shoot_ = true;
-      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
-      break;
-    case PUSH:
-      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
-      shooter_cmd_sender_->checkError(ros::Time::now());
-      break;
-    case CHARGE:
-      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
-      break;
-    case BURST:
-      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
-      break;
-    case NORMAL:
-      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
-      break;
   }
 }
 
@@ -254,7 +220,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOn()
 void ChassisGimbalShooterManual::robotDie()
 {
   ManualBase::robotDie();
-  modeFSM(STOP);
+  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
   turn_flag_ = false;
   use_scope_ = false;
   adjust_image_transmission_ = false;
@@ -263,13 +229,13 @@ void ChassisGimbalShooterManual::robotDie()
 void ChassisGimbalShooterManual::chassisOutputOn()
 {
   ChassisGimbalManual::chassisOutputOn();
-  modeFSM(CHARGE);
+  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
 }
 
 void ChassisGimbalShooterManual::shooterOutputOn()
 {
   ChassisGimbalManual::shooterOutputOn();
-  modeFSM(STOP);
+  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
   shooter_calibration_->reset();
 }
 
@@ -284,20 +250,20 @@ void ChassisGimbalShooterManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbu
   ChassisGimbalManual::updateRc(dbus_data);
   if (std::abs(dbus_data->wheel) > 0.01)
   {
-    modeFSM(RAW);
+    chassisMode(rm_msgs::ChassisCmd::RAW);
   }
   else
   {
-    modeFSM(FOLLOW);
+    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
   }
   vel_cmd_sender_->setAngularZVel((std::abs(dbus_data->ch_r_y) > 0.01 || std::abs(dbus_data->ch_r_x) > 0.01) ?
                                       dbus_data->wheel * gyro_rotate_reduction_ :
                                       dbus_data->wheel);
   vel_cmd_sender_->setLinearXVel(is_gyro_ ? dbus_data->ch_r_y * gyro_move_reduction_ : dbus_data->ch_r_y);
   vel_cmd_sender_->setLinearYVel(is_gyro_ ? -dbus_data->ch_r_x * gyro_move_reduction_ : -dbus_data->ch_r_x);
-  // RC下，shooter不是STOP就setBulletSpeed
+
   if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP)
-    modeFSM(TRACK);
+    gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed());
 }
 
 void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data)
@@ -305,104 +271,112 @@ void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbu
   ChassisGimbalManual::updatePc(dbus_data);
   if (chassis_cmd_sender_->power_limit_->getState() != rm_common::PowerLimit::BURST && !is_gyro_ && !is_balance_)
   {  // Capacitor enter fast charge when chassis stop.
-    // 底盘不动才充电, PC
     if (!dbus_data->key_shift && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW &&
         std::sqrt(std::pow(vel_cmd_sender_->getMsg()->linear.x, 2) + std::pow(vel_cmd_sender_->getMsg()->linear.y, 2)) >
             0.0)
-      modeFSM(NORMAL);
+      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
     else if (chassis_power_ < 6.0 && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW)
-      modeFSM(CHARGE);
+      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
   }
 }
 
 void ChassisGimbalShooterManual::rightSwitchDownRise()
 {
   ChassisGimbalManual::rightSwitchDownRise();
-  modeFSM(NORMAL);
-  modeFSM(STOP);
+  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
 }
 
 void ChassisGimbalShooterManual::rightSwitchMidRise()
 {
   ChassisGimbalManual::rightSwitchMidRise();
-  modeFSM(BURST);
-  modeFSM(STOP);
+  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
+  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
 }
 
 void ChassisGimbalShooterManual::rightSwitchUpRise()
 {
   ChassisGimbalManual::rightSwitchUpRise();
-  modeFSM(CHARGE);
-  modeFSM(STOP);
+  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
+  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
 }
 
 void ChassisGimbalShooterManual::leftSwitchDownRise()
 {
   ChassisGimbalManual::leftSwitchDownRise();
-  modeFSM(RATE);
-  modeFSM(STOP);
+  gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
+  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
 }
 
 void ChassisGimbalShooterManual::leftSwitchMidRise()
 {
   ChassisGimbalManual::leftSwitchMidRise();
-  modeFSM(READY);
+  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
 }
 
 void ChassisGimbalShooterManual::leftSwitchMidOn(ros::Duration duration)
 {
   if (track_data_.id == 0)
-    modeFSM(RATE);
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
   else
-    modeFSM(TRACK);
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
 }
 
 void ChassisGimbalShooterManual::leftSwitchUpRise()
 {
   ChassisGimbalManual::leftSwitchUpRise();
-  modeFSM(TRACK);
+  gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
 }
 
 void ChassisGimbalShooterManual::leftSwitchUpOn(ros::Duration duration)
 {
   if (track_data_.id == 0)
-    modeFSM(RATE);
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
   else
-    modeFSM(TRACK);
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
   if (duration > ros::Duration(1.))
   {
-    modeFSM(PUSH);
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
+    shooter_cmd_sender_->checkError(ros::Time::now());
   }
   else if (duration < ros::Duration(0.02))
   {
-    modeFSM(PUSH);
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
+    shooter_cmd_sender_->checkError(ros::Time::now());
   }
   else
-    modeFSM(READY);
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
 }
 
 void ChassisGimbalShooterManual::mouseLeftPress()
 {
-  modeFSM(READY);
+  if (shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::STOP)
+  {
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
+    prepare_shoot_ = false;
+  }
   if (prepare_shoot_)
   {
-    modeFSM(PUSH);
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
+    shooter_cmd_sender_->checkError(ros::Time::now());
   }
 }
 
 void ChassisGimbalShooterManual::mouseRightPress()
 {
   if (track_data_.id == 0)
-    modeFSM(RATE);
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
   else
   {
-    modeFSM(TRACK);
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
+    gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed());
   }
   if (switch_armor_target_srv_->getArmorTarget() == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)
   {
     if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP)
     {
-      modeFSM(PUSH);
+      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
+      shooter_cmd_sender_->checkError(ros::Time::now());
     }
   }
 }
@@ -425,13 +399,13 @@ void ChassisGimbalShooterManual::cPress()
 {
   if (is_gyro_)
   {
-    modeFSM(FOLLOW);
+    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
     vel_cmd_sender_->setAngularZVel(0.0);
   }
   else
   {
-    modeFSM(RAW);
-    modeFSM(NORMAL);
+    chassisMode(rm_msgs::ChassisCmd::RAW);
+    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
@@ -441,12 +415,12 @@ void ChassisGimbalShooterManual::cPress()
 
 void ChassisGimbalShooterManual::bPress()
 {
-  modeFSM(CHARGE);
+  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
 }
 
 void ChassisGimbalShooterManual::bRelease()
 {
-  modeFSM(NORMAL);
+  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
 }
 
 void ChassisGimbalShooterManual::rPress()
@@ -477,11 +451,10 @@ void ChassisGimbalShooterManual::wPress()
   if ((robot_id_ == rm_msgs::GameRobotStatus::BLUE_HERO || robot_id_ == rm_msgs::GameRobotStatus::RED_HERO) &&
       gimbal_cmd_sender_->getEject())
   {
-    // TODO 开倍镜才能进，这里getEject()只能是false
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    modeFSM(FOLLOW);
-    modeFSM(NORMAL);
+    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   }
 }
 
@@ -493,8 +466,8 @@ void ChassisGimbalShooterManual::aPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    modeFSM(FOLLOW);
-    modeFSM(NORMAL);
+    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   }
 }
 
@@ -506,8 +479,8 @@ void ChassisGimbalShooterManual::sPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    modeFSM(FOLLOW);
-    modeFSM(NORMAL);
+    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   }
 }
 
@@ -519,8 +492,8 @@ void ChassisGimbalShooterManual::dPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    modeFSM(FOLLOW);
-    modeFSM(NORMAL);
+    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   }
 }
 
@@ -593,12 +566,13 @@ void ChassisGimbalShooterManual::xReleasing()
 {
   if (turn_flag_)
   {
-    modeFSM(DIRECT);
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::DIRECT);
+    gimbal_cmd_sender_->setPoint(point_out_);
     double roll{}, pitch{}, yaw{};
     quatToRPY(tf_buffer_.lookupTransform("odom", "yaw", ros::Time(0)).transform.rotation, roll, pitch, yaw);
     if (std::abs(angles::shortest_angular_distance(yaw, yaw_current_)) > finish_turning_threshold_)
     {
-      modeFSM(RATE);
+      gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
       turn_flag_ = false;
     }
   }
@@ -613,21 +587,20 @@ void ChassisGimbalShooterManual::shiftPress()
 {
   if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::FOLLOW && is_gyro_)
   {
-    modeFSM(FOLLOW);
+    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
     vel_cmd_sender_->setAngularZVel(1.0);
   }
-  modeFSM(BURST);
+  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
 }
 
 void ChassisGimbalShooterManual::shiftRelease()
 {
-  // 松shift，底盘不动的时候CHARGE
   if (chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::RAW ||
       std::sqrt(std::pow(vel_cmd_sender_->getMsg()->linear.x, 2) + std::pow(vel_cmd_sender_->getMsg()->linear.y, 2)) >
           0.0)
-    modeFSM(NORMAL);
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   else
-    modeFSM(CHARGE);
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
 }
 
 void ChassisGimbalShooterManual::ctrlVPress()

From 2ee3f6a3a1ecdbd227bc2145e7701e7fce657220 Mon Sep 17 00:00:00 2001
From: BlanchardLj <1714148437@qq.com>
Date: Thu, 11 Jul 2024 10:06:19 +0800
Subject: [PATCH 3/4] Change the function name to setChassisMode.

---
 .../rm_manual/chassis_gimbal_shooter_manual.h |  2 +-
 src/chassis_gimbal_shooter_manual.cpp         | 20 +++++++++----------
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
index 9f51be4f..7275ca31 100644
--- a/include/rm_manual/chassis_gimbal_shooter_manual.h
+++ b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -51,7 +51,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) override;
   void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override;
   void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
-  void chassisMode(int mode);
+  void setChassisMode(int mode);
   void leftSwitchUpOn(ros::Duration duration);
   void mouseLeftPress();
   void mouseLeftRelease()
diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
index 2c35481d..90dac51a 100644
--- a/src/chassis_gimbal_shooter_manual.cpp
+++ b/src/chassis_gimbal_shooter_manual.cpp
@@ -182,7 +182,7 @@ void ChassisGimbalShooterManual::sendCommand(const ros::Time& time)
   }
 }
 
-void ChassisGimbalShooterManual::chassisMode(int mode)
+void ChassisGimbalShooterManual::setChassisMode(int mode)
 {
   switch (mode)
   {
@@ -250,11 +250,11 @@ void ChassisGimbalShooterManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbu
   ChassisGimbalManual::updateRc(dbus_data);
   if (std::abs(dbus_data->wheel) > 0.01)
   {
-    chassisMode(rm_msgs::ChassisCmd::RAW);
+    setChassisMode(rm_msgs::ChassisCmd::RAW);
   }
   else
   {
-    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
   }
   vel_cmd_sender_->setAngularZVel((std::abs(dbus_data->ch_r_y) > 0.01 || std::abs(dbus_data->ch_r_x) > 0.01) ?
                                       dbus_data->wheel * gyro_rotate_reduction_ :
@@ -399,12 +399,12 @@ void ChassisGimbalShooterManual::cPress()
 {
   if (is_gyro_)
   {
-    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
     vel_cmd_sender_->setAngularZVel(0.0);
   }
   else
   {
-    chassisMode(rm_msgs::ChassisCmd::RAW);
+    setChassisMode(rm_msgs::ChassisCmd::RAW);
     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_);
@@ -453,7 +453,7 @@ void ChassisGimbalShooterManual::wPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
     chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   }
 }
@@ -466,7 +466,7 @@ void ChassisGimbalShooterManual::aPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
     chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   }
 }
@@ -479,7 +479,7 @@ void ChassisGimbalShooterManual::sPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
     chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   }
 }
@@ -492,7 +492,7 @@ void ChassisGimbalShooterManual::dPress()
   {
     gimbal_cmd_sender_->setEject(false);
     manual_to_referee_pub_data_.hero_eject_flag = gimbal_cmd_sender_->getEject();
-    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
     chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   }
 }
@@ -587,7 +587,7 @@ void ChassisGimbalShooterManual::shiftPress()
 {
   if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::FOLLOW && is_gyro_)
   {
-    chassisMode(rm_msgs::ChassisCmd::FOLLOW);
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
     vel_cmd_sender_->setAngularZVel(1.0);
   }
   chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);

From 294508420eff4802ba29160e889b007412de52bb Mon Sep 17 00:00:00 2001
From: BlanchardLj <1714148437@qq.com>
Date: Wed, 17 Jul 2024 15:10:54 +0800
Subject: [PATCH 4/4] Change the function to the parent class.

---
 include/rm_manual/chassis_gimbal_manual.h         |  1 +
 include/rm_manual/chassis_gimbal_shooter_manual.h |  1 -
 src/chassis_gimbal_manual.cpp                     | 15 +++++++++++++++
 src/chassis_gimbal_shooter_manual.cpp             | 15 ---------------
 4 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/include/rm_manual/chassis_gimbal_manual.h b/include/rm_manual/chassis_gimbal_manual.h
index 52b0ccc0..6c07ed8b 100644
--- a/include/rm_manual/chassis_gimbal_manual.h
+++ b/include/rm_manual/chassis_gimbal_manual.h
@@ -30,6 +30,7 @@ class ChassisGimbalManual : public ManualBase
   void dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) override;
   void capacityDataCallback(const rm_msgs::PowerManagementSampleAndStatusData ::ConstPtr& data) override;
   void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
+  void setChassisMode(int mode);
   virtual void wPress()
   {
     x_scale_ = x_scale_ >= 1.0 ? 1.0 : x_scale_ + 1.0;
diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
index 7275ca31..bd147d4b 100644
--- a/include/rm_manual/chassis_gimbal_shooter_manual.h
+++ b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -51,7 +51,6 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) override;
   void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override;
   void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
-  void setChassisMode(int mode);
   void leftSwitchUpOn(ros::Duration duration);
   void mouseLeftPress();
   void mouseLeftRelease()
diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp
index cd250d1c..1ab65aa3 100644
--- a/src/chassis_gimbal_manual.cpp
+++ b/src/chassis_gimbal_manual.cpp
@@ -214,4 +214,19 @@ void ChassisGimbalManual::mouseMidRise(double m_z)
   }
 }
 
+void ChassisGimbalManual::setChassisMode(int mode)
+{
+  switch (mode)
+  {
+    case rm_msgs::ChassisCmd::RAW:
+      chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
+      is_gyro_ = true;
+      break;
+    case rm_msgs::ChassisCmd::FOLLOW:
+      chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
+      is_gyro_ = false;
+      break;
+  }
+}
+
 }  // namespace rm_manual
diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
index 90dac51a..04567b1b 100644
--- a/src/chassis_gimbal_shooter_manual.cpp
+++ b/src/chassis_gimbal_shooter_manual.cpp
@@ -182,21 +182,6 @@ void ChassisGimbalShooterManual::sendCommand(const ros::Time& time)
   }
 }
 
-void ChassisGimbalShooterManual::setChassisMode(int mode)
-{
-  switch (mode)
-  {
-    case rm_msgs::ChassisCmd::RAW:
-      chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
-      is_gyro_ = true;
-      break;
-    case rm_msgs::ChassisCmd::FOLLOW:
-      chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
-      is_gyro_ = false;
-      break;
-  }
-}
-
 void ChassisGimbalShooterManual::remoteControlTurnOff()
 {
   ChassisGimbalManual::remoteControlTurnOff();