Refactored HandleDrivetrain from joystick_reader to aos_input.
Change-Id: I4ed796cf90e698ed2198f0da4a2b75e88e28c434
diff --git a/aos/common/commonmath.h b/aos/common/commonmath.h
index f9337b2..acbcb1c 100644
--- a/aos/common/commonmath.h
+++ b/aos/common/commonmath.h
@@ -1,6 +1,8 @@
#ifndef AOS_COMMON_MATH_H_
#define AOS_COMMON_MATH_H_
+#include <cmath>
+
namespace aos {
// Clips a value so that it is in [min, max]
@@ -22,6 +24,20 @@
}
}
+// Adds deadband to provided value. deadband is the region close to the origin
+// to add the deadband to, and max is the maximum input value used to re-scale
+// the output after adding the deadband.
+static inline double Deadband(double value, const double deadband,
+ const double max) {
+ if (::std::abs(value) < deadband) {
+ value = 0.0;
+ } else if (value > 0.0) {
+ value = (value - deadband) / (max - deadband);
+ } else {
+ value = (value + deadband) / (max - deadband);
+ }
+ return value;
+}
} // namespace aos
#endif // AOS_COMMON_MATH_H_
diff --git a/aos/input/BUILD b/aos/input/BUILD
index 30cb053..d8fc2c8 100644
--- a/aos/input/BUILD
+++ b/aos/input/BUILD
@@ -16,3 +16,22 @@
'//aos/common/logging:queue_logging',
],
)
+
+cc_library(
+ name = 'drivetrain_input',
+ srcs = [
+ 'drivetrain_input.cc'
+ ],
+ hdrs = [
+ 'drivetrain_input.h',
+ ],
+ deps = [
+ '//aos/common/input:driver_station_data',
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/messages:robot_state',
+ '//aos/common/network:socket',
+ '//aos/common:math',
+ '//frc971/control_loops/drivetrain:drivetrain_queue',
+ ],
+)
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
new file mode 100644
index 0000000..a0d95cf
--- /dev/null
+++ b/aos/input/drivetrain_input.cc
@@ -0,0 +1,188 @@
+#include "aos/input/drivetrain_input.h"
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <cmath>
+
+#include "aos/common/commonmath.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::ControlBit;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::POVLocation;
+
+namespace aos {
+namespace input {
+
+void DrivetrainInputReader::HandleDrivetrain(
+ const ::aos::input::driver_station::Data &data) {
+ bool is_control_loop_driving = false;
+
+ const auto wheel_and_throttle = GetWheelAndThrottle(data);
+ const double wheel = wheel_and_throttle.wheel;
+ const double throttle = wheel_and_throttle.throttle;
+
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get()) {
+ robot_velocity_ = drivetrain_queue.status->robot_speed;
+ }
+
+ if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
+ if (drivetrain_queue.status.get()) {
+ left_goal_ = drivetrain_queue.status->estimated_left_position;
+ right_goal_ = drivetrain_queue.status->estimated_right_position;
+ }
+ }
+ const double current_left_goal =
+ left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
+ const double current_right_goal =
+ right_goal_ + wheel * wheel_multiplier_ + throttle * 0.3;
+ if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
+ is_control_loop_driving = true;
+ }
+ auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
+ new_drivetrain_goal->steering = wheel;
+ new_drivetrain_goal->throttle = throttle;
+ new_drivetrain_goal->quickturn = data.IsPressed(kQuickTurn);
+ new_drivetrain_goal->control_loop_driving = is_control_loop_driving;
+ new_drivetrain_goal->left_goal = current_left_goal;
+ new_drivetrain_goal->right_goal = current_right_goal;
+ new_drivetrain_goal->left_velocity_goal = 0;
+ new_drivetrain_goal->right_velocity_goal = 0;
+
+ new_drivetrain_goal->linear.max_velocity = 3.0;
+ new_drivetrain_goal->linear.max_acceleration = 20.0;
+
+ if (!new_drivetrain_goal.Send()) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+}
+
+DrivetrainInputReader::WheelAndThrottle
+SteeringWheelDrivetrainInputReader::GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) {
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+}
+
+DrivetrainInputReader::WheelAndThrottle
+PistolDrivetrainInputReader::GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) {
+ const double unscaled_wheel = data.GetAxis(kSteeringWheel);
+ double wheel;
+ if (unscaled_wheel < 0.0) {
+ wheel = unscaled_wheel / 0.484375;
+ } else {
+ wheel = unscaled_wheel / 0.385827;
+ }
+
+ const double unscaled_throttle = -data.GetAxis(kDriveThrottle);
+ double unmodified_throttle;
+ if (unscaled_throttle < 0.0) {
+ unmodified_throttle = unscaled_throttle / 0.086614;
+ } else {
+ unmodified_throttle = unscaled_throttle / 0.265625;
+ }
+ unmodified_throttle = aos::Deadband(unmodified_throttle, 0.1, 1.0);
+
+ // Apply a sin function that's scaled to make it feel better.
+ constexpr double throttle_range = M_PI_2 * 0.5;
+
+ double throttle = ::std::sin(throttle_range * unmodified_throttle) /
+ ::std::sin(throttle_range);
+ throttle = ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
+ throttle = 2.0 * unmodified_throttle - throttle;
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+}
+
+DrivetrainInputReader::WheelAndThrottle
+XboxDrivetrainInputReader::GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) {
+ // xbox
+ constexpr double kWheelDeadband = 0.05;
+ constexpr double kThrottleDeadband = 0.05;
+ const double wheel =
+ aos::Deadband(-data.GetAxis(kSteeringWheel), kWheelDeadband, 1.0);
+
+ const double unmodified_throttle =
+ aos::Deadband(-data.GetAxis(kDriveThrottle), kThrottleDeadband, 1.0);
+
+ // Apply a sin function that's scaled to make it feel better.
+ constexpr double throttle_range = M_PI_2 * 0.9;
+
+ double throttle = ::std::sin(throttle_range * unmodified_throttle) /
+ ::std::sin(throttle_range);
+ throttle = ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
+ throttle = 2.0 * unmodified_throttle - throttle;
+ return DrivetrainInputReader::WheelAndThrottle{wheel, throttle};
+}
+
+std::unique_ptr<SteeringWheelDrivetrainInputReader>
+SteeringWheelDrivetrainInputReader::Make() {
+ const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+ const ButtonLocation kQuickTurn(1, 5);
+ const ButtonLocation kTurn1(1, 7);
+ const ButtonLocation kTurn2(1, 11);
+ std::unique_ptr<SteeringWheelDrivetrainInputReader> result(
+ new SteeringWheelDrivetrainInputReader(kSteeringWheel, kDriveThrottle,
+ kQuickTurn, kTurn1, kTurn2));
+ return result;
+}
+
+std::unique_ptr<PistolDrivetrainInputReader>
+PistolDrivetrainInputReader::Make() {
+ // Pistol Grip controller
+ const JoystickAxis kSteeringWheel(1, 2), kDriveThrottle(1, 1);
+ const ButtonLocation kQuickTurn(1, 7);
+ const ButtonLocation kTurn1(1, 8);
+
+ // Nop
+ const ButtonLocation kTurn2(1, 9);
+ std::unique_ptr<PistolDrivetrainInputReader> result(
+ new PistolDrivetrainInputReader(kSteeringWheel, kDriveThrottle,
+ kQuickTurn, kTurn1, kTurn2));
+ result->set_wheel_multiplier(0.2);
+ return result;
+}
+
+std::unique_ptr<XboxDrivetrainInputReader> XboxDrivetrainInputReader::Make() {
+ // xbox
+ const JoystickAxis kSteeringWheel(1, 5), kDriveThrottle(1, 2);
+ const ButtonLocation kQuickTurn(1, 5);
+
+ // Nop
+ const ButtonLocation kTurn1(1, 1);
+ const ButtonLocation kTurn2(1, 2);
+
+ std::unique_ptr<XboxDrivetrainInputReader> result(
+ new XboxDrivetrainInputReader(kSteeringWheel, kDriveThrottle, kQuickTurn,
+ kTurn1, kTurn2));
+ return result;
+}
+::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
+ InputType type) {
+ std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader;
+
+ using InputType = DrivetrainInputReader::InputType;
+ switch (type) {
+ case InputType::kSteeringWheel:
+ drivetrain_input_reader = SteeringWheelDrivetrainInputReader::Make();
+ break;
+ case InputType::kPistol:
+ drivetrain_input_reader = PistolDrivetrainInputReader::Make();
+ break;
+ case InputType::kXbox:
+ drivetrain_input_reader = XboxDrivetrainInputReader::Make();
+ break;
+ }
+ return drivetrain_input_reader;
+}
+
+} // namespace input
+} // namespace aos
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
new file mode 100644
index 0000000..4b13ac6
--- /dev/null
+++ b/aos/input/drivetrain_input.h
@@ -0,0 +1,144 @@
+#ifndef AOS_INPUT_DRIVETRAIN_INPUT_H_
+#define AOS_INPUT_DRIVETRAIN_INPUT_H_
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <cmath>
+#include <memory>
+
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+
+namespace aos {
+namespace input {
+
+// We have a couple different joystick configurations used to drive our skid
+// steer robots. These configurations don't change very often, and there are a
+// small, discrete, set of them. The interface to the drivetrain is the same
+// across all of our drivetrains, so we can abstract that away from our users.
+//
+// We want to be able to re-use that code across years, and switch joystick
+// types quickly and efficiently.
+//
+// DrivetrainInputReader is the abstract base class which provides a consistent
+// API to control drivetrains.
+//
+// To construct the appropriate DrivetrainInputReader, call
+// DrivetrainInputReader::Make with the input type enum.
+// To use it, call HandleDrivetrain(data) every time a joystick packet is
+// received.
+//
+// Base class for the interface to the drivetrain implemented by multiple
+// joystick types.
+class DrivetrainInputReader {
+ public:
+ // Inputs driver station button and joystick locations
+ DrivetrainInputReader(driver_station::JoystickAxis kSteeringWheel,
+ driver_station::JoystickAxis kDriveThrottle,
+ driver_station::ButtonLocation kQuickTurn,
+ driver_station::ButtonLocation kTurn1,
+ driver_station::ButtonLocation kTurn2)
+ : kSteeringWheel(kSteeringWheel),
+ kDriveThrottle(kDriveThrottle),
+ kQuickTurn(kQuickTurn),
+ kTurn1(kTurn1),
+ kTurn2(kTurn2) {}
+
+ virtual ~DrivetrainInputReader() = default;
+
+ // List of driver joystick types.
+ enum class InputType {
+ kSteeringWheel,
+ kPistol,
+ kXbox,
+ };
+
+ // Constructs the appropriate DrivetrainInputReader.
+ static std::unique_ptr<DrivetrainInputReader> Make(InputType type);
+
+ // Processes new joystick data and publishes drivetrain goal messages.
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data);
+
+ // Sets the scalar for the steering wheel for closed loop mode converting
+ // steering ratio to meters displacement on the two wheels.
+ void set_wheel_multiplier(double wheel_multiplier) {
+ wheel_multiplier_ = wheel_multiplier;
+ }
+
+ // Returns the current robot velocity in m/s.
+ double robot_velocity() const { return robot_velocity_; }
+
+ protected:
+ const driver_station::JoystickAxis kSteeringWheel;
+ const driver_station::JoystickAxis kDriveThrottle;
+ const driver_station::ButtonLocation kQuickTurn;
+ const driver_station::ButtonLocation kTurn1;
+ const driver_station::ButtonLocation kTurn2;
+
+ // Structure containing the (potentially adjusted) steering and throttle
+ // values from the joysticks.
+ struct WheelAndThrottle {
+ double wheel;
+ double throttle;
+ };
+
+ private:
+ // Computes the steering and throttle from the provided driverstation data.
+ virtual WheelAndThrottle GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) = 0;
+
+ double robot_velocity_ = 0.0;
+ // Goals to send to the drivetrain in closed loop mode.
+ double left_goal_ = 0.0;
+ double right_goal_ = 0.0;
+ // The scale for the joysticks for closed loop mode converting
+ // joysticks to meters displacement on the two wheels.
+ double wheel_multiplier_ = 0.5;
+};
+
+// Implements DrivetrainInputReader for the original steering wheel.
+class SteeringWheelDrivetrainInputReader : public DrivetrainInputReader {
+ public:
+ using DrivetrainInputReader::DrivetrainInputReader;
+
+ // Creates a DrivetrainInputReader with the corresponding joystick ports and
+ // axis for the big steering wheel and throttle stick.
+ static std::unique_ptr<SteeringWheelDrivetrainInputReader> Make();
+
+ private:
+ WheelAndThrottle GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) override;
+};
+
+class PistolDrivetrainInputReader : public DrivetrainInputReader {
+ public:
+ using DrivetrainInputReader::DrivetrainInputReader;
+
+ // Creates a DrivetrainInputReader with the corresponding joystick ports and
+ // axis for the (cheap) pistol grip controller.
+ static std::unique_ptr<PistolDrivetrainInputReader> Make();
+
+ private:
+ WheelAndThrottle GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) override;
+};
+
+class XboxDrivetrainInputReader : public DrivetrainInputReader {
+ public:
+ using DrivetrainInputReader::DrivetrainInputReader;
+
+ // Creates a DrivetrainInputReader with the corresponding joystick ports and
+ // axis for the Xbox controller.
+ static std::unique_ptr<XboxDrivetrainInputReader> Make();
+
+ private:
+ WheelAndThrottle GetWheelAndThrottle(
+ const ::aos::input::driver_station::Data &data) override;
+};
+
+} // namespace input
+} // namespace aos
+
+#endif // AOS_INPUT_DRIVETRAIN_INPUT_H_
diff --git a/y2017/BUILD b/y2017/BUILD
index 4bf45fa..763e331 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -36,6 +36,7 @@
'//aos/common/util:log_interval',
'//aos/common:time',
'//aos/input:joystick_input',
+ '//aos/input:drivetrain_input',
'//aos/linux_code:init',
'//frc971/autonomous:auto_queue',
'//frc971/control_loops/drivetrain:drivetrain_queue',
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index c0113c3..e0f66ba 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -8,6 +8,7 @@
#include "aos/common/logging/logging.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
+#include "aos/input/drivetrain_input.h"
#include "aos/input/joystick_input.h"
#include "aos/linux_code/init.h"
#include "frc971/autonomous/auto.q.h"
@@ -23,46 +24,12 @@
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::POVLocation;
+using ::aos::input::DrivetrainInputReader;
namespace y2017 {
namespace input {
namespace joysticks {
-// Keep the other versions around so we can switch quickly.
-//#define STEERINGWHEEL
-#define PISTOL
-//#define XBOX
-
-#ifdef STEERINGWHEEL
-const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kQuickTurn(1, 5);
-const ButtonLocation kTurn1(1, 7);
-const ButtonLocation kTurn2(1, 11);
-
-#endif
-
-#ifdef PISTOL
-// Pistol Grip controller
-const JoystickAxis kSteeringWheel(1, 2), kDriveThrottle(1, 1);
-const ButtonLocation kQuickTurn(1, 7);
-const ButtonLocation kTurn1(1, 8);
-
-// Nop
-const ButtonLocation kTurn2(1, 9);
-#endif
-
-#ifdef XBOX
-// xbox
-const JoystickAxis kSteeringWheel(1, 5), kDriveThrottle(1, 2);
-const ButtonLocation kQuickTurn(1, 5);
-
-// Nop
-const ButtonLocation kTurn1(1, 1);
-const ButtonLocation kTurn2(1, 2);
-
-#endif
-
-
const ButtonLocation kGearSlotBack(2, 11);
const ButtonLocation kIntakeDown(3, 9);
@@ -81,9 +48,14 @@
const ButtonLocation kExtra2(3, 10);
const ButtonLocation kHang(3, 2);
+std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
+
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() {}
+ Reader() {
+ drivetrain_input_reader_ = DrivetrainInputReader::Make(
+ DrivetrainInputReader::InputType::kSteeringWheel);
+ }
enum class ShotDistance { VISION_SHOT, MIDDLE_SHOT, FAR_SHOT };
@@ -112,116 +84,9 @@
}
int intake_accumulator_ = 0;
- double Deadband(double value, const double deadband) {
- if (::std::abs(value) < deadband) {
- value = 0.0;
- } else if (value > 0.0) {
- value = (value - deadband) / (1.0 - deadband);
- } else {
- value = (value + deadband) / (1.0 - deadband);
- }
- return value;
- }
-
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- bool is_control_loop_driving = false;
-
-#ifdef STEERINGWHEEL
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
-#endif
-
-#ifdef XBOX
- // xbox
- constexpr double kWheelDeadband = 0.05;
- constexpr double kThrottleDeadband = 0.05;
- const double wheel =
- Deadband(-data.GetAxis(kSteeringWheel), kWheelDeadband);
-
- const double unmodified_throttle =
- Deadband(-data.GetAxis(kDriveThrottle), kThrottleDeadband);
-
- // Apply a sin function that's scaled to make it feel better.
- constexpr double throttle_range = M_PI_2 * 0.9;
-
- double throttle = ::std::sin(throttle_range * unmodified_throttle) /
- ::std::sin(throttle_range);
- throttle =
- ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
- throttle = 2.0 * unmodified_throttle - throttle;
-#endif
-
-#ifdef PISTOL
- const double unscaled_wheel = data.GetAxis(kSteeringWheel);
- double wheel;
- if (unscaled_wheel < 0.0) {
- wheel = unscaled_wheel / 0.484375;
- } else {
- wheel = unscaled_wheel / 0.385827;
- }
-
- const double unscaled_throttle = -data.GetAxis(kDriveThrottle);
- double unmodified_throttle;
- if (unscaled_throttle < 0.0) {
- unmodified_throttle = unscaled_throttle / 0.086614;
- } else {
- unmodified_throttle = unscaled_throttle / 0.265625;
- }
- unmodified_throttle = Deadband(unmodified_throttle, 0.1);
-
- // Apply a sin function that's scaled to make it feel better.
- constexpr double throttle_range = M_PI_2 * 0.5;
-
- double throttle = ::std::sin(throttle_range * unmodified_throttle) /
- ::std::sin(throttle_range);
- throttle =
- ::std::sin(throttle_range * throttle) / ::std::sin(throttle_range);
- throttle = 2.0 * unmodified_throttle - throttle;
-#endif
-
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
- robot_velocity_ = drivetrain_queue.status->robot_speed;
- }
-
- if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2) ||
- data.PosEdge(kGearSlotBack)) {
- if (drivetrain_queue.status.get()) {
- left_goal_ = drivetrain_queue.status->estimated_left_position;
- right_goal_ = drivetrain_queue.status->estimated_right_position;
- }
- }
-#ifdef PISTOL
- double current_left_goal = left_goal_ - wheel * 0.20 + throttle * 0.3;
- double current_right_goal = right_goal_ + wheel * 0.20 + throttle * 0.3;
-#else
- double current_left_goal = left_goal_ - wheel * 0.5 + throttle * 0.3;
- double current_right_goal = right_goal_ + wheel * 0.5 + throttle * 0.3;
-#endif
- if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
- is_control_loop_driving = true;
- }
- if (data.IsPressed(kGearSlotBack)) {
- is_control_loop_driving = true;
- current_left_goal = left_goal_ - 0.03;
- current_right_goal = right_goal_ - 0.03;
- }
- auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
- new_drivetrain_goal->steering = wheel;
- new_drivetrain_goal->throttle = throttle;
- new_drivetrain_goal->quickturn = data.IsPressed(kQuickTurn);
- new_drivetrain_goal->control_loop_driving = is_control_loop_driving;
- new_drivetrain_goal->left_goal = current_left_goal;
- new_drivetrain_goal->right_goal = current_right_goal;
- new_drivetrain_goal->left_velocity_goal = 0;
- new_drivetrain_goal->right_velocity_goal = 0;
-
- new_drivetrain_goal->linear.max_velocity = 3.0;
- new_drivetrain_goal->linear.max_acceleration = 20.0;
-
- if (!new_drivetrain_goal.Send()) {
- LOG(WARNING, "sending stick values failed\n");
- }
+ drivetrain_input_reader_->HandleDrivetrain(data);
+ robot_velocity_ = drivetrain_input_reader_->robot_velocity();
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
@@ -253,7 +118,6 @@
}
}
-
if (data.IsPressed(kVisionAlign)) {
// Align shot using vision
// TODO(campbell): Add vision aligning.
@@ -441,10 +305,6 @@
double hood_goal_ = 0.3;
double shooter_velocity_ = 0.0;
- // Goals to send to the drivetrain in closed loop mode.
- double left_goal_ = 0.0;
- double right_goal_ = 0.0;
-
bool was_running_ = false;
bool auto_running_ = false;
diff --git a/y2017_bot3/joystick_reader.cc b/y2017_bot3/joystick_reader.cc
new file mode 100644
index 0000000..0d43efd
--- /dev/null
+++ b/y2017_bot3/joystick_reader.cc
@@ -0,0 +1,146 @@
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "aos/common/actions/actions.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/input/drivetrain_input.h"
+#include "aos/input/joystick_input.h"
+#include "aos/linux_code/init.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2017_bot3/actors/autonomous_actor.h"
+#include "y2017_bot3/control_loops/superstructure/superstructure.q.h"
+
+using ::frc971::control_loops::drivetrain_queue;
+using ::y2017_bot3::control_loops::superstructure_queue;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::ControlBit;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::POVLocation;
+using ::aos::input::DrivetrainInputReader;
+
+namespace y2017_bot3 {
+namespace input {
+namespace joysticks {
+
+const ButtonLocation kHangerOn(3, 1);
+const ButtonLocation kRollersOn(3, 2);
+const ButtonLocation kGearOut(3, 3);
+
+const ButtonLocation kRollerOn(3, 4);
+
+std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader() {
+ drivetrain_input_reader_ = DrivetrainInputReader::Make(
+ DrivetrainInputReader::InputType::kSteeringWheel);
+ }
+
+ void RunIteration(const ::aos::input::driver_station::Data &data) override {
+ 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_) {
+ HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+
+ // Process pending actions.
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+ }
+
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ LOG(DEBUG, "Canceling\n");
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ drivetrain_input_reader_->HandleDrivetrain(data);
+ robot_velocity_ = drivetrain_input_reader_->robot_velocity();
+ }
+
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ superstructure_queue.status.FetchLatest();
+ if (!superstructure_queue.status.get()) {
+ LOG(ERROR, "Got no superstructure status packet.\n");
+ return;
+ }
+
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+ new_superstructure_goal->voltage_rollers = 0.0;
+
+ if (data.IsPressed(kRollerOn)) {
+ new_superstructure_goal->voltage_rollers = 12.0;
+ }
+
+ if (data.IsPressed(kHangerOn)) {
+ new_superstructure_goal->hanger_voltage = 12.0;
+ }
+
+
+ if (data.IsPressed(kGearOut)) {
+ new_superstructure_goal->fingers_out = true;
+ } else {
+ new_superstructure_goal->fingers_out = false;
+ }
+
+ LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
+
+ private:
+ void StartAuto() {
+ LOG(INFO, "Starting auto mode\n");
+ ::frc971::autonomous::AutonomousActionParams params;
+ ::frc971::autonomous::auto_mode.FetchLatest();
+ if (::frc971::autonomous::auto_mode.get() != nullptr) {
+ params.mode = ::frc971::autonomous::auto_mode->mode;
+ } else {
+ LOG(WARNING, "no auto mode values\n");
+ params.mode = 0;
+ }
+ action_queue_.EnqueueAction(
+ ::frc971::autonomous::MakeAutonomousAction(params));
+ }
+
+ void StopAuto() {
+ LOG(INFO, "Stopping auto mode\n");
+ action_queue_.CancelAllActions();
+ }
+ double robot_velocity_ = 0.0;
+ ::aos::common::actions::ActionQueue action_queue_;
+
+ bool was_running_ = false;
+ bool auto_running_ = false;
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2017_bot3
+
+int main() {
+ ::aos::Init(-1);
+ ::y2017_bot3::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}