Turn lights on when button is pushed
Finished.
Change-Id: I53880c31a5d46499b7875e154bae9b5b7716091c
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 343d809..357c4b6 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -105,6 +105,9 @@
::std::max(-kMaxIndexerRollerVoltage,
::std::min(unsafe_goal->indexer.voltage_rollers,
kMaxIndexerRollerVoltage));
+
+ // Set the lights on or off
+ output->lights_on = unsafe_goal->lights_on;
}
}
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
index dd8a65a..3c44ed3 100644
--- a/y2017/control_loops/superstructure/superstructure.q
+++ b/y2017/control_loops/superstructure/superstructure.q
@@ -174,6 +174,7 @@
TurretGoal turret;
HoodGoal hood;
ShooterGoal shooter;
+ bool lights_on;
};
message Status {
@@ -221,6 +222,9 @@
double voltage_turret;
double voltage_hood;
+
+ // If true, the lights are on.
+ bool lights_on;
};
queue Goal goal;
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index 9cee19f..4d3355f 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -111,6 +111,7 @@
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
// Default the intake to in.
intake_goal_ = constants::Values::kIntakeRange.lower;
+ bool lights_on = false;
if (!data.GetControlBit(ControlBit::kEnabled)) {
action_queue_.CancelAllActions();
@@ -130,6 +131,7 @@
if (data.IsPressed(kVisionAlign)) {
// Align shot using vision
// TODO(campbell): Add vision aligning.
+ lights_on = true;
shooter_velocity_ = 100.0;
} else if (data.IsPressed(kCloseShot)) {
// Close shot
@@ -176,6 +178,7 @@
new_superstructure_goal->hood.profile_params.max_acceleration = 25.0;
new_superstructure_goal->intake.voltage_rollers = 0.0;
+ new_superstructure_goal->lights_on = lights_on;
if (data.IsPressed(kHang)) {
new_superstructure_goal->intake.voltage_rollers = -12.0;
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 2873d2c..1fdf1ec 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -461,6 +461,60 @@
::std::atomic<bool> run_{true};
};
+class SolenoidWriter {
+ public:
+ SolenoidWriter()
+ : superstructure_(".y2017.control_loops.superstructure_queue.output") {}
+
+ ::frc971::wpilib::BufferedPcm *pcm() { return &pcm_; }
+
+ void set_lights(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ lights_ = ::std::move(s);
+ }
+
+ void operator()() {
+ ::aos::SetCurrentThreadName("Solenoids");
+ ::aos::SetCurrentThreadRealtimePriority(27);
+
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(20),
+ ::std::chrono::milliseconds(1));
+
+ while (run_) {
+ {
+ const int iterations = phased_loop.SleepUntilNext();
+ if (iterations != 1) {
+ LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
+ }
+ }
+
+ {
+ superstructure_.FetchLatest();
+ if (superstructure_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+ lights_->Set(superstructure_->lights_on);
+ }
+ }
+
+ pcm_.Flush();
+ }
+ }
+
+ void Quit() { run_ = false; }
+
+ private:
+ ::frc971::wpilib::BufferedPcm pcm_;
+
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> lights_;
+
+ ::aos::Queue<
+ ::y2017::control_loops::SuperstructureQueue::Output>
+ superstructure_;
+
+ ::std::atomic<bool> run_{true};
+};
+
+
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
void set_drivetrain_left_victor(::std::unique_ptr<VictorSP> t) {
@@ -631,6 +685,11 @@
::std::thread superstructure_writer_thread(
::std::ref(superstructure_writer));
+ SolenoidWriter solenoid_writer;
+ solenoid_writer.set_lights(solenoid_writer.pcm()->MakeSolenoid(0));
+
+ ::std::thread solenoid_thread(::std::ref(solenoid_writer));
+
// Wait forever. Not much else to do...
while (true) {
const int r = select(0, nullptr, nullptr, nullptr, nullptr);