Merge "Use setpoint in joystick reader"
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index 67156dd..60eabc2 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -180,6 +180,34 @@
if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
}
+bool AutonomousActor::WaitForPreloaded() {
+ set_preloaded(true);
+ SendSuperstructureGoal();
+
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ ActorBase::kLoopOffset);
+
+ bool loaded = false;
+ while (!loaded) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ phased_loop.SleepUntilNext();
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+
+ loaded = (superstructure_status_fetcher_->state() ==
+ control_loops::superstructure::SuperstructureState::LOADED);
+ }
+
+ set_preloaded(false);
+ SendSuperstructureGoal();
+
+ return true;
+}
+
void AutonomousActor::SendSuperstructureGoal() {
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -220,6 +248,7 @@
transfer_roller_back_voltage_);
superstructure_builder.add_catapult(catapult_goal_offset);
superstructure_builder.add_fire(fire_);
+ superstructure_builder.add_preloaded(preloaded_);
superstructure_builder.add_auto_aim(true);
if (builder.Send(superstructure_builder.Finish()) !=
@@ -260,5 +289,27 @@
SendSuperstructureGoal();
}
+bool AutonomousActor::WaitForBallsShot(int num_wanted) {
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ ActorBase::kLoopOffset);
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+ int initial_balls = superstructure_status_fetcher_->shot_count();
+ LOG(INFO) << "Waiting for balls, started with " << initial_balls;
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+ if (superstructure_status_fetcher_->shot_count() - initial_balls >=
+ num_wanted) {
+ return true;
+ }
+ }
+}
+
} // namespace actors
} // namespace y2022
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index 37a8cdb..8200f70 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -48,8 +48,10 @@
}
void set_fire_at_will(bool fire) { fire_ = fire; }
+ void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
void SendSuperstructureGoal();
+ bool WaitForBallsShot(int num_shot);
void ExtendFrontIntake();
void RetractFrontIntake();
void ExtendBackIntake();
@@ -57,6 +59,10 @@
void SendStartingPosition(const Eigen::Vector3d &start);
void MaybeSendStartingPosition();
+ // Tells the superstructure the ball was preloaded and waits until it updates
+ // the state
+ bool WaitForPreloaded();
+
void SplineAuto();
void Replan();
@@ -68,6 +74,7 @@
double transfer_roller_front_voltage_ = 0.0;
double transfer_roller_back_voltage_ = 0.0;
bool fire_ = false;
+ bool preloaded_ = false;
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index ce4ed01..e45917a 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -86,7 +86,7 @@
unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
}
- // Supersturcture state machine:
+ // Superstructure state machine:
// 1. IDLE: Wait until an intake beambreak is triggerred, meaning that a ball
// is being intaked. This means that the transfer rollers have a ball. If
// we've been waiting here for too long without any beambreak triggered, the
@@ -113,6 +113,22 @@
// goes back to its return position. We have now finished the shot, so return
// to IDLE.
+ // If we started off preloaded, skip to the loaded state.
+ // Make sure we weren't already there just in case.
+ if (unsafe_goal != nullptr && unsafe_goal->preloaded()) {
+ switch (state_) {
+ case SuperstructureState::IDLE:
+ case SuperstructureState::TRANSFERRING:
+ case SuperstructureState::LOADING:
+ state_ = SuperstructureState::LOADED;
+ loading_timer_ = timestamp;
+ break;
+ case SuperstructureState::LOADED:
+ case SuperstructureState::SHOOTING:
+ break;
+ }
+ }
+
const bool is_spitting = ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
transfer_roller_speed_front < 0) ||
(intake_state_ == IntakeState::INTAKE_BACK_BALL &&
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 6266b3c..8c7c862 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -48,6 +48,9 @@
// If true, auto-track the turret to point at the goal.
auto_aim:bool (id: 11);
+
+ // If true, we started with the ball loaded and should proceed to that state.
+ preloaded:bool (id: 12);
}
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index ac4e079..66a615e 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -1199,6 +1199,25 @@
EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
}
+// Test that we are able to signal that the ball was preloaded
+TEST_F(SuperstructureTest, Preloaded) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_preloaded(true);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(dt());
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
+}
+
// Tests that the turret switches to auto-aiming when we set auto_aim to
// true.
TEST_F(SuperstructureTest, TurretAutoAim) {
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 1005c46..4b215ac 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -13,16 +13,16 @@
// State of the superstructure state machine
enum SuperstructureState : ubyte {
// Before a ball is intaked, when neither intake beambreak is triggered
- IDLE,
+ IDLE = 0,
// Transferring ball with transfer rollers. Moves turret to loading position.
- TRANSFERRING,
+ TRANSFERRING = 1,
// Loading the ball into the catapult
- LOADING,
+ LOADING = 2,
// The ball is loaded into the catapult
- LOADED,
+ LOADED = 3,
// Waiting for the turret to be at shooting goal and then telling the
// catapult to fire.
- SHOOTING,
+ SHOOTING = 4,
}
table AimerStatus {