Revert "Make loading positions unwrap the turret"
This reverts commit 9ad1efeb89c8fc7b3983d543f8612fa2c24378b6.
Something doesn't interact with aiming well and it does a bunch of 360's...
Change-Id: I345b5b3b296cf8aa9662fe99de32a2f19565d8ff
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 42453ff..1f0d1fc 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -281,11 +281,19 @@
(turret_intake_state_ == RequestedIntake::kFront
? constants::Values::kTurretFrontIntakePos()
: constants::Values::kTurretBackIntakePos());
- // 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);
+ // 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);
+ }
turret_loading_goal_buffer.Finish(
frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index cbc71d4..ff243e2 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -998,7 +998,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.
@@ -1010,7 +1010,7 @@
IntakeState::NO_BALL);
}
-TEST_F(SuperstructureTest, TestTurretUnWrapsWhenLoading) {
+TEST_F(SuperstructureTest, TestTurretWrapsWhenLoading) {
SetEnabled(true);
WaitUntilZeroed();
@@ -1032,22 +1032,21 @@
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
0.001);
- superstructure_plant_.set_intake_beambreak_back(true);
+ superstructure_plant_.set_intake_beambreak_front(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);
+ IntakeState::INTAKE_FRONT_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 goes to -pi instead of +pi because -pi is closest to the center of the
- // range at -1.675.
+ -constants::Values::kTurretFrontIntakePos() - 2.0 * M_PI, 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