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(