Merge pull request #149 from Fawo011/new

Add new branch and add the new velocity planning for high radio frequency.
rm-controls · Mar 4, 2024 · 6b89a28 · 6b89a28
2 parents 18f9ed1 + 6f1a152
commit 6b89a28
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 6 deletions.
diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
@@ -56,7 +56,7 @@ namespace rm_shooter_controllers
 struct Config
 {
   double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold,
-      forward_push_threshold, exit_push_threshold;
+      forward_push_threshold,exit_push_threshold;
   double extra_wheel_speed;
 };
 

diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
@@ -197,12 +197,28 @@ void Controller::push(const ros::Time& time, const ros::Duration& period)
   }
   if ((cmd_.wheel_speed == 0. || wheel_speed_ready) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz)
   {  // Time to shoot!!!
-    if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) <
-        config_.forward_push_threshold)
+    if (cmd_.hz >= 20)
     {
-      ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ -
-                               2. * M_PI / static_cast<double>(push_per_rotation_));
-      last_shoot_time_ = time;
+      config_.forward_push_threshold += 0.5;
+      if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) <
+          config_.forward_push_threshold)
+      {
+        ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ -
+                                     2. * M_PI / static_cast<double>(push_per_rotation_),
+                                 -1 * cmd_.hz * 2. * M_PI / static_cast<double>(push_per_rotation_));
+        last_shoot_time_ = time;
+      }
+      config_.forward_push_threshold -= 0.5;
+    }
+    else
+    {
+      if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) <
+          config_.forward_push_threshold)
+      {
+        ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ -
+                                 2. * M_PI / static_cast<double>(push_per_rotation_));
+        last_shoot_time_ = time;
+      }
     }
     // Check block
     if ((ctrl_trigger_.joint_.getEffort() < -config_.block_effort &&