Add a catapult test which takes a shot.
It is hard to test that the optimization problem actually works
correctly, but we can check the plumbing to make sure it takes and
counts shots correctly.
Change-Id: I506fc2113d61471cb997e38e289de8f77a2ed4ab
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index 00bb3b3..e4d76d3 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -367,6 +367,7 @@
CHECK_NOTNULL(catapult_voltage);
*catapult_voltage = 0.0;
if (catapult_mpc_.started()) {
+ ++shot_count_;
// Finished the catapult, time to fire.
catapult_state_ = CatapultState::RESETTING;
}
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index a5ffc92..a8141a0 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -198,6 +198,9 @@
bool mpc_active() const { return !use_profile_; }
+ // Returns the number of shots taken.
+ int shot_count() const { return shot_count_; }
+
// Runs either the MPC or the profiled subsystem depending on if we are
// shooting or not. Returns the status.
const flatbuffers::Offset<
@@ -219,6 +222,8 @@
bool last_firing_ = false;
bool use_profile_ = true;
+
+ int shot_count_ = 0;
};
} // namespace catapult
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index a1e3f13..da1af82 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -121,6 +121,7 @@
status_builder.add_catapult(catapult_status_offset);
status_builder.add_solve_time(catapult_.solve_time());
status_builder.add_mpc_active(catapult_.mpc_active());
+ status_builder.add_shot_count(catapult_.shot_count());
(void)status->Send(status_builder.Finish());
}
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 7ab3c30..6147851 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -97,6 +97,8 @@
void set_peak_acceleration(double value) { peak_acceleration_ = value; }
void set_peak_velocity(double value) { peak_velocity_ = value; }
+ void set_controller_index(size_t index) { plant_->set_index(index); }
+
PositionSensorSimulator *encoder() { return &encoder_; }
private:
@@ -195,6 +197,11 @@
superstructure_status_fetcher_->intake_back());
turret_.Simulate(superstructure_output_fetcher_->turret_voltage(),
superstructure_status_fetcher_->turret());
+ if (superstructure_status_fetcher_->mpc_active()) {
+ catapult_.set_controller_index(0);
+ } else {
+ catapult_.set_controller_index(1);
+ }
catapult_.Simulate(
superstructure_output_fetcher_->catapult_voltage(),
superstructure_status_fetcher_->catapult());
@@ -750,6 +757,88 @@
constants::Values::kIntakeRange().upper);
}
+// Make sure that we can shoot the catapult and reload it.
+TEST_F(SuperstructureTest, ShootCatapult) {
+ SetEnabled(true);
+ superstructure_plant_.intake_front()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+ superstructure_plant_.intake_back()->InitializePosition(
+ constants::Values::kIntakeRange().middle());
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ catapult_return_position_offset =
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kCatapultRange().lower,
+ CreateProfileParameters(*builder.fbb(), 4.0, 20.0));
+
+ CatapultGoal::Builder catapult_goal_builder =
+ builder.MakeBuilder<CatapultGoal>();
+
+ catapult_goal_builder.add_fire(false);
+ catapult_goal_builder.add_shot_position(0.3);
+ catapult_goal_builder.add_shot_velocity(15.0);
+ catapult_goal_builder.add_return_position(catapult_return_position_offset);
+ flatbuffers::Offset<CatapultGoal> catapult_goal_offset =
+ catapult_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_catapult(catapult_goal_offset);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_FALSE(superstructure_status_fetcher_->mpc_active());
+ EXPECT_FLOAT_EQ(superstructure_status_fetcher_->catapult()->position(),
+ constants::Values::kCatapultRange().lower);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ catapult_return_position_offset =
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kCatapultRange().lower,
+ CreateProfileParameters(*builder.fbb(), 4.0, 20.0));
+
+ CatapultGoal::Builder catapult_goal_builder =
+ builder.MakeBuilder<CatapultGoal>();
+
+ catapult_goal_builder.add_fire(true);
+ catapult_goal_builder.add_shot_position(0.5);
+ catapult_goal_builder.add_shot_velocity(20.0);
+ catapult_goal_builder.add_return_position(catapult_return_position_offset);
+ flatbuffers::Offset<CatapultGoal> catapult_goal_offset =
+ catapult_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_catapult(catapult_goal_offset);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::milliseconds(100));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_TRUE(superstructure_status_fetcher_->mpc_active());
+ EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+ EXPECT_GT(superstructure_status_fetcher_->catapult()->position(),
+ constants::Values::kCatapultRange().lower + 0.1);
+ RunFor(chrono::milliseconds(1950));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_NEAR(superstructure_status_fetcher_->catapult()->position(),
+ constants::Values::kCatapultRange().lower, 1e-3);
+ EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 8033d8a..4487f1d 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -23,6 +23,9 @@
solve_time:double (id: 7);
mpc_active:bool (id: 8);
+
+ // The number of shots we have taken.
+ shot_count:int32 (id: 9);
}
root_type Status;