Don't unwrap the turret when firing a second ball
We only want to do this when intaking the 1st.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I8bb0ab803e7d2878c5f38dec6e53ed9a235d0764
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 24bb00d..abf3aef 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -281,11 +281,26 @@
(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);
+ if (transitioning_second_ball_) {
+ // Turn to the loading position as close to the current position as
+ // possible since we just aimed.
+ turret_loading_position =
+ turret_.estimated_position() +
+ aos::math::NormalizeAngle(turret_loading_position -
+ turret_.estimated_position());
+ }
+
+ if (!transitioning_second_ball_ ||
+ (turret_loading_position > values_->turret_range.upper ||
+ turret_loading_position < values_->turret_range.lower)) {
+ // 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 if we aren't transitioning a second ball. If we are doing
+ // the second ball, we need to reset back to the middle of the range
+ 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(
@@ -345,6 +360,7 @@
(turret_intake_state_ == RequestedIntake::kBack &&
!back_intake_has_ball_)) {
state_ = SuperstructureState::IDLE;
+ transitioning_second_ball_ = false;
break;
}
@@ -493,6 +509,8 @@
fire_ = false;
discarding_ball_ = false;
state_ = SuperstructureState::IDLE;
+ transitioning_second_ball_ =
+ (front_intake_has_ball_ || back_intake_has_ball_);
}
break;
@@ -619,6 +637,7 @@
}
status_builder.add_front_intake_has_ball(front_intake_has_ball_);
status_builder.add_back_intake_has_ball(back_intake_has_ball_);
+ status_builder.add_transitioning_second_ball(transitioning_second_ball_);
status_builder.add_aimer(aimer_offset);
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index edb3831..5e3415c 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -103,6 +103,7 @@
SuperstructureState state_ = SuperstructureState::IDLE;
bool front_intake_has_ball_ = false;
bool back_intake_has_ball_ = false;
+ bool transitioning_second_ball_ = false;
std::optional<double> last_shot_angle_ = std::nullopt;
RequestedIntake turret_intake_state_ = RequestedIntake::kFront;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index cbc71d4..e582d9e 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -997,8 +997,9 @@
SuperstructureState::TRANSFERRING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_BACK_BALL);
+ EXPECT_TRUE(superstructure_status_fetcher_->transitioning_second_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.
@@ -1006,6 +1007,7 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+ EXPECT_FALSE(superstructure_status_fetcher_->transitioning_second_ball());
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::NO_BALL);
}
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 44dc19a..bfa8f3f 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -51,6 +51,8 @@
// Whether the front/rear intakes currently are holding balls.
front_intake_has_ball:bool (id: 18);
back_intake_has_ball:bool (id: 19);
+ // Whether we just shot a ball and are transitioning a second one
+ transitioning_second_ball:bool (id: 24);
// Whether the flippers are open for shooting
flippers_open:bool (id: 12);
// Whether the flippers failed to open and we are retrying