Properly set turret intake side
Before it was set only when one intake had a ball.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I199057664ef90fbccf29b8bfea1aa7607f1d419c
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 15201f7..398106c 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -237,10 +237,10 @@
case SuperstructureState::IDLE: {
// Only change the turret's goal loading position when idle, to prevent us
// spinning the turret around when TRANSFERRING...
+ if (have_active_intake_request) {
+ turret_intake_state_ = unsafe_goal->turret_intake();
+ }
if (front_intake_has_ball_ != back_intake_has_ball_) {
- if (have_active_intake_request) {
- turret_intake_state_ = unsafe_goal->turret_intake();
- }
turret_intake_state_ = front_intake_has_ball_ ? RequestedIntake::kFront
: RequestedIntake::kBack;
}
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 08edbf0..6c19052 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -998,6 +998,8 @@
SuperstructureState::TRANSFERRING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_BACK_BALL);
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
+ constants::Values::kTurretBackIntakePos(), 0.001);
// Since the intake beambreak hasn't triggered in a while, it should realize
// the ball was lost.