commit 27d16d365b9b1d441292170dd8302855d646152d
author Milind Upadhyay <milind.upadhyay@gmail.com> Sat Apr 16 13:41:45 2022 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sat Apr 16 13:23:23 2022 -0800
tree c913c6db414bc97c1545c91e9e2ed9833364043d
parent c4df1b52b3104740e3cf095db638fe469f200cbf

Make loading positions unwrap the turret

This reverts commit c4df1b52b3104740e3cf095db638fe469f200cbf.
Update the aimer with the turret loading goal when transferring
so it doesn't have to wrap

Change-Id: I987f91a53d3bdd07a6a8dbc63b1e5f88216775a3
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>

y2022/control_loops/superstructure/superstructure.cc

diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 1f0d1fc..3961990 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -281,19 +281,11 @@
       (turret_intake_state_ == RequestedIntake::kFront
            ? constants::Values::kTurretFrontIntakePos()
            : constants::Values::kTurretBackIntakePos());
-  // Turn to the loading position as close to the current position as
-  // possible.
-  turret_loading_position =
-      turret_.estimated_position() +
-      aos::math::NormalizeAngle(turret_loading_position -
-                                turret_.estimated_position());
-  // if out of range, reset back to within +/- pi of zero.
-  if (turret_loading_position > values_->turret_range.upper ||
-      turret_loading_position < values_->turret_range.lower) {
-    turret_loading_position =
-        frc971::zeroing::Wrap(values_->turret_range.middle_soft(),
-                              turret_loading_position, 2.0 * M_PI);
-  }
+  // Turn to the loading position as close to the middle of the range as
+  // possible. Do the unwraping before we have a ball so we don't have to unwrap
+  // to shoot.
+  turret_loading_position = frc971::zeroing::Wrap(
+      values_->turret_range.middle_soft(), turret_loading_position, 2.0 * M_PI);
 
   turret_loading_goal_buffer.Finish(
       frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
@@ -357,6 +349,7 @@
       }
 
       turret_goal = &turret_loading_goal_buffer.message();
+      aimer_.UpdateTurretGoal(turret_loading_position);
 
       const bool turret_near_goal =
           std::abs(turret_.estimated_position() - turret_loading_position) <
@@ -495,7 +488,7 @@
 
       // Once the shot is complete and the catapult is back to its return
       // position, go back to IDLE
-      if (catapult_.shot_count() > prev_shot_count_ ) {
+      if (catapult_.shot_count() > prev_shot_count_) {
         prev_shot_count_ = catapult_.shot_count();
         fire_ = false;
         discarding_ball_ = false;