Add left/right switches for auto
Change-Id: Idde67276c32050fe7541bf621c4eb818a773a950
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index fd9df73..e75b825 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -203,6 +203,11 @@
vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
}
+ // Auto mode switches.
+ void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+ autonomous_modes_.at(i) = ::std::move(sensor);
+ }
+
void RunIteration() override {
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -254,6 +259,18 @@
superstructure_message.Send();
}
+
+ {
+ auto auto_mode_message = ::frc971::autonomous::auto_mode.MakeMessage();
+ auto_mode_message->mode = 0;
+ for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
+ if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
+ auto_mode_message->mode |= 1 << i;
+ }
+ }
+ LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
+ auto_mode_message.Send();
+ }
}
private:
@@ -262,6 +279,8 @@
::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
+ ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+
::frc971::wpilib::AbsoluteEncoder intake_encoder_;
// TODO(sabina): Add wrist and elevator hall effects.
};
@@ -646,6 +665,9 @@
reader.set_pwm_trigger(true);
reader.set_vacuum_sensor(7);
+ reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
+ reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));
+
::std::thread reader_thread(::std::ref(reader));
CameraReader camera_reader;