Pre-serialize balls while intaking
Change-Id: I90d1b515748bad727d8e8b2d659b8e59ca545e80
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 794d1e7..c504fb4 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -195,6 +195,7 @@
set_intake_goal(1.25);
set_roller_voltage(12.0);
+ set_intake_preloading(true);
SendSuperstructureGoal();
if (!spline->WaitForPlan()) return;
@@ -306,6 +307,7 @@
builder.MakeBuilder<superstructure::Goal>();
superstructure_builder.add_intake(intake_offset);
+ superstructure_builder.add_intake_preloading(intake_preloading_);
superstructure_builder.add_roller_voltage(roller_voltage_);
superstructure_builder.add_roller_speed_compensation(
kRollerSpeedCompensation);
@@ -318,6 +320,7 @@
void AutonomousActor::RetractIntake() {
set_intake_goal(-0.89);
set_roller_voltage(0.0);
+ set_intake_preloading(false);
SendSuperstructureGoal();
}
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 7bdb44b..1bb864e 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -8,9 +8,9 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2020/actors/auto_splines.h"
-#include "y2020/vision/galactic_search_path_generated.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/vision/galactic_search_path_generated.h"
namespace y2020 {
namespace actors {
@@ -30,6 +30,9 @@
void Reset();
void set_intake_goal(double intake_goal) { intake_goal_ = intake_goal; }
+ void set_intake_preloading(bool intake_preloading) {
+ intake_preloading_ = intake_preloading;
+ }
void set_roller_voltage(double roller_voltage) {
roller_voltage_ = roller_voltage;
}
@@ -48,6 +51,7 @@
double intake_goal_ = 0.0;
double roller_voltage_ = 0.0;
+ bool intake_preloading_ = false;
const float kRollerSpeedCompensation = 2.0;
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
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;
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index bb57997..6565460 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -107,6 +107,7 @@
double accelerator_speed = 0.0;
double finisher_speed = 0.0;
double climber_speed = 0.0;
+ bool preload_intake = false;
const bool auto_track = data.IsPressed(kAutoTrack);
@@ -143,6 +144,7 @@
intake_pos = 1.2;
roller_speed = 7.0f;
roller_speed_compensation = 2.0f;
+ preload_intake = true;
}
if (superstructure_status_fetcher_.get() &&
@@ -154,6 +156,7 @@
if (data.IsPressed(kIntakeIn)) {
roller_speed = 6.0f;
roller_speed_compensation = 2.0f;
+ preload_intake = true;
} else if (data.IsPressed(kSpit)) {
roller_speed = -6.0f;
}
@@ -205,6 +208,7 @@
superstructure_goal_builder.add_turret_tracking(auto_track);
superstructure_goal_builder.add_hood_tracking(auto_track);
superstructure_goal_builder.add_shooter_tracking(auto_track);
+ superstructure_goal_builder.add_intake_preloading(preload_intake);
if (!builder.Send(superstructure_goal_builder.Finish())) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index fb46d71..4a7343f 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -211,6 +211,10 @@
ball_beambreak_reader_.set_input_two(ball_beambreak_inputs_[1].get());
}
+ void set_ball_intake_beambreak(::std::unique_ptr<frc::DigitalInput> sensor) {
+ ball_intake_beambreak_ = ::std::move(sensor);
+ }
+
void Start() override {
if (FLAGS_shooter_tuning) {
AddToDMA(&ball_beambreak_reader_);
@@ -294,6 +298,8 @@
position_builder.add_intake_joint(intake_joint_offset);
position_builder.add_turret(turret_offset);
position_builder.add_shooter(shooter_offset);
+ position_builder.add_intake_beambreak_triggered(
+ !ball_intake_beambreak_->Get());
builder.Send(position_builder.Finish());
}
@@ -354,6 +360,8 @@
frc971::wpilib::ADIS16470 *imu_ = nullptr;
+ std::unique_ptr<frc::DigitalInput> ball_intake_beambreak_;
+
// Used to interface with the two beam break sensors that the ball for tuning
// shooter parameters has to pass through.
// We will time how long it takes to pass between the two sensors to get its
@@ -582,6 +590,8 @@
sensor_reader.set_left_accelerator_encoder(make_encoder(4));
sensor_reader.set_right_accelerator_encoder(make_encoder(3));
+ sensor_reader.set_ball_intake_beambreak(make_unique<frc::DigitalInput>(4));
+
if (FLAGS_shooter_tuning) {
sensor_reader.set_ball_beambreak_inputs(
make_unique<frc::DigitalInput>(6), make_unique<frc::DigitalInput>(7));