Send turret to load before a ball is detected
Start once we were requested to intake instead of waiting for the
beambreak, which takes too long.
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I57eec09da609dd1179ac093487239d9ab050b647
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index e45917a..c87cc95 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -179,6 +179,44 @@
}
}
+ // Send the turret to the loading position if we have a ball in the intake, or
+ // are trying to intake one
+ double turret_loading_position = std::numeric_limits<double>::quiet_NaN();
+ if (state_ == SuperstructureState::TRANSFERRING &&
+ intake_state_ != IntakeState::NO_BALL) {
+ turret_loading_position = (intake_state_ == IntakeState::INTAKE_FRONT_BALL
+ ? constants::Values::kTurretFrontIntakePos()
+ : constants::Values::kTurretBackIntakePos());
+ } else if (state_ == SuperstructureState::IDLE && unsafe_goal != nullptr) {
+ if (unsafe_goal->roller_speed_front() > 0) {
+ turret_loading_position = constants::Values::kTurretFrontIntakePos();
+ } else if (unsafe_goal->roller_speed_back() > 0) {
+ turret_loading_position = constants::Values::kTurretBackIntakePos();
+ }
+ }
+
+ if (!std::isnan(turret_loading_position)) {
+ // 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(
+ *turret_goal_buffer.fbb(), turret_loading_position));
+ turret_goal = &turret_goal_buffer.message();
+ }
+
switch (state_) {
case SuperstructureState::IDLE: {
if (is_spitting) {
@@ -203,31 +241,6 @@
break;
}
- double turret_loading_position =
- (intake_state_ == IntakeState::INTAKE_FRONT_BALL
- ? 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(
- *turret_goal_buffer.fbb(), turret_loading_position));
- turret_goal = &turret_goal_buffer.message();
-
const bool turret_near_goal =
std::abs(turret_.estimated_position() - turret_loading_position) <
kTurretGoalThreshold;
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 8862d22..7816563 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -26,8 +26,11 @@
intake_back:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
// Positive is rollers intaking.
+ // When spinning the rollers, the turret will be moved to that side,
+ // so both shouldn't be positive at the same time.
roller_speed_front:double (id: 3);
roller_speed_back:double (id: 4);
+
transfer_roller_speed_front:double (id: 5);
transfer_roller_speed_back:double (id: 12);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 66a615e..73ceee7 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -776,6 +776,7 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
0.0);
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
@@ -783,8 +784,10 @@
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);
+ // Since we spun the front rollers, the turret should be trying to intake from
+ // there
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
+ constants::Values::kTurretFrontIntakePos(), 0.001);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
superstructure_plant_.set_intake_beambreak_front(true);