commit 9cbdb023faeca23cfb6ec58a7114156eea77ee18
author James Kuszmaul <jabukuszmaul@gmail.com> Sun Sep 19 17:42:29 2021 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Sep 26 07:15:17 2021 -0700
tree 111b91ea66cd84a3b511c4a4d95600623a7783a7
parent 71f91216f6d39a13afb2b6b3c58d1ba9b35cb14f

Add dither to y2020 turret

Basically just duplicate what Austin did with the hood a while back.

Change-Id: I7df9a45056de1dfca878402013facd3349453825
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

y2020/control_loops/superstructure/superstructure.cc

diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index ce816a0..98de31d 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -164,11 +164,16 @@
 
   if (output != nullptr) {
     // Friction is a pain and putting a really high burden on the integrator.
+    // TODO(james): I'm not sure how helpful this gain is.
     const double turret_velocity_sign =
         turret_status->velocity() * kTurretFrictionGain;
     output_struct.turret_voltage +=
         std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
                    kTurretFrictionVoltageLimit);
+    const double time_sec =
+        aos::time::DurationInSeconds(position_timestamp.time_since_epoch());
+    output_struct.turret_voltage +=
+        kTurretDitherGain * std::sin(2.0 * M_PI * time_sec * 30.0);
     output_struct.turret_voltage =
         std::clamp(output_struct.turret_voltage, -turret_.operating_voltage(),
                    turret_.operating_voltage());