Refactored Joystick Reader and Updated 2019.
Change-Id: I1d3d75436ae9c131c71609440d98af83298ec26c
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
new file mode 100644
index 0000000..991ea17
--- /dev/null
+++ b/aos/input/action_joystick_input.cc
@@ -0,0 +1,50 @@
+#include "aos/input/action_joystick_input.h"
+
+#include "aos/input/driver_station_data.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+
+using ::aos::input::driver_station::ControlBit;
+
+namespace aos {
+namespace input {
+
+void ActionJoystickInput::RunIteration(
+ const ::aos::input::driver_station::Data &data) {
+ const bool last_auto_running = auto_running_;
+ auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
+ data.GetControlBit(ControlBit::kEnabled);
+ if (auto_running_ != last_auto_running) {
+ if (auto_running_) {
+ StartAuto();
+ } else {
+ StopAuto();
+ }
+ }
+
+ if (!auto_running_) {
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+ drivetrain_input_reader_->HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+
+ // Process pending actions.
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+}
+
+void ActionJoystickInput::StartAuto() {
+ LOG(INFO, "Starting auto mode\n");
+ action_queue_.EnqueueAction(::frc971::autonomous::MakeAutonomousAction(0));
+}
+
+void ActionJoystickInput::StopAuto() {
+ LOG(INFO, "Stopping auto mode\n");
+ action_queue_.CancelAllActions();
+}
+
+} // namespace input
+} // namespace aos