commit 93ddcb4b13f2478b809756adcb3f4a3ab779b749
author Austin Schuh <austin.linux@gmail.com> Mon Oct 25 21:54:11 2021 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Oct 27 20:16:20 2021 -0700
tree 82d8a853f872f0088d87c5e712adf0033a8fd397
parent 6f94616dbfaa7804cab857442ee92fa8e2be6c6b

Lower the acceleration on the intake when jiggling the balls

This should reduce wear on the pin in the intake by being less
aggressive.  Removing the jiggling causes balls to get stuck.  It
doesn't need to be aggressive to be effective.

Change-Id: If03b971bb49e64d8e81465cb5be9f4c5c583b99a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

frc971/control_loops/profiled_subsystem.h

diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 6780e3b..86e608c 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -192,6 +192,9 @@
 
   const ::frc971::constants::Range &range() const { return range_; }
 
+  double default_velocity() const { return default_velocity_; }
+  double default_acceleration() const { return default_acceleration_; }
+
  protected:
   // Limits the provided goal to the soft limits.  Prints "name" when it fails
   // to aid debugging.

frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h

diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 126502b..dd64111 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -64,6 +64,16 @@
 
   void set_max_position(double max_position) { max_position_ = max_position; }
 
+  // Sets a temporary acceleration limit.  No accelerations faster than this may
+  // be commanded.
+  void set_max_acceleration(double max_acceleration) {
+    max_acceleration_ = max_acceleration;
+  }
+  // Clears the acceleration limit.
+  void clear_max_acceleration() {
+    max_acceleration_ = std::numeric_limits<float>::infinity();
+  }
+
   // Resets constrained min/max position
   void clear_min_position() { min_position_ = params_.range.lower_hard; }
 
@@ -99,6 +109,7 @@
  private:
   State state_ = State::UNINITIALIZED;
   double min_position_, max_position_;
+  float max_acceleration_ = std::numeric_limits<float>::infinity();
 
   const StaticZeroingSingleDOFProfiledSubsystemParams<TSubsystemParams> params_;
 
@@ -198,7 +209,17 @@
       }
 
       if (goal) {
-        profiled_subsystem_.AdjustProfile(goal->profile_params());
+        if (goal->profile_params()) {
+          profiled_subsystem_.AdjustProfile(
+              goal->profile_params()->max_velocity(),
+              std::min(goal->profile_params()->max_acceleration(),
+                       max_acceleration_));
+        } else {
+          profiled_subsystem_.AdjustProfile(
+              profiled_subsystem_.default_velocity(),
+              std::min(profiled_subsystem_.default_acceleration(),
+                       static_cast<double>(max_acceleration_)));
+        }
 
         double safe_goal = goal->unsafe_goal();
         if (safe_goal < min_position_) {

y2020/control_loops/superstructure/superstructure.cc

diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index f62109c..d3c2464 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -113,6 +113,7 @@
     }
 
     if (unsafe_goal->shooting()) {
+      intake_joint_.set_max_acceleration(30.0);
       constexpr std::chrono::milliseconds kPeriod =
           std::chrono::milliseconds(250);
       if ((position_timestamp - shooting_start_time_) % (kPeriod * 2) <
@@ -122,6 +123,7 @@
         intake_joint_.set_min_position(-0.75);
       }
     } else {
+      intake_joint_.clear_max_acceleration();
       intake_joint_.clear_min_position();
     }