Refactored Joystick Reader and Updated 2019.
Change-Id: I1d3d75436ae9c131c71609440d98af83298ec26c
diff --git a/aos/input/BUILD b/aos/input/BUILD
index dd79c5e..0da8d01 100644
--- a/aos/input/BUILD
+++ b/aos/input/BUILD
@@ -48,3 +48,18 @@
"//aos/robot_state",
],
)
+
+cc_library(
+ name = "action_joystick_input",
+ srcs = ["action_joystick_input.cc"],
+ hdrs = ["action_joystick_input.h"],
+ deps = [
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/input:drivetrain_input",
+ "//aos/input:joystick_input",
+ "//aos/logging",
+ "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:base_autonomous_actor",
+ ],
+)
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
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
new file mode 100644
index 0000000..d4023ca
--- /dev/null
+++ b/aos/input/action_joystick_input.h
@@ -0,0 +1,48 @@
+#ifndef AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
+#define AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
+
+#include "aos/input/driver_station_data.h"
+#include "aos/input/drivetrain_input.h"
+#include "aos/input/joystick_input.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+
+namespace aos {
+namespace input {
+
+// Class to abstract out managing actions, autonomous mode, and drivetrains.
+// Turns out we do the same thing every year, so let's stop copying it.
+class ActionJoystickInput : public ::aos::input::JoystickInput {
+ public:
+ ActionJoystickInput(
+ ::aos::EventLoop *event_loop,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config)
+ : ::aos::input::JoystickInput(event_loop),
+ drivetrain_input_reader_(DrivetrainInputReader::Make(
+ DrivetrainInputReader::InputType::kPistol, dt_config)) {}
+
+ virtual ~ActionJoystickInput() {}
+
+ private:
+ // Handles any year specific superstructure code.
+ virtual void HandleTeleop(const ::aos::input::driver_station::Data &data) = 0;
+
+ void RunIteration(const ::aos::input::driver_station::Data &data) override;
+
+ void StartAuto();
+ void StopAuto();
+
+ // True if the internal state machine thinks auto is running right now.
+ bool auto_running_ = false;
+ // True if an action was running last cycle.
+ bool was_running_ = false;
+
+ ::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
+ ::aos::common::actions::ActionQueue action_queue_;
+};
+
+} // namespace input
+} // namespace aos
+
+#endif // AOS_INPUT_ACTION_JOYSTICK_INPUT_H_