Merge pull request #227 from liyixin135/standard

Slow down the gyro's speed when shooting buff
rm-controls · May 30, 2024 · 8a8cb7e · 8a8cb7e
2 parents abdcb34 + 41a8f12
commit 8a8cb7e
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 2 deletions.
diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h
@@ -161,7 +161,7 @@ class Vel2DCommandSender : public CommandSenderBase<geometry_msgs::Twist>
       max_angular_z_.init(xml_rpc_value);
     std::string topic;
     nh.getParam("power_limit_topic", topic);
-    target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 4.);
+    target_vel_yaw_threshold_ = getParam(nh, "target_vel_yaw_threshold", 3.);
     chassis_power_limit_subscriber_ =
         nh.subscribe<rm_msgs::ChassisCmd>(topic, 1, &Vel2DCommandSender::chassisCmdCallback, this);
   }
@@ -186,6 +186,15 @@ class Vel2DCommandSender : public CommandSenderBase<geometry_msgs::Twist>
       vel_direction_ = 1.;
     msg_.angular.z = scale * max_angular_z_.output(power_limit_) * vel_direction_;
   };
+  void setAngularZVel(double scale, double limit)
+  {
+    if (track_data_.v_yaw > target_vel_yaw_threshold_)
+      vel_direction_ = -1.;
+    if (track_data_.v_yaw < -target_vel_yaw_threshold_)
+      vel_direction_ = 1.;
+    double angular_z = max_angular_z_.output(power_limit_) > limit ? limit : max_angular_z_.output(power_limit_);
+    msg_.angular.z = scale * angular_z * vel_direction_;
+  };
   void set2DVel(double scale_x, double scale_y, double scale_z)
   {
     setLinearXVel(scale_x);

diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h
@@ -115,7 +115,6 @@ class RefereeBase
   StringTriggerChangeUi *servo_mode_trigger_change_ui_{}, *stone_num_trigger_change_ui_{},
       *joint_temperature_trigger_change_ui_{}, *gripper_state_trigger_change_ui_{};
 
-
   FixedUi* fixed_ui_{};
 
   CoverFlashUi* cover_flash_ui_{};