Pre-serialize balls while intaking
Change-Id: I90d1b515748bad727d8e8b2d659b8e59ca545e80
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 70ea319..4d446ee 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -209,26 +209,30 @@
status->Send(status_builder.Finish());
if (output != nullptr) {
+ output_struct.washing_machine_spinner_voltage = 0.0;
+ output_struct.feeder_voltage = 0.0;
+ output_struct.intake_roller_voltage = 0.0;
if (unsafe_goal) {
- output_struct.washing_machine_spinner_voltage = 0.0;
+ if ((unsafe_goal->shooting() || unsafe_goal->intake_preloading()) &&
+ !position->intake_beambreak_triggered()) {
+ output_struct.washing_machine_spinner_voltage = 5.0;
+ output_struct.feeder_voltage = 12.0;
+ }
+
if (unsafe_goal->shooting()) {
if (shooter_.ready() && shooter_.finisher_goal() > 10.0 &&
shooter_.accelerator_goal() > 10.0) {
output_struct.feeder_voltage = 12.0;
- } else {
- output_struct.feeder_voltage = 0.0;
}
output_struct.washing_machine_spinner_voltage = 5.0;
output_struct.intake_roller_voltage = 3.0;
} else {
- output_struct.feeder_voltage = 0.0;
output_struct.intake_roller_voltage =
unsafe_goal->roller_voltage() +
std::max(velocity * unsafe_goal->roller_speed_compensation(), 0.0f);
}
- } else {
- output_struct.intake_roller_voltage = 0.0;
}
+
output->Send(Output::Pack(*output->fbb(), &output_struct));
}
}
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index 792b6b4..990234a 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -49,6 +49,10 @@
// Whether the kicker and flywheel should choose a velocity automatically.
shooter_tracking:bool (id: 9);
+ // Whether to serialize a ball under the accelerator tower
+ // so it is ready to shoot.
+ intake_preloading:bool (id: 12);
+
// Positive is deploying climber and to climb; cannot run in reverse
climber_voltage:float (id: 10);
}
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 029475a..4fa49fc 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -204,6 +204,8 @@
position_builder.add_intake_joint(intake_offset);
position_builder.add_turret(turret_offset);
position_builder.add_shooter(shooter_offset);
+ position_builder.add_intake_beambreak_triggered(
+ intake_beambreak_triggered_);
builder.Send(position_builder.Finish());
}
@@ -384,6 +386,10 @@
finisher_plant_->set_voltage_offset(value);
}
+ void set_intake_beambreak_triggered(bool triggered) {
+ intake_beambreak_triggered_ = triggered;
+ }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
@@ -419,6 +425,8 @@
double peak_turret_velocity_ = 1e10;
float climber_voltage_ = 0.0f;
+
+ bool intake_beambreak_triggered_ = false;
};
class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
@@ -936,6 +944,51 @@
VerifyNearGoal();
}
+// Tests that preserializing balls works.
+TEST_F(SuperstructureTest, Preserializing) {
+ SetEnabled(true);
+ // Set a reasonable goal.
+
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_preloading(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ }
+
+ superstructure_plant_.set_intake_beambreak_triggered(false);
+
+ // Give it time to stabilize.
+ RunFor(chrono::seconds(1));
+
+ // Preloads balls.
+ superstructure_output_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr);
+ EXPECT_EQ(superstructure_output_fetcher_->feeder_voltage(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->washing_machine_spinner_voltage(),
+ 5.0);
+
+ VerifyNearGoal();
+
+ superstructure_plant_.set_intake_beambreak_triggered(true);
+
+ // Give it time to stabilize.
+ RunFor(chrono::seconds(1));
+
+ // Stops preloading balls once one ball is in place
+ superstructure_output_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr);
+ EXPECT_EQ(superstructure_output_fetcher_->feeder_voltage(), 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->washing_machine_spinner_voltage(),
+ 0.0);
+
+ VerifyNearGoal();
+}
+
// Makes sure that a negative number is not added to the to the
// roller_voltage
TEST_F(SuperstructureTest, NegativeRollerSpeedCompensation) {
diff --git a/y2020/control_loops/superstructure/superstructure_position.fbs b/y2020/control_loops/superstructure/superstructure_position.fbs
index 1e9f81a..b1576ea 100644
--- a/y2020/control_loops/superstructure/superstructure_position.fbs
+++ b/y2020/control_loops/superstructure/superstructure_position.fbs
@@ -27,6 +27,10 @@
// Position of the control panel, relative to start, positive counterclockwise from above.
control_panel:frc971.RelativePosition (id: 4);
+
+ // Value of the beambreak sensor detecting when
+ // a ball is just below the accelerator tower; true is a ball.
+ intake_beambreak_triggered:bool (id: 5);
}
root_type Position;