diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt
index 678d931b..726a12e4 100644
--- a/rm_chassis_controllers/CMakeLists.txt
+++ b/rm_chassis_controllers/CMakeLists.txt
@@ -82,6 +82,7 @@ add_library(${PROJECT_NAME}
         src/sentry.cpp
         src/swerve.cpp
         src/balance.cpp
+        src/active_suspension.cpp
         ${BIPEDAL_SOURCES}
 )
 
diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h
new file mode 100644
index 00000000..98447253
--- /dev/null
+++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h
@@ -0,0 +1,53 @@
+//
+// Created by xuncheng on 2025/11/29.
+//
+
+#include "rm_chassis_controllers/chassis_base.h"
+#include "rm_chassis_controllers/omni.h"
+#include <effort_controllers/joint_position_controller.h>
+#include <rm_msgs/ChassisActiveSusCmd.h>
+#include <std_msgs/Bool.h>
+#include <rm_common/ros_utilities.h>
+#include <string>
+#include <Eigen/QR>
+#include <pluginlib/class_list_macros.hpp>
+
+namespace rm_chassis_controllers
+{
+enum class State
+{
+  DOWN = 0,
+  MID = 1,
+  UP = 2
+};
+
+class ActiveSuspensionController : public OmniController
+{
+public:
+  ActiveSuspensionController() = default;
+  bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
+  void ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg);
+
+private:
+  void moveJoint(const ros::Time& time, const ros::Duration& period) override;
+
+  std::vector<std::shared_ptr<effort_controllers::JointPositionController>> active_suspension_joints_;
+  std::vector<hardware_interface::JointHandle> active_suspension_joint_handles_{};
+
+  ros::Subscriber active_suspension_sub_;
+  double current_pos_{ 0. };
+  double target_pos_{ 0. };
+  double feedforward_offset{ 0. };
+  double feedforward_effort{ 0. };
+  double stretch_coff_A_{ 0. };
+  double stretch_coff_k_{ 0. };
+  double shrink_coff_A_{ 0. };
+  double shrink_coff_k_{ 0. };
+  double feedforward_effect_time_{ 0. };
+  double static_effort{ 0. };
+
+  ros::Time feedforward_timer;
+  State current_state_{ State::DOWN };
+  State last_state_{ State::DOWN };
+};
+}  // namespace rm_chassis_controllers
diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h
index 93587dca..faca1374 100644
--- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h
+++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h
@@ -47,6 +47,7 @@
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <nav_msgs/Odometry.h>
+#include <rm_msgs/ChassisActiveSusCmd.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <rm_chassis_controllers/PowerLimitConfig.h>
 
@@ -56,6 +57,7 @@ struct Command
 {
   geometry_msgs::Twist cmd_vel_;
   rm_msgs::ChassisCmd cmd_chassis_;
+  rm_msgs::ChassisActiveSusCmd cmd_active_sus_;
   ros::Time stamp_;
 };
 template <typename... T>
diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h
index e02c5d7f..1bc67336 100644
--- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h
+++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h
@@ -17,11 +17,13 @@ class OmniController : public ChassisBase<rm_control::RobotStateInterface, hardw
   bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
 
 private:
-  void moveJoint(const ros::Time& time, const ros::Duration& period) override;
   geometry_msgs::Twist odometry() override;
 
   std::vector<std::shared_ptr<effort_controllers::JointVelocityController>> joints_;
   Eigen::MatrixXd chassis2joints_;
+
+protected:
+  void moveJoint(const ros::Time& time, const ros::Duration& period) override;
 };
 
 }  // namespace rm_chassis_controllers
diff --git a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml
index d1f951d5..93c4fdce 100644
--- a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml
+++ b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml
@@ -8,6 +8,13 @@
             type of hardware interface.
         </description>
     </class>
+    <class name="rm_chassis_controllers/ActiveSuspensionController"
+           type="rm_chassis_controllers::ActiveSuspensionController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The ActiveSuspensionController .
+        </description>
+    </class>
     <class name="rm_chassis_controllers/SentryController"
            type="rm_chassis_controllers::SentryController"
            base_class_type="controller_interface::ControllerBase">
diff --git a/rm_chassis_controllers/src/active_suspension.cpp b/rm_chassis_controllers/src/active_suspension.cpp
new file mode 100644
index 00000000..38ce2ed4
--- /dev/null
+++ b/rm_chassis_controllers/src/active_suspension.cpp
@@ -0,0 +1,88 @@
+//
+// Created by qiayuan on 2022/7/29.
+//
+
+#include "rm_chassis_controllers/active_suspension.h"
+
+namespace rm_chassis_controllers
+{
+bool ActiveSuspensionController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh,
+                                      ros::NodeHandle& controller_nh)
+{
+  OmniController::init(robot_hw, root_nh, controller_nh);
+
+  active_suspension_sub_ = controller_nh.subscribe("/cmd_active_suspension", 10,
+                                                   &ActiveSuspensionController::ActiveSuspensionCallBack, this);
+
+  XmlRpc::XmlRpcValue suspension_legs;
+  controller_nh.getParam("suspension_leg", suspension_legs);
+  stretch_coff_A_ = getParam(controller_nh, "stretch_coff_A_", 0.);
+  stretch_coff_k_ = getParam(controller_nh, "stretch_coff_k_", 0.);
+  shrink_coff_A_ = getParam(controller_nh, "shrink_coff_A_", 0.);
+  shrink_coff_k_ = getParam(controller_nh, "shrink_coff_k_", 0.);
+  feedforward_offset = getParam(controller_nh, "feed_forward_offset", 0.);
+  feedforward_effect_time_ = getParam(controller_nh, "feedforward_effect_time_", 0.);
+
+  for (const auto& suspension_leg : suspension_legs)
+  {
+    ros::NodeHandle nh_suspension = ros::NodeHandle(controller_nh, "suspension_leg/" + suspension_leg.first);
+    active_suspension_joints_.push_back(std::make_shared<effort_controllers::JointPositionController>());
+    if (!active_suspension_joints_.back()->init(effort_joint_interface_, nh_suspension))
+    {
+      ROS_ERROR("Failed to init active_suspension_joint");
+      return false;
+    }
+  }
+  return true;
+}
+void ActiveSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period)
+{
+  OmniController::moveJoint(time, period);
+  switch (current_state_)
+  {
+    case State::DOWN:
+      target_pos_ = -0.05;
+      static_effort = -6.0;
+      break;
+    case State::MID:
+      target_pos_ = 0.30;
+      static_effort = 9.0;
+      break;
+    case State::UP:
+      target_pos_ = 0.80;
+      static_effort = 8.0;
+      break;
+  }
+  if (current_state_ != last_state_)
+  {
+    last_state_ = current_state_;
+    feedforward_timer = ros::Time::now();
+  }
+  if ((ros::Time::now() - feedforward_timer).toSec() < feedforward_effect_time_)
+  {
+    if (current_state_ == State::MID)
+      feedforward_effort = 1.2 * (stretch_coff_A_ - 45.0 * current_pos_);
+    else if (current_state_ == State::UP)
+      feedforward_effort = 2 * (stretch_coff_A_ + stretch_coff_k_ * current_pos_);
+    else if (current_state_ == State::DOWN)
+      feedforward_effort = shrink_coff_A_ + shrink_coff_k_ * current_pos_;
+  }
+  else
+    feedforward_effort = 0.0;
+
+  for (auto& joint : active_suspension_joints_)
+  {
+    joint->setCommand(target_pos_);
+    joint->update(time, period);
+    current_pos_ = joint->getPosition();
+    auto cmd = joint->joint_.getCommand();
+    cmd += feedforward_effort + feedforward_offset + static_effort;
+    joint->joint_.setCommand(cmd);
+  }
+}
+void ActiveSuspensionController::ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg)
+{
+  current_state_ = static_cast<State>(msg.mode);
+}
+}  // namespace rm_chassis_controllers
+PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::ActiveSuspensionController, controller_interface::ControllerBase)
diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp
index 1e744ebd..f70b2588 100644
--- a/rm_chassis_controllers/src/chassis_base.cpp
+++ b/rm_chassis_controllers/src/chassis_base.cpp
@@ -149,13 +149,13 @@ void ChassisBase<T...>::update(const ros::Time& time, const ros::Duration& perio
     vel_cmd_.y = ramp_y_->output();
     vel_cmd_.z = cmd_vel.angular.z;
   }
-
+  // test
   if (cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_source_frame.empty())
     follow_source_frame_ = "yaw";
   else
     follow_source_frame_ = cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_source_frame;
   if (cmd_rt_buffer_.readFromRT()->cmd_chassis_.command_source_frame.empty())
-    command_source_frame_ = "yaw";
+    command_source_frame_ = "bask_link";
   else
     command_source_frame_ = cmd_rt_buffer_.readFromRT()->cmd_chassis_.command_source_frame;
 
@@ -173,10 +173,10 @@ void ChassisBase<T...>::update(const ros::Time& time, const ros::Duration& perio
       raw();
       break;
     case FOLLOW:
-      follow(time, period);
+      raw();
       break;
     case TWIST:
-      twist(time, period);
+      raw();
       break;
   }
 
diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt
index 995f5af7..fb4183bd 100644
--- a/rm_gimbal_controllers/CMakeLists.txt
+++ b/rm_gimbal_controllers/CMakeLists.txt
@@ -68,7 +68,8 @@ include_directories(
 )
 
 ## Declare a cpp library
-add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp)
+add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp
+                                )
 
 ## Specify libraries to link executable targets against
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg
old mode 100755
new mode 100644
index c8de53c0..cae9b458
--- a/rm_gimbal_controllers/cfg/BulletSolver.cfg
+++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg
@@ -12,20 +12,21 @@ gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of
 gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0)
 gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0)
 gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5)
-gen.add("wait_next_armor_delay", double_t, 0, "Delay of shooting next target when in center mode", 0.105, 0.0, 0.5)
-gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.105, 0.0, 0.5)
-gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1)
-gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0)
+gen.add("center_delay", double_t, 0, "Delay of shooting this target when in center mode", 0.105, -0.5, 0.5)
 gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0)
 gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0)
 gen.add("switch_angle_offset", double_t, 0, "Switch angle offset", 0.0, -20.0, 20)
 gen.add("switch_duration_scale",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0)
 gen.add("switch_duration_rate",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0)
 gen.add("switch_duration_offset",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0)
-gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99)
-gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 0.0, 4.5, 20)
-gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0)
+gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 1, 99)
+gen.add("max_selected_armor_", int_t, 0, "Max selected armor number", 0, -4, 4)
+gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20)
 gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0)
 gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0)
+gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0)
+gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1)
+gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.0)
+gen.add("move_switch_time_coff_",double_t, 0, "The offset of the traject_switch_time", 0.0 , -1.0, 1.0)
 
 exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver"))
diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
index e81ba1dc..6a4ea233 100644
--- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
+++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
@@ -42,6 +42,7 @@
 #include <geometry_msgs/TransformStamped.h>
 #include <visualization_msgs/Marker.h>
 #include <rm_gimbal_controllers/BulletSolverConfig.h>
+#include <rm_msgs/BulletSolverData.h>
 #include <dynamic_reconfigure/server.h>
 #include <rm_common/hardware_interface/robot_state_interface.h>
 #include <rm_common/eigen_types.h>
@@ -50,16 +51,29 @@
 #include <std_msgs/Float64.h>
 #include <rm_msgs/TrackData.h>
 #include <rm_msgs/ShootBeforehandCmd.h>
+#include <rm_msgs/LocalHeatState.h>
 
 namespace rm_gimbal_controllers
 {
 struct Config
 {
   double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18,
-      resistance_coff_qd_30, g, delay, wait_next_armor_delay, wait_diagonal_armor_delay, dt, timeout, max_switch_angle,
-      min_switch_angle, switch_angle_offset, switch_duration_scale, switch_duration_rate, switch_duration_offset,
-      min_shoot_beforehand_vel, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay;
+      resistance_coff_qd_30, g, delay, center_delay, max_switch_angle, switch_angle_offset, switch_duration_scale,
+      switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, track_rotate_target_delay,
+      track_move_target_delay;
   int min_fit_switch_count;
+  int max_selected_armor_;
+  double traject_ahead_;
+  int clean_shoot_num_;
+  double end_pos_offset, move_switch_time_coff_;
+};
+struct TrajectoryFunctionCoefficients
+{
+  double a0, a1, a2, a3;
+};
+struct TrajectoryLimitParams
+{
+  double start_pos, end_pos, start_vel, end_vel;
 };
 
 class BulletSolver
@@ -68,7 +82,7 @@ class BulletSolver
   explicit BulletSolver(ros::NodeHandle& controller_nh);
 
   bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw,
-             double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z);
+             double r1, double r2, double dz, int armors_num, double start_vel);
   double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1,
                         double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed);
   double getResistanceCoefficient(double bullet_speed) const;
@@ -80,6 +94,26 @@ class BulletSolver
   {
     return -output_pitch_;
   }
+  double getTrajectYaw() const
+  {
+    return traject_output_yaw_;
+  }
+  bool getUsingtraject() const
+  {
+    return using_traject_;
+  }
+  int getShootnum() const
+  {
+    return shoot_num_;
+  }
+  double getTrajectEffortff() const
+  {
+    return traject_effort_ff_;
+  }
+  bool getTrackTarget()
+  {
+    return track_target_;
+  }
   double getGimbalSwitchDuration(double v_yaw);
   void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel,
                                  geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw,
@@ -88,6 +122,8 @@ class BulletSolver
   void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time);
   void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg);
   void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t);
+  double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_);
+  void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg);
   ~BulletSolver() = default;
 
 private:
@@ -95,28 +131,56 @@ class BulletSolver
   std::shared_ptr<realtime_tools::RealtimePublisher<visualization_msgs::Marker>> path_real_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::ShootBeforehandCmd>> shoot_beforehand_cmd_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> fly_time_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::BulletSolverData>> bullet_solver_pub;
   ros::Subscriber identified_target_change_sub_;
+  ros::Subscriber shoot_state_sub_;
   ros::Time switch_armor_time_{};
   realtime_tools::RealtimeBuffer<Config> config_rt_buffer_;
   dynamic_reconfigure::Server<rm_gimbal_controllers::BulletSolverConfig>* d_srv_{};
   Config config_{};
   double max_track_target_vel_;
-  double output_yaw_{}, output_pitch_{};
+  double output_yaw_{}, output_pitch_{}, traject_output_yaw_{};
   double bullet_speed_{}, resistance_coff_{};
   double fly_time_;
   double switch_hysteresis_;
   double last_yaw_{}, filtered_yaw_{};
   double gimbal_switch_duration_{};
+  double yaw_subtract_;
+  double switch_armor_angle{};
+  double filtered_v_yaw_{};
+  double switchtime{};
+  double traject_effort_ff_{};
+  double traject_switch_time_{};
+  double switche_time_yaw_{};
+  double traject_max_acc_{};
+  int shoot_num_ = 0;
   int shoot_beforehand_cmd_{};
-  int selected_armor_;
   int count_;
+  int next_count_;
+  int traject_count_;
+  int ban_shoot_count_ = 0;
+  int selected_armor_ = 0;
+  int last_selected_armor_ = 0;
+
   bool track_target_ = true;
   bool identified_target_change_ = true;
   bool is_in_delay_before_switch_{};
   bool dynamic_reconfig_initialized_{};
+  bool change_armor = false;
+  bool using_traject_{};
+  bool last_shoot_state_{};
+  bool is_aheading_two_{};
 
+  geometry_msgs::Point after_traject_output_yaw_{};
   geometry_msgs::Point target_pos_{};
   visualization_msgs::Marker marker_desire_;
   visualization_msgs::Marker marker_real_;
+  ros::Time start_using_traject_time;
+  ros::Time ban_shoot_time_;
+
+  TrajectoryFunctionCoefficients trajectory_function_coefficients;
+  TrajectoryLimitParams stauts_limit_;
+
+  mutable std::mutex heat_mutex_;
 };
 }  // namespace rm_gimbal_controllers
diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
index 2f4ab34a..5f3b884c 100644
--- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
+++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
@@ -139,15 +139,15 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
   void starting(const ros::Time& time) override;
   void update(const ros::Time& time, const ros::Duration& period) override;
-  void setDes(const ros::Time& time, double yaw_des, double pitch_des);
+  void setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw = true,
+              bool update_pitch = true);
 
 private:
   void rate(const ros::Time& time, const ros::Duration& period);
   void track(const ros::Time& time);
   void direct(const ros::Time& time);
   void traj(const ros::Time& time);
-  bool setDesIntoLimit(const tf2::Quaternion& base2gimbal_des, const urdf::JointConstSharedPtr& joint_urdf,
-                       tf2::Quaternion& base2new_des);
+  bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle);
   void moveJoint(const ros::Time& time, const ros::Duration& period);
   void updateChassisVel();
   double feedForward(const ros::Time& time);
@@ -179,13 +179,13 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
 
   rm_msgs::GimbalCmd cmd_gimbal_;
   rm_msgs::TrackData data_track_;
-  std::string gimbal_des_frame_id_{}, imu_name_{};
+  std::string gimbal_des_frame_id_{}, imu_name_{}, gimbal_traject_des_frame_id_;
   double publish_rate_{};
   bool state_changed_{};
   int loop_count_{};
 
   // Transform
-  geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_;
+  geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, odom2gimbal_traject_des_;
 
   // Gravity Compensation
   geometry_msgs::Vector3 mass_origin_;
@@ -210,6 +210,13 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   };
   int state_ = RATE;
   bool start_ = false;
+
+  ros::Duration period_;
+  ros::Time time_;
+  double pos_real[3]{ 0. };
+  double pos_des[3]{ 0. }, vel_des[3]{ 0. }, angle_error[3]{ 0. }, traject_pos_des[3]{ 0. },
+      traject_angle_error[3]{ 0. };
+  double last_acc_yaw = 0;
 };
 
 }  // namespace rm_gimbal_controllers
diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp
index 29acb90f..4c45ef5d 100644
--- a/rm_gimbal_controllers/src/bullet_solver.cpp
+++ b/rm_gimbal_controllers/src/bullet_solver.cpp
@@ -39,6 +39,7 @@
 #include <cmath>
 #include <tf/transform_datatypes.h>
 #include <rm_common/ori_tool.h>
+#include <rm_msgs/PowerHeatData.h>
 
 namespace rm_gimbal_controllers
 {
@@ -52,21 +53,21 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
     .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.),
     .g = getParam(controller_nh, "g", 0.),
     .delay = getParam(controller_nh, "delay", 0.),
-    .wait_next_armor_delay = getParam(controller_nh, "wait_next_armor_delay", 0.105),
-    .wait_diagonal_armor_delay = getParam(controller_nh, "wait_diagonal_armor_delay", 0.105),
-    .dt = getParam(controller_nh, "dt", 0.),
-    .timeout = getParam(controller_nh, "timeout", 0.),
+    .center_delay = getParam(controller_nh, "center_delay", 0.0),
     .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0),
-    .min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.0),
     .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 0.0),
     .switch_duration_scale = getParam(controller_nh, "switch_duration_scale", 0.),
     .switch_duration_rate = getParam(controller_nh, "switch_duration_rate", 0.),
-    .switch_duration_offset = getParam(controller_nh, "switch_duration_offset", 0.08),
-    .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 4.5),
-    .max_chassis_angular_vel = getParam(controller_nh, "max_chassis_angular_vel", 8.5),
+    .switch_duration_offset = getParam(controller_nh, "switch_duration_offset", 0.1),
+    .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 3.0),
     .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.),
     .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.),
     .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3),
+    .max_selected_armor_ = getParam(controller_nh, "max_selecteed_armor", 3),
+    .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.),
+    .clean_shoot_num_ = getParam(controller_nh, "clean_shoot_num_", 1),
+    .end_pos_offset = getParam(controller_nh, "end_pos_offset", 0.),
+    .move_switch_time_coff_ = getParam(controller_nh, "move_switch_time_coff_", 0.001),
   };
   max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0);
   switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0);
@@ -99,8 +100,12 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
   shoot_beforehand_cmd_pub_.reset(
       new realtime_tools::RealtimePublisher<rm_msgs::ShootBeforehandCmd>(controller_nh, "shoot_beforehand_cmd", 10));
   fly_time_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::Float64>(controller_nh, "fly_time", 10));
+  bullet_solver_pub.reset(
+      new realtime_tools::RealtimePublisher<rm_msgs::BulletSolverData>(controller_nh, "bullet_solver_data", 10));
   identified_target_change_sub_ =
       controller_nh.subscribe<std_msgs::Bool>("/change", 10, &BulletSolver::identifiedTargetChangeCB, this);
+  shoot_state_sub_ = controller_nh.subscribe<rm_msgs::LocalHeatState>("/local_heat_state/shooter_state", 50,
+                                                                      &BulletSolver::heatCB, this);
 }
 
 double BulletSolver::getResistanceCoefficient(double bullet_speed) const
@@ -121,12 +126,11 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) const
 }
 
 bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw,
-                         double v_yaw, double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z)
+                         double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel)
 {
   config_ = *config_rt_buffer_.readFromRT();
   bullet_speed_ = bullet_speed;
   resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001;
-
   if (abs(yaw - last_yaw_) > 1.)
     filtered_yaw_ = yaw;
   else if (last_yaw_ != yaw)
@@ -134,82 +138,158 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
     filtered_yaw_ = filtered_yaw_ + (yaw - filtered_yaw_) * (0.001 / (0.01 + 0.001));
   }
   last_yaw_ = yaw;
-
+  filtered_v_yaw_ = filtered_v_yaw_ + (v_yaw - filtered_v_yaw_) * 0.05;
   double temp_z = pos.z;
   double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2));
-  output_yaw_ = std::atan2(pos.y, pos.x);
+  double output_yaw_central = std::atan2(pos.y, pos.x);
   output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)));
   double rough_fly_time =
       (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_;
-  selected_armor_ = 0;
   double r = r1;
   double z = pos.z;
-  double min_switch_angle = config_.min_switch_angle / 180 * M_PI;
+  if (track_target_)
+    yaw += filtered_v_yaw_ * config_.track_rotate_target_delay;
+  pos.x += vel.x * config_.track_move_target_delay;
+  pos.y += vel.y * config_.track_move_target_delay;
   if (track_target_)
   {
-    if (std::abs(v_yaw) >= max_track_target_vel_ + switch_hysteresis_)
+    if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_)
       track_target_ = false;
   }
   else
   {
-    if (std::abs(v_yaw) <= max_track_target_vel_ - switch_hysteresis_)
+    if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_)
       track_target_ = true;
   }
-  double switch_armor_angle =
-      track_target_ ? M_PI / armors_num - (2 * rough_fly_time + getGimbalSwitchDuration(abs(v_yaw))) / 2 * abs(v_yaw) +
-                          config_.switch_angle_offset :
-                      min_switch_angle;
-  double yaw_subtract = filtered_yaw_ - output_yaw_;
-  while (yaw_subtract > M_PI)
-    yaw_subtract -= 2 * M_PI;
-  while (yaw_subtract < -M_PI)
-    yaw_subtract += 2 * M_PI;
-  if (((yaw_subtract > switch_armor_angle) && v_yaw > 1.) || ((yaw_subtract < -switch_armor_angle) && v_yaw < -1.))
+  switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset;
+  double move_switch_time_coff_ =
+      v_yaw * vel.x > 0.0 ? config_.move_switch_time_coff_ : -config_.move_switch_time_coff_;
+  double move_switch_time_offset = move_switch_time_coff_ * vel.x;
+  traject_switch_time_ =
+      1.321 * exp(-0.289 * abs(filtered_v_yaw_ + 1.0)) * config_.traject_ahead_ + move_switch_time_offset;
+  // traject_switch_time_ = (switch_armor_angle - config_.traject_ahead_ - switche_time_yaw_) / filtered_v_yaw_;
+  yaw_subtract_ = filtered_yaw_ - output_yaw_central;
+  while (yaw_subtract_ > M_PI)
+    yaw_subtract_ -= 2 * M_PI;
+  while (yaw_subtract_ < -M_PI)
+    yaw_subtract_ += 2 * M_PI;
+  double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time;
+  if (((after_fly_yaw_subtract_ < switch_armor_angle - 0.1 && filtered_v_yaw_ > 1.) ||
+       (after_fly_yaw_subtract_ > switch_armor_angle + 0.1 && filtered_v_yaw_ < -1.)) &&
+      track_target_)
+  {
+    selected_armor_ = 0;
+  }
+  else if (!track_target_)
+  {
+    selected_armor_ = 0;
+  }
+
+  if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 1.) ||
+      (after_fly_yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -1.))
   {
     count_++;
-    if (identified_target_change_)
+    if (change_armor)
     {
       count_ = 0;
-      identified_target_change_ = false;
+      next_count_ = 0;
+      change_armor = false;
+      switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num;
     }
     if (count_ >= config_.min_fit_switch_count)
     {
-      if (count_ == config_.min_fit_switch_count)
-        switch_armor_time_ = ros::Time::now();
       selected_armor_ = v_yaw > 0. ? -1 : 1;
       r = armors_num == 4 ? r2 : r1;
       z = armors_num == 4 ? pos.z + dz : pos.z;
+      if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.)
+      {
+        selected_armor_ = -2;
+        next_count_++;
+        if (next_count_ == config_.min_fit_switch_count)
+        {
+          switch_armor_time_ = ros::Time::now();
+          is_aheading_two_ = true;
+        }
+      }
+      else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < switch_armor_angle) && v_yaw < 0.)
+      {
+        selected_armor_ = 2;
+        next_count_++;
+        if (next_count_ == config_.min_fit_switch_count)
+        {
+          switch_armor_time_ = ros::Time::now();
+          is_aheading_two_ = true;
+        }
+      }
+      if (count_ == config_.min_fit_switch_count)
+      {
+        if (is_aheading_two_)
+        {
+          is_aheading_two_ = false;
+        }
+        else
+        {
+          switch_armor_time_ = ros::Time::now();
+        }
+        traject_count_ = 0;
+      }
+    }
+  }
+  if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_) && abs(filtered_v_yaw_) > 2. &&
+      count_ > 2)
+  {
+    traject_count_++;
+    if (traject_count_ == 2)
+    {
+      using_traject_ = true;
+      start_using_traject_time = ros::Time::now();
     }
   }
-  double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 2 * M_PI / armors_num;
-  double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_;
+  double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 1.57;
+  double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_central;
   while (aim_yaw_subtract > M_PI)
     aim_yaw_subtract -= 2 * M_PI;
   while (aim_yaw_subtract < -M_PI)
     aim_yaw_subtract += 2 * M_PI;
-  is_in_delay_before_switch_ = ((((aim_yaw_subtract + v_yaw * config_.delay) > switch_armor_angle) && v_yaw > 1.) ||
-                                (((aim_yaw_subtract + v_yaw * config_.delay) < -switch_armor_angle) && v_yaw < -1.)) &&
-                               track_target_;
-  if (track_target_)
-    yaw += v_yaw * config_.track_rotate_target_delay;
-  pos.x += vel.x * config_.track_move_target_delay;
-  pos.y += vel.y * config_.track_move_target_delay;
+  is_in_delay_before_switch_ =
+      ((((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) > switch_armor_angle) && filtered_v_yaw_ > 1.) ||
+       (((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) < -switch_armor_angle) && filtered_v_yaw_ < -1.)) &&
+      track_target_;
   int count{};
   double error = 999;
   if (track_target_)
   {
-    target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num);
-    target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num);
+    target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 1.57);
+    target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 1.57);
   }
   else
   {
     target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x));
     target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x));
-    if ((v_yaw > 1.0 && (yaw_subtract + v_yaw * (fly_time_ + config_.wait_next_armor_delay) +
-                         selected_armor_ * 2 * M_PI / armors_num) > 0.) ||
-        (v_yaw < -1.0 && (yaw_subtract + v_yaw * (fly_time_ + config_.wait_next_armor_delay) +
-                          selected_armor_ * 2 * M_PI / armors_num) < 0.))
-      selected_armor_ = v_yaw > 0. ? -2 : 2;
+    if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.5) ||
+        (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.5))
+    {
+      selected_armor_ = filtered_v_yaw_ > 0. ? -1 : 1;
+    }
+    if (((filtered_v_yaw_ > 1.0 &&
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.5) ||
+         (filtered_v_yaw_ < -1.0 &&
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.5)))
+    {
+      selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2;
+    }
+    if (((filtered_v_yaw_ > 1.0 &&
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.5) ||
+         (filtered_v_yaw_ < -1.0 &&
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.5)))
+    {
+      selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3;
+    }
+    if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) ||
+        (selected_armor_ < 0 && selected_armor_ < -config_.max_selected_armor_))
+    {
+      selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_;
+    }
     if (selected_armor_ % 2 == 0)
     {
       r = r1;
@@ -217,6 +297,65 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
     }
   }
   target_pos_.z = z;
+
+  double r_trajcet_;
+  if (selected_armor_ == 1 || selected_armor_ == -1)
+  {
+    r_trajcet_ = r1;
+  }
+  else
+  {
+    r_trajcet_ = r2;
+  }
+
+  if (!using_traject_)
+  {
+    switchtime = 0.08;
+
+    if (filtered_v_yaw_ > 0)
+    {
+      after_traject_output_yaw_.x =
+          pos.x + vel.x * (fly_time_ + switchtime) -
+          r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num);
+      after_traject_output_yaw_.y =
+          pos.y + vel.y * (fly_time_ + switchtime) -
+          r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num);
+    }
+    else
+    {
+      after_traject_output_yaw_.x =
+          pos.x + vel.x * (fly_time_ + switchtime) -
+          r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num);
+      after_traject_output_yaw_.y =
+          pos.y + vel.y * (fly_time_ + switchtime) -
+          r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num);
+    }
+    stauts_limit_.start_pos = output_yaw_;
+    stauts_limit_.start_vel = start_vel;
+    stauts_limit_.end_pos =
+        std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset;
+    stauts_limit_.end_vel = stauts_limit_.start_vel;
+
+    if (switchtime > 0.2)
+    {
+      using_traject_ = false;
+    }
+    trajectory_function_coefficients.a0 = stauts_limit_.start_pos;
+    trajectory_function_coefficients.a1 = stauts_limit_.start_vel;
+
+    Eigen::Matrix2d A;
+    A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2);
+    Eigen::Vector2d B;
+    B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime),
+        stauts_limit_.end_vel - stauts_limit_.start_vel;
+
+    Eigen::Vector2d X = A.colPivHouseholderQr().solve(B);
+
+    trajectory_function_coefficients.a2 = X(0);
+    trajectory_function_coefficients.a3 = X(1);
+    traject_max_acc_ = 2 * trajectory_function_coefficients.a2;
+  }
+
   while (error >= 0.001)
   {
     output_yaw_ = std::atan2(target_pos_.y, target_pos_.x);
@@ -262,9 +401,56 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
     fly_time_pub_->msg_.data = fly_time_;
     fly_time_pub_->unlockAndPublish();
   }
+  if (using_traject_)
+  {
+    ros::Time temp = ros::Time::now();
+    traject_output_yaw_ = planningPoint(temp, start_using_traject_time);
+    if (ros::Time::now() - start_using_traject_time > ros::Duration(switchtime))
+    {
+      using_traject_ = false;
+    }
+  }
+  else
+  {
+    traject_output_yaw_ = output_yaw_;
+  }
+  if (bullet_solver_pub->trylock())
+  {
+    bullet_solver_pub->msg_.selected_armor_ = selected_armor_;
+    bullet_solver_pub->msg_.yaw_subtract_ = yaw_subtract_;
+    bullet_solver_pub->msg_.output_yaw_central = output_yaw_central;
+    bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_;
+    bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle;
+    bullet_solver_pub->msg_.test_number = switch_armor_time_.toSec();
+    bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_;
+    bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_;
+    if (v_yaw > 1.0)
+    {
+      bullet_solver_pub->msg_.center_this_armor_ =
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 0 * 2 * M_PI / armors_num);
+      bullet_solver_pub->msg_.center_next_armor_ =
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num);
+      bullet_solver_pub->msg_.center_diagonal_armor_ =
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num);
+      bullet_solver_pub->msg_.center_triple_next_armor_ =
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 3 * 2 * M_PI / armors_num);
+    }
+    else if (v_yaw < -1.0)
+    {
+      bullet_solver_pub->msg_.center_this_armor_ =
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 0 * 2 * M_PI / armors_num);
+      bullet_solver_pub->msg_.center_next_armor_ =
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num);
+      bullet_solver_pub->msg_.center_diagonal_armor_ =
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num);
+      bullet_solver_pub->msg_.center_triple_next_armor_ =
+          (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 3 * 2 * M_PI / armors_num);
+    }
+    bullet_solver_pub->unlockAndPublish();
+  }
+  last_selected_armor_ = selected_armor_;
   return true;
 }
-
 void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel,
                                              geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw,
                                              double v_yaw, double r1, double r2, double dz, int armors_num)
@@ -353,11 +539,9 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec
                                     double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real,
                                     double bullet_speed)
 {
+  config_ = *config_rt_buffer_.readFromRT();
   double delay;
-  if (track_target_)
-    delay = 0.;
-  else
-    delay = selected_armor_ % 2 == 0 ? config_.wait_diagonal_armor_delay : config_.wait_next_armor_delay;
+  delay = track_target_ ? 0. : config_.center_delay;
   double r, z;
   if (selected_armor_ % 2 == 0)
   {
@@ -403,6 +587,11 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg)
 {
   if (msg->data)
     identified_target_change_ = true;
+  else if (!msg->data && identified_target_change_)
+  {
+    change_armor = true;
+    identified_target_change_ = false;
+  }
 }
 
 double BulletSolver::getGimbalSwitchDuration(double v_yaw)
@@ -419,12 +608,16 @@ void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw)
     shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR;
   else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel)
   {
-    if ((ros::Time::now() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw)) - config_.delay)
+    if ((ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay)) &&
+        (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime))
       shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT;
-    if (((ros::Time::now() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw))))
+    else if (ros::Time::now().toSec() >
+                 (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime &&
+             ros::Time::now().toSec() <
+                 (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime + config_.delay)
       shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT;
-    if (is_in_delay_before_switch_)
-      shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT;
+    else
+      shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR;
   }
   else
     shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR;
@@ -436,6 +629,32 @@ void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw)
   }
 }
 
+double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_)
+{
+  double a0 = trajectory_function_coefficients.a0;
+  double a1 = trajectory_function_coefficients.a1;
+  double a2 = trajectory_function_coefficients.a2;
+  double a3 = trajectory_function_coefficients.a3;
+  double n_time_ = (time - start_trajectory_time_).toSec();
+  double plansetpoint = a0 + a1 * n_time_ + a2 * pow(n_time_, 2) + a3 * pow(n_time_, 3);
+  traject_effort_ff_ = 1.4 - 1.4 * n_time_ / switchtime;
+  return plansetpoint;
+}
+
+void BulletSolver::heatCB(const rm_msgs::LocalHeatStateConstPtr& msg)
+{
+  std::lock_guard<std::mutex> lock(heat_mutex_);
+  if (msg->has_shoot && last_shoot_state_ != msg->has_shoot)
+  {
+    shoot_num_ += 1;
+  }
+  last_shoot_state_ = msg->has_shoot;
+  config_ = *config_rt_buffer_.readFromRT();
+  if (config_.clean_shoot_num_ == 0)
+  {
+    shoot_num_ = 0;
+  }
+}
 void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t /*unused*/)
 {
   ROS_INFO("[Bullet Solver] Dynamic params change");
@@ -449,21 +668,21 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
     config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30;
     config.g = init_config.g;
     config.delay = init_config.delay;
-    config.wait_next_armor_delay = init_config.wait_next_armor_delay;
-    config.wait_diagonal_armor_delay = init_config.wait_diagonal_armor_delay;
-    config.dt = init_config.dt;
-    config.timeout = init_config.timeout;
+    config.center_delay = init_config.center_delay;
     config.max_switch_angle = init_config.max_switch_angle;
-    config.min_switch_angle = init_config.min_switch_angle;
     config.switch_angle_offset = init_config.switch_angle_offset;
     config.switch_duration_scale = init_config.switch_duration_scale;
     config.switch_duration_rate = init_config.switch_duration_rate;
     config.switch_duration_offset = init_config.switch_duration_offset;
     config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel;
-    config.max_chassis_angular_vel = init_config.max_chassis_angular_vel;
     config.track_rotate_target_delay = init_config.track_rotate_target_delay;
     config.track_move_target_delay = init_config.track_move_target_delay;
     config.min_fit_switch_count = init_config.min_fit_switch_count;
+    config.max_selected_armor_ = init_config.max_selected_armor_;
+    config.traject_ahead_ = init_config.traject_ahead_;
+    config.clean_shoot_num_ = init_config.clean_shoot_num_;
+    config.end_pos_offset = init_config.end_pos_offset;
+    config.move_switch_time_coff_ = init_config.move_switch_time_coff_;
     dynamic_reconfig_initialized_ = true;
   }
   Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10,
@@ -473,21 +692,21 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config,
                         .resistance_coff_qd_30 = config.resistance_coff_qd_30,
                         .g = config.g,
                         .delay = config.delay,
-                        .wait_next_armor_delay = config.wait_next_armor_delay,
-                        .wait_diagonal_armor_delay = config.wait_diagonal_armor_delay,
-                        .dt = config.dt,
-                        .timeout = config.timeout,
+                        .center_delay = config.center_delay,
                         .max_switch_angle = config.max_switch_angle,
-                        .min_switch_angle = config.min_switch_angle,
                         .switch_angle_offset = config.switch_angle_offset,
                         .switch_duration_scale = config.switch_duration_scale,
                         .switch_duration_rate = config.switch_duration_rate,
                         .switch_duration_offset = config.switch_duration_offset,
                         .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel,
-                        .max_chassis_angular_vel = config.max_chassis_angular_vel,
                         .track_rotate_target_delay = config.track_rotate_target_delay,
                         .track_move_target_delay = config.track_move_target_delay,
-                        .min_fit_switch_count = config.min_fit_switch_count };
+                        .min_fit_switch_count = config.min_fit_switch_count,
+                        .max_selected_armor_ = config.max_selected_armor_,
+                        .traject_ahead_ = config.traject_ahead_,
+                        .clean_shoot_num_ = config.clean_shoot_num_,
+                        .end_pos_offset = config.end_pos_offset,
+                        .move_switch_time_coff_ = config.move_switch_time_coff_ };
   config_rt_buffer_.writeFromNonRT(config_non_rt);
 }
 }  // namespace rm_gimbal_controllers
diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
index 50ff79c4..68eecddb 100644
--- a/rm_gimbal_controllers/src/gimbal_base.cpp
+++ b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -147,6 +147,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
   odom2base_.child_frame_id = getBaseFrameID(joint_urdfs_);
   odom2base_.transform.rotation.w = 1.;
 
+  odom2gimbal_traject_des_.header.frame_id = "odom";
+  odom2gimbal_traject_des_.child_frame_id = gimbal_traject_des_frame_id_;
+  odom2gimbal_traject_des_.transform.rotation.w = 1.;
+
   cmd_gimbal_sub_ = controller_nh.subscribe<rm_msgs::GimbalCmd>("command", 1, &Controller::commandCB, this);
   data_track_sub_ = controller_nh.subscribe<rm_msgs::TrackData>("/track", 1, &Controller::trackCB, this);
   publish_rate_ = getParam(controller_nh, "publish_rate", 100.);
@@ -167,6 +171,8 @@ void Controller::update(const ros::Time& time, const ros::Duration& period)
   cmd_gimbal_ = *cmd_rt_buffer_.readFromRT();
   data_track_ = *track_rt_buffer_.readFromNonRT();
   config_ = *config_rt_buffer_.readFromRT();
+  period_ = period;
+  time_ = time;
   try
   {
     odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time);
@@ -201,15 +207,30 @@ void Controller::update(const ros::Time& time, const ros::Duration& period)
   moveJoint(time, period);
 }
 
-void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des)
+void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des,
+                        bool update_yaw, bool update_pitch)
 {
-  tf2::Quaternion odom2base, odom2gimbal_des;
+  tf2::Quaternion odom2base, odom2gimbal_des_q, base2gimbal_des, base2gimbal_traject_des;
   tf2::fromMsg(odom2base_.transform.rotation, odom2base);
-  odom2gimbal_des.setRPY(0, pitch_des, yaw_des);
-  tf2::Quaternion base2gimbal_des = odom2base.inverse() * odom2gimbal_des;
+  tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des_q);
+
+  double current_rpy[3], des_rpy[3], traject_rpy[3];
+  quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), current_rpy[0], current_rpy[1], current_rpy[2]);
+  odom2gimbal_des_q.setRPY(0, pitch_des, yaw_des);
+  quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), des_rpy[0], des_rpy[1], des_rpy[2]);
+  odom2gimbal_des_q.setRPY(0, pitch_des, traject_yaw_des);
+  quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), traject_rpy[0], traject_rpy[1], traject_rpy[2]);
+
   for (const auto& it : joint_urdfs_)
-    pos_des_in_limit_[it.first] = setDesIntoLimit(base2gimbal_des, it.second, base2gimbal_des);
+  {
+    bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true);
+    pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]);
+  }
+
+  base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]);
+  base2gimbal_traject_des.setRPY(traject_rpy[0], traject_rpy[1], traject_rpy[2]);
   odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des);
+  odom2gimbal_traject_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_traject_des);
   odom2gimbal_des_.header.stamp = time;
   robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers");
 }
@@ -232,7 +253,14 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period)
   {
     double roll{}, pitch{}, yaw{};
     quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw);
-    setDes(time, yaw + period.toSec() * cmd_gimbal_.rate_yaw, pitch + period.toSec() * cmd_gimbal_.rate_pitch);
+
+    bool pitch_needs_update = std::abs(cmd_gimbal_.rate_pitch) > 1e-6;
+    bool yaw_needs_update = std::abs(cmd_gimbal_.rate_yaw) > 1e-6;
+
+    double new_yaw = yaw_needs_update ? (yaw + period.toSec() * cmd_gimbal_.rate_yaw) : yaw;
+    double new_pitch = pitch_needs_update ? (pitch + period.toSec() * cmd_gimbal_.rate_pitch) : pitch;
+
+    setDes(time, new_yaw, new_pitch, new_yaw, yaw_needs_update, pitch_needs_update);
   }
 }
 
@@ -278,7 +306,7 @@ void Controller::track(const ros::Time& time)
   target_vel.z -= chassis_vel_->linear_->z();
   bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw,
                                              data_track_.radius_1, data_track_.radius_2, data_track_.dz,
-                                             data_track_.armors_num, chassis_vel_->angular_->z());
+                                             data_track_.armors_num, ctrls_.at(2)->joint_.getVelocity());
   bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw);
 
   if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time)
@@ -298,7 +326,7 @@ void Controller::track(const ros::Time& time)
   }
 
   if (solve_success)
-    setDes(time, bullet_solver_->getYaw(), bullet_solver_->getPitch());
+    setDes(time, bullet_solver_->getYaw(), bullet_solver_->getPitch(), bullet_solver_->getTrajectYaw());
   else
   {
     odom2gimbal_des_.header.stamp = time;
@@ -330,7 +358,7 @@ void Controller::direct(const ros::Time& time)
   double pitch = -std::atan2(aim_point_odom.z - odom2gimbal_.transform.translation.z,
                              std::sqrt(std::pow(aim_point_odom.x - odom2gimbal_.transform.translation.x, 2) +
                                        std::pow(aim_point_odom.y - odom2gimbal_.transform.translation.y, 2)));
-  setDes(time, yaw, pitch);
+  setDes(time, yaw, pitch, yaw);
 }
 
 void Controller::traj(const ros::Time& time)
@@ -361,32 +389,34 @@ void Controller::traj(const ros::Time& time)
   {
     ROS_WARN("%s", ex.what());
   }
-  setDes(time, traj[2], traj[1]);
+  setDes(time, traj[2], traj[1], traj[2]);
 }
 
-bool Controller::setDesIntoLimit(const tf2::Quaternion& base2gimbal_des, const urdf::JointConstSharedPtr& joint_urdf,
-                                 tf2::Quaternion& base2new_des)
+bool Controller::setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update,
+                                 double current_angle)
 {
-  double base2gimbal_current_des[3];
-  quatToRPY(toMsg(base2gimbal_des), base2gimbal_current_des[0], base2gimbal_current_des[1], base2gimbal_current_des[2]);
+  // 如果不需要更新该轴，保持当前值不变
+  if (!update)
+  {
+    angle = current_angle;
+    return true;
+  }
+
   double upper_limit = joint_urdf->limits ? joint_urdf->limits->upper : 1e16;
   double lower_limit = joint_urdf->limits ? joint_urdf->limits->lower : -1e16;
-  int index = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2;
-  if ((base2gimbal_current_des[index] <= upper_limit && base2gimbal_current_des[index] >= lower_limit) ||
-      (angles::two_pi_complement(base2gimbal_current_des[index]) <= upper_limit &&
-       angles::two_pi_complement(base2gimbal_current_des[index]) >= lower_limit))
+
+  if ((angle <= upper_limit && angle >= lower_limit) ||
+      (angles::two_pi_complement(angle) <= upper_limit && angles::two_pi_complement(angle) >= lower_limit))
   {
-    base2new_des = base2gimbal_des;
     return true;
   }
   else
   {
-    base2gimbal_current_des[index] =
-        std::abs(angles::shortest_angular_distance(base2gimbal_current_des[index], upper_limit)) <
-                std::abs(angles::shortest_angular_distance(base2gimbal_current_des[index], lower_limit)) ?
-            upper_limit :
-            lower_limit;
-    base2new_des.setRPY(base2gimbal_current_des[0], base2gimbal_current_des[1], base2gimbal_current_des[2]);
+    // 超限时，钳位到最近的限位值
+    angle = std::abs(angles::shortest_angular_distance(angle, upper_limit)) <
+                    std::abs(angles::shortest_angular_distance(angle, lower_limit)) ?
+                upper_limit :
+                lower_limit;
     return false;
   }
 }
@@ -420,11 +450,13 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
     if (ctrls_.find(2) != ctrls_.end())
       angular_vel.z = ctrls_.at(2)->joint_.getVelocity();
   }
-  double pos_real[3]{ 0. }, pos_des[3]{ 0. }, vel_des[3]{ 0. }, angle_error[3]{ 0. };
   quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]);
   quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]);
+  quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]);
   for (int i = 0; i < 3; i++)
     angle_error[i] = angles::shortest_angular_distance(pos_real[i], pos_des[i]);
+  for (int i = 0; i < 3; i++)
+    traject_angle_error[i] = angles::shortest_angular_distance(pos_real[i], traject_pos_des[i]);
   if (state_ == RATE)
   {
     vel_des[2] = cmd_gimbal_.rate_yaw;
@@ -483,6 +515,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
       ROS_WARN("%s", ex.what());
     }
   }
+
   for (const auto& in_limit : pos_des_in_limit_)
     if (!in_limit.second)
       vel_des[in_limit.first] = 0.;
@@ -497,10 +530,37 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
   }
   if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end())
   {
-    pid_pos_.at(2)->computeCommand(angle_error[2], period);
-    ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() -
-                             updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() +
-                             config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z);
+    if (bullet_solver_->getUsingtraject())
+    {
+      pid_pos_.at(2)->computeCommand(traject_angle_error[2], period);
+    }
+    else
+    {
+      pid_pos_.at(2)->computeCommand(angle_error[2], period);
+    }
+    if (state_ == TRACK)
+    {
+      if (bullet_solver_->getUsingtraject())
+      {
+        ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() -
+                                 updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() +
+                                 config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z +
+                                 bullet_solver_->getTrajectEffortff());
+      }
+      else
+      {
+        ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() -
+                                 updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() +
+                                 config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z);
+      }
+    }
+    else
+    {
+      ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() -
+                               updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() +
+                               config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z);
+    }
+
     ctrls_.at(2)->update(time, period);
   }
 
@@ -513,10 +573,12 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
       {
         pub.second->msg_.header.stamp = time;
         pub.second->msg_.set_point = pos_des[pub.first];
+        pub.second->msg_.traject_set_point = traject_pos_des[pub.first];
         pub.second->msg_.set_point_dot = vel_des[pub.first];
         pub.second->msg_.process_value = pos_real[pub.first];
         pub.second->msg_.error = angle_error[pub.first];
         pub.second->msg_.command = pid_pos_[pub.first]->getCurrentCmd();
+        pub.second->msg_.shoot_number = bullet_solver_->getShootnum();
         pub.second->unlockAndPublish();
       }
     }
diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index 07089712..a68f830c 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -297,15 +297,8 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd)
     {
       for (size_t j = 0; j < ctrls_friction_[i].size(); j++)
       {
-        // Used to distinguish the front and rear friction wheels.
-        if (j == 0)
-          ctrls_friction_[i][j]->setCommand(
-              wheel_speed_directions_[i][j] *
-              (cmd_.wheel_speed + config_.extra_wheel_speed + cmd_.wheels_speed_offset_back));
-        if (j == 1)
-          ctrls_friction_[i][j]->setCommand(
-              wheel_speed_directions_[i][j] *
-              (cmd_.wheel_speed + config_.extra_wheel_speed + cmd_.wheels_speed_offset_front));
+        ctrls_friction_[i][j]->setCommand(wheel_speed_directions_[i][j] *
+                                          (cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offsets_[i][j]));
       }
     }
   }
@@ -341,7 +334,7 @@ void Controller::normalize()
     enter_ready_ = false;
   }
   else
-    ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() - 0.01) / push_angle));
+    ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() - 0.08) / push_angle));
 }
 
 void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period)