commit 2c39086edd763e4c605ebbc0cafda188dd032764
author Ravago Jones <ravagojones@gmail.com> Fri Mar 11 17:23:21 2022 -0800
committer Ravago Jones <ravagojones@gmail.com> Fri Mar 11 17:26:42 2022 -0800
tree 9afab472967436b7f4207bf922401ea5435756b4
parent 84b957ddb61830fdaeba0928adff5e5762bbdb2e

Wrap turret intake loctions

Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I0e5bd91808613264b7469490cfb2f4ecac4339ae

y2022/control_loops/superstructure/superstructure.cc

diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index b25d074..c2decea 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -184,6 +184,20 @@
                ? constants::Values::kTurretFrontIntakePos()
                : constants::Values::kTurretBackIntakePos());
 
+      // Turn to the loading position as close to the current position as
+      // possible
+      // Strategy is copied from frc971/control_loops/aiming/aiming.cc
+      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 =
+            aos::math::NormalizeAngle(turret_loading_position);
+      }
+
       turret_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 2f73370..1ccdd51 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -1003,6 +1003,45 @@
             IntakeState::NO_BALL);
 }
 
+TEST_F(SuperstructureTest, TestTurretWrapsWhenLoading) {
+  SetEnabled(true);
+  WaitUntilZeroed();
+
+  constexpr double kTurretGoal = -4.0;
+  {
+    auto builder = superstructure_goal_sender_.MakeBuilder();
+    flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+        turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+            *builder.fbb(), kTurretGoal);
+    Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+    goal_builder.add_turret(turret_offset);
+    builder.CheckOk(builder.Send(goal_builder.Finish()));
+  }
+  RunFor(std::chrono::seconds(5));
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+  EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+            IntakeState::NO_BALL);
+  EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
+              0.001);
+
+  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_BACK_BALL);
+
+  RunFor(std::chrono::seconds(3));
+
+  ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+  EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
+              -constants::Values::kTurretBackIntakePos(), 0.001);
+  // it chooses -pi because -pi is closer to -4 than positive pi
+}
+
 // Make sure that the front and back intakes are never switched
 TEST_F(SuperstructureTest, RunIntakes) {
   SetEnabled(true);