commit 9ad1efeb89c8fc7b3983d543f8612fa2c24378b6
author Ravago Jones <ravagojones@gmail.com> Wed Apr 13 16:27:24 2022 -0700
committer Ravago Jones <ravagojones@gmail.com> Wed Apr 13 16:56:51 2022 -0700
tree 88767e215b86daa47f3cfccab0b5d5c177f6ece9
parent b2a11d83ab839bb92be6317a8eaa2477d928e12c

Make loading positions unwrap the turret

This makes us unwrap before we have a ball so we don't have to
unwrap as we're aiming.

Change-Id: If3b483c5e36ea69c9ecb8754e3d0e855316f54b9
Signed-off-by: Ravago Jones <ravagojones@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 15f9c2a..c3d8fa5 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -222,19 +222,12 @@
       (turret_intake_state_ == RequestedIntake::kFront
            ? constants::Values::kTurretFrontIntakePos()
            : constants::Values::kTurretBackIntakePos());
-  // Turn to the loading position as close to the current position as
-  // possible.
+  // 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 =
-      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 > constants::Values::kTurretRange().upper ||
-      turret_loading_position < constants::Values::kTurretRange().lower) {
-    turret_loading_position =
-        frc971::zeroing::Wrap(constants::Values::kTurretRange().middle_soft(),
-                              turret_loading_position, 2.0 * M_PI);
-  }
+      frc971::zeroing::Wrap(constants::Values::kTurretRange().middle_soft(),
+                            turret_loading_position, 2.0 * M_PI);
 
   turret_loading_goal_buffer.Finish(
       frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(

y2022/control_loops/superstructure/superstructure_lib_test.cc

diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 7123a47..42fd861 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -981,7 +981,7 @@
   EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
             IntakeState::INTAKE_BACK_BALL);
   EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
-              -constants::Values::kTurretBackIntakePos(), 0.001);
+              constants::Values::kTurretBackIntakePos(), 0.001);
 
   // Since the intake beambreak hasn't triggered in a while, it should realize
   // the ball was lost.
@@ -993,7 +993,7 @@
             IntakeState::NO_BALL);
 }
 
-TEST_F(SuperstructureTest, TestTurretWrapsWhenLoading) {
+TEST_F(SuperstructureTest, TestTurretUnWrapsWhenLoading) {
   SetEnabled(true);
   WaitUntilZeroed();
 
@@ -1015,21 +1015,22 @@
   EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
               0.001);
 
-  superstructure_plant_.set_intake_beambreak_front(true);
+  superstructure_plant_.set_intake_beambreak_back(true);
   RunFor(dt() * 2);
 
   ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
   EXPECT_EQ(superstructure_status_fetcher_->state(),
             SuperstructureState::TRANSFERRING);
   EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
-            IntakeState::INTAKE_FRONT_BALL);
+            IntakeState::INTAKE_BACK_BALL);
 
   RunFor(std::chrono::seconds(3));
 
   ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
   EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
-              -constants::Values::kTurretFrontIntakePos() - 2.0 * M_PI, 0.001);
-  // it chooses -pi because -pi is closer to -4 than positive pi
+              constants::Values::kTurretBackIntakePos(), 0.001);
+  // It goes to -pi instead of +pi because -pi is closest to the center of the
+  // range at -1.675.
 }
 
 // Make sure that the front and back intakes are never switched