Fix turret done
It was waiting for us to be near the profiled goal, not the final goal.
Whops.
Change-Id: I0aac4b08d18386d559aeb19f6d19d08676de6203
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 4c5f235..f62109c 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -234,14 +234,19 @@
output_struct.washing_machine_spinner_voltage = 0.0;
output_struct.feeder_voltage = 0.0;
output_struct.intake_roller_voltage = 0.0;
+ output_struct.climber_voltage = 0.0;
if (unsafe_goal) {
- output_struct.climber_voltage =
- std::clamp(unsafe_goal->climber_voltage(), -12.0f, 12.0f);
+ if (unsafe_goal->has_turret()) {
+ output_struct.climber_voltage =
+ std::clamp(unsafe_goal->climber_voltage(), -12.0f, 12.0f);
- // Make sure the turret is relatively close to the goal before turning the
- // climber on.
- if (std::abs(turret_.goal(0) - turret_.position()) > 0.1) {
- output_struct.climber_voltage = 0;
+ // Make sure the turret is relatively close to the goal before turning
+ // the climber on.
+ CHECK(unsafe_goal->has_turret());
+ if (std::abs(unsafe_goal->turret()->unsafe_goal() -
+ turret_.position()) > 0.1) {
+ output_struct.climber_voltage = 0;
+ }
}
if (unsafe_goal->shooting() || unsafe_goal->intake_preloading()) {
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index b34273c..56521d7 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -942,25 +942,39 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ // Since there is a turret lockout, we need to set a turret goal...
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), M_PI / 2.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_climber_voltage(-10.0);
+ goal_builder.add_turret(turret_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
- // Give it time to stabilize.
- RunFor(chrono::seconds(1));
+ // The turret needs to move out of the way first. This takes some time.
+ RunFor(chrono::milliseconds(100));
+ EXPECT_EQ(superstructure_plant_.climber_voltage(), 0.0);
- // Can go backwards.
+ // Now, we should be far enough that it should work.
+ RunFor(chrono::seconds(10));
EXPECT_EQ(superstructure_plant_.climber_voltage(), -10.0);
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ // Since there is a turret lockout, we need to set a turret goal...
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), M_PI / 2.0);
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_climber_voltage(10.0);
+ goal_builder.add_turret(turret_offset);
ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}