Merge branch 'rm-controls:master' into master

rm-controls · Oct 21, 2024 · 0bc634b · 0bc634b
2 parents aa4dbc4 + d24cdff
commit 0bc634b
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 1 deletion.
diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h
@@ -51,6 +51,7 @@
 #include <rm_msgs/GameRobotHp.h>
 #include <rm_msgs/StatusChangeRequest.h>
 #include <rm_msgs/ShootData.h>
+#include <rm_msgs/LegCmd.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <sensor_msgs/JointState.h>
 #include <nav_msgs/Odometry.h>
@@ -564,6 +565,32 @@ class BalanceCommandSender : public CommandSenderBase<std_msgs::UInt8>
   void setZero() override{};
 };
 
+class LegCommandSender : public CommandSenderBase<rm_msgs::LegCmd>
+{
+public:
+  explicit LegCommandSender(ros::NodeHandle& nh) : CommandSenderBase<rm_msgs::LegCmd>(nh)
+  {
+  }
+
+  void setJump(bool jump)
+  {
+    msg_.jump = jump;
+  }
+  void setLgeLength(double length)
+  {
+    msg_.leg_length = length;
+  }
+  bool getJump()
+  {
+    return msg_.jump;
+  }
+  double getLgeLength()
+  {
+    return msg_.leg_length;
+  }
+  void setZero() override{};
+};
+
 class Vel3DCommandSender : public HeaderStampCommandSenderBase<geometry_msgs::TwistStamped>
 {
 public:

diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h
@@ -66,6 +66,8 @@ class PowerLimit
       ROS_ERROR("power gain no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("buffer_threshold", buffer_threshold_))
       ROS_ERROR("buffer threshold no defined (namespace: %s)", nh.getNamespace().c_str());
+    if (!nh.getParam("is_new_capacitor", is_new_capacitor_))
+      ROS_ERROR("is_new_capacitor no defined (namespace: %s)", nh.getNamespace().c_str());
   }
   typedef enum
   {
@@ -124,7 +126,7 @@ class PowerLimit
             chassis_cmd.power_limit = burst_power_;
           else
           {
-            switch (cap_state_)
+            switch (is_new_capacitor_ ? expect_state_ : cap_state_)
             {
               case NORMAL:
                 normal(chassis_cmd);
@@ -186,6 +188,7 @@ class PowerLimit
   double charge_power_{}, extra_power_{}, burst_power_{};
   double buffer_threshold_{};
   double power_gain_{};
+  bool is_new_capacitor_{};
   uint8_t expect_state_{}, cap_state_{};
 
   bool referee_is_online_{ false };

diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt
@@ -22,6 +22,7 @@ add_message_files(
         GimbalCmd.msg
         GimbalDesError.msg
         GimbalPosState.msg
+        LegCmd.msg
         LpData.msg
         LocalHeatState.msg
         KalmanData.msg

diff --git a/rm_msgs/msg/LegCmd.msg b/rm_msgs/msg/LegCmd.msg
@@ -0,0 +1,2 @@
+bool jump
+float64 leg_length