Move aos/input and aos/robot_state to frc971/input
Neither folder makes any sense as part of aos/.
Change-Id: I10e0532da4e688c18a9354012b783c43566fd2a1
diff --git a/frc971/codelab/BUILD b/frc971/codelab/BUILD
index c50a2c1..d5c1c59 100644
--- a/frc971/codelab/BUILD
+++ b/frc971/codelab/BUILD
@@ -84,6 +84,6 @@
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
- "//aos/robot_state:config",
+ "//frc971/input:config",
],
)
diff --git a/frc971/codelab/codelab.json b/frc971/codelab/codelab.json
index dedb19f..49e35b3 100644
--- a/frc971/codelab/codelab.json
+++ b/frc971/codelab/codelab.json
@@ -18,6 +18,6 @@
}
],
"imports": [
- "../../aos/robot_state/robot_state_config.json"
+ "../../frc971/input/robot_state_config.json"
]
}
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index f1c54a7..ec480c0 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -152,7 +152,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- "//aos/robot_state:config",
+ "//frc971/input:config",
],
)
@@ -317,7 +317,7 @@
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
- "//aos/robot_state:robot_state_fbs",
+ "//frc971/input:robot_state_fbs",
"//aos/util:log_interval",
"//aos/util:trapezoid_profile",
"//frc971:shifter_hall_effect",
@@ -355,7 +355,7 @@
":drivetrain_output_fbs",
":drivetrain_position_fbs",
":drivetrain_status_fbs",
- "//aos/robot_state:robot_state_fbs",
+ "//frc971/input:robot_state_fbs",
"//aos/util:log_interval",
],
"@platforms//os:none": [
diff --git a/frc971/control_loops/drivetrain/drivetrain_simulation_config.json b/frc971/control_loops/drivetrain/drivetrain_simulation_config.json
index c113bc0..8523cb1 100644
--- a/frc971/control_loops/drivetrain/drivetrain_simulation_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_simulation_config.json
@@ -1,6 +1,6 @@
{
"imports": [
- "../../../aos/robot_state/robot_state_config.json",
+ "../../../frc971/input/robot_state_config.json",
"drivetrain_config.json",
"drivetrain_simulation_channels.json"
]
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 17a32b8..fe733bb 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -7,12 +7,12 @@
#include "frc971/control_loops/drivetrain/gear.h"
#ifdef __linux__
#include "aos/logging/logging.h"
-#include "aos/robot_state/robot_state_generated.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/input/robot_state_generated.h"
#else
#include "frc971/control_loops/drivetrain/drivetrain_goal_float_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_float_generated.h"
diff --git a/frc971/input/BUILD b/frc971/input/BUILD
new file mode 100644
index 0000000..72be201
--- /dev/null
+++ b/frc971/input/BUILD
@@ -0,0 +1,101 @@
+package(default_visibility = ["//visibility:public"])
+
+load("//aos:config.bzl", "aos_config")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+cc_library(
+ name = "joystick_input",
+ srcs = [
+ "joystick_input.cc",
+ ],
+ hdrs = [
+ "joystick_input.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":robot_state_fbs",
+ "//aos/events:event_loop",
+ "//frc971/input:driver_station_data",
+ "//aos/logging",
+ ],
+)
+
+cc_library(
+ name = "drivetrain_input",
+ srcs = [
+ "drivetrain_input.cc",
+ ],
+ hdrs = [
+ "drivetrain_input.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":robot_state_fbs",
+ "//aos:math",
+ "//frc971/input:driver_station_data",
+ "//aos/logging",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/drivetrain:spline_goal_fbs",
+ ],
+)
+
+cc_library(
+ name = "driver_station_data",
+ srcs = [
+ "driver_station_data.cc",
+ ],
+ hdrs = [
+ "driver_station_data.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":joystick_state_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_library(
+ name = "action_joystick_input",
+ srcs = ["action_joystick_input.cc"],
+ hdrs = ["action_joystick_input.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":drivetrain_input",
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//frc971/input:joystick_input",
+ "//aos/logging",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/autonomous:auto_mode_fbs",
+ "//frc971/autonomous:base_autonomous_actor",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "robot_state_fbs",
+ srcs = ["robot_state.fbs"],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+flatbuffer_cc_library(
+ name = "joystick_state_fbs",
+ srcs = ["joystick_state.fbs"],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+aos_config(
+ name = "config",
+ src = "robot_state_config.json",
+ flatbuffers = [
+ ":joystick_state_fbs",
+ ":robot_state_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["//aos/events:config"],
+)
diff --git a/frc971/input/action_joystick_input.cc b/frc971/input/action_joystick_input.cc
new file mode 100644
index 0000000..4f1d655
--- /dev/null
+++ b/frc971/input/action_joystick_input.cc
@@ -0,0 +1,67 @@
+#include "frc971/input/action_joystick_input.h"
+
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/autonomous/auto_mode_generated.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/input/driver_station_data.h"
+
+using ::frc971::input::driver_station::ControlBit;
+
+namespace frc971 {
+namespace input {
+
+void ActionJoystickInput::RunIteration(
+ const ::frc971::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_) {
+ auto_was_running_ = true;
+ StartAuto();
+ } else {
+ StopAuto();
+ }
+ }
+
+ if (!auto_running_ ||
+ (input_config_.run_teleop_in_auto && !action_queue_.Running())) {
+ if (auto_was_running_) {
+ AutoEnded();
+ auto_was_running_ = false;
+ }
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ action_queue_.CancelAllActions();
+ }
+ drivetrain_input_reader_->HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+
+ if (auto_action_running_ &&
+ data.IsPressed(input_config_.cancel_auto_button)) {
+ StopAuto();
+ }
+
+ // Process pending actions.
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+}
+
+void ActionJoystickInput::StartAuto() {
+ AOS_LOG(INFO, "Starting auto mode\n");
+ frc971::autonomous::AutonomousActionParamsT params;
+ params.mode = GetAutonomousMode();
+
+ action_queue_.EnqueueAction(autonomous_action_factory_.Make(params));
+ auto_action_running_ = true;
+}
+
+void ActionJoystickInput::StopAuto() {
+ AOS_LOG(INFO, "Stopping auto mode\n");
+ action_queue_.CancelAllActions();
+ auto_action_running_ = false;
+ AutoEnded();
+}
+
+} // namespace input
+} // namespace frc971
diff --git a/frc971/input/action_joystick_input.h b/frc971/input/action_joystick_input.h
new file mode 100644
index 0000000..cdd0aa6
--- /dev/null
+++ b/frc971/input/action_joystick_input.h
@@ -0,0 +1,133 @@
+#ifndef AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
+#define AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
+
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/autonomous/auto_mode_generated.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/input/driver_station_data.h"
+#include "frc971/input/drivetrain_input.h"
+#include "frc971/input/joystick_input.h"
+
+namespace frc971 {
+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 ::frc971::input::JoystickInput {
+ public:
+ // Configuration parameters that don't really belong in the DrivetrainConfig.
+ struct InputConfig {
+ // If true, we will run teleop during auto mode after auto mode ends. This
+ // is to support the 2019 sandstorm mode. Auto will run, and then when the
+ // action ends (either when it's done, or when the driver triggers it to
+ // finish early), we will run teleop regardless of the mode.
+ bool run_teleop_in_auto = false;
+ // A button, for use with the run_teleop_in_auto, that will cancel the auto
+ // mode, and if run_telop_in_auto is specified, resume teloperation.
+ const driver_station::ButtonLocation cancel_auto_button = {-1, -1};
+ };
+ ActionJoystickInput(
+ ::aos::EventLoop *event_loop,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config,
+ DrivetrainInputReader::InputType input_type,
+ const InputConfig &input_config)
+ : ::frc971::input::JoystickInput(event_loop),
+ input_config_(input_config),
+ drivetrain_input_reader_(
+ DrivetrainInputReader::Make(event_loop, input_type, dt_config)),
+ dt_config_(dt_config),
+ autonomous_action_factory_(
+ ::frc971::autonomous::BaseAutonomousActor::MakeFactory(event_loop)),
+ autonomous_mode_fetcher_(
+ event_loop->MakeFetcher<::frc971::autonomous::AutonomousMode>(
+ "/autonomous")) {}
+
+ virtual ~ActionJoystickInput() {}
+
+ protected:
+ bool was_running_action() { return was_running_; }
+
+ // Returns true if an action is running.
+ bool ActionRunning() { return action_queue_.Running(); }
+ // Cancels all actions.
+ void CancelAllActions() { action_queue_.CancelAllActions(); }
+ // Cancels the current action.
+ void CancelCurrentAction() { action_queue_.CancelCurrentAction(); }
+
+ // Enqueues an action.
+ void EnqueueAction(::std::unique_ptr<::aos::common::actions::Action> action) {
+ action_queue_.EnqueueAction(::std::move(action));
+ }
+
+ // Returns the current robot velocity.
+ double robot_velocity() const {
+ return drivetrain_input_reader_->robot_velocity();
+ }
+
+ // Returns the drivetrain config.
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ dt_config() const {
+ return dt_config_;
+ }
+
+ // Sets the vision align function. This function runs before the normal
+ // drivetrain code runs. If it returns true, we are in vision align mode and
+ // no drivetain code is run. If it returns false, the vision align function
+ // is assumed to be disabled and normal drive code is run.
+ void set_vision_align_fn(
+ ::std::function<bool(const ::frc971::input::driver_station::Data &data)>
+ vision_align_fn) {
+ drivetrain_input_reader_->set_vision_align_fn(vision_align_fn);
+ }
+
+ private:
+ // Handles anything that needs to be cleaned up when the auto action exits.
+ virtual void AutoEnded() {}
+ // Handles any year specific superstructure code.
+ virtual void HandleTeleop(
+ const ::frc971::input::driver_station::Data &data) = 0;
+
+ void RunIteration(const ::frc971::input::driver_station::Data &data) override;
+
+ void StartAuto();
+ void StopAuto();
+
+ // Returns the current autonomous mode which has been selected by robot
+ // inputs.
+ virtual uint32_t GetAutonomousMode() {
+ autonomous_mode_fetcher_.Fetch();
+ if (autonomous_mode_fetcher_.get() == nullptr) {
+ AOS_LOG(WARNING, "no auto mode values\n");
+ return 0;
+ }
+ return autonomous_mode_fetcher_->mode();
+ }
+
+ // True if the internal state machine thinks auto is running right now.
+ bool auto_running_ = false;
+ // True if we think that the auto *action* is running right now.
+ bool auto_action_running_ = false;
+ // True if an action was running last cycle.
+ bool was_running_ = false;
+
+ const InputConfig input_config_;
+
+ // Bool to track if auto was running the last cycle through. This lets us
+ // call AutoEnded when the auto mode function stops.
+ bool auto_was_running_ = false;
+ ::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
+ ::aos::common::actions::ActionQueue action_queue_;
+
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ dt_config_;
+
+ ::frc971::autonomous::BaseAutonomousActor::Factory autonomous_action_factory_;
+
+ ::aos::Fetcher<::frc971::autonomous::AutonomousMode> autonomous_mode_fetcher_;
+};
+
+} // namespace input
+} // namespace frc971
+
+#endif // AOS_INPUT_ACTION_JOYSTICK_INPUT_H_
diff --git a/frc971/input/driver_station_data.cc b/frc971/input/driver_station_data.cc
new file mode 100644
index 0000000..18d96fd
--- /dev/null
+++ b/frc971/input/driver_station_data.cc
@@ -0,0 +1,124 @@
+#include "frc971/input/driver_station_data.h"
+
+#include "glog/logging.h"
+
+namespace frc971 {
+namespace input {
+namespace driver_station {
+
+Data::Data() : current_values_(), old_values_() {}
+
+void Data::Update(const aos::JoystickState *new_values) {
+ old_values_ = current_values_;
+ CHECK(new_values->has_joysticks());
+ CHECK_EQ(new_values->joysticks()->size(), current_values_.joysticks.size());
+ for (size_t i = 0; i < current_values_.joysticks.size(); ++i) {
+ const aos::Joystick *joystick = new_values->joysticks()->Get(i);
+ current_values_.joysticks[i].buttons = joystick->buttons();
+ current_values_.joysticks[i].pov = joystick->pov();
+ for (size_t j = 0; j < joystick->axis()->size(); ++j) {
+ current_values_.joysticks[i].axis[j] = joystick->axis()->Get(j);
+ }
+
+ current_values_.joysticks[i].pov = joystick->pov();
+ }
+ current_values_.test_mode = new_values->test_mode();
+ current_values_.fms_attached = new_values->fms_attached();
+ current_values_.enabled = new_values->enabled();
+ current_values_.autonomous = new_values->autonomous();
+ current_values_.team_id = new_values->team_id();
+ current_values_.switch_left = new_values->switch_left();
+ current_values_.scale_left = new_values->scale_left();
+}
+
+bool Data::GetButton(const ButtonLocation location,
+ const Data::SavedJoystickState &values) {
+ if (location.joystick() < 0 ||
+ location.joystick() > static_cast<int>(values.joysticks.size())) {
+ return false;
+ }
+ if (location.number() <= 0 || location.number() > 16) {
+ return false;
+ }
+ return values.joysticks[location.joystick() - 1].buttons &
+ (1 << (location.number() - 1));
+}
+
+bool Data::DoGetPOV(const POVLocation location,
+ const Data::SavedJoystickState &values) {
+ return values.joysticks[location.joystick() - 1].pov == location.number();
+}
+
+bool Data::GetControlBitValue(const ControlBit bit,
+ const Data::SavedJoystickState &values) {
+ switch (bit) {
+ case ControlBit::kTestMode:
+ return values.test_mode;
+ case ControlBit::kFmsAttached:
+ return values.fms_attached;
+ case ControlBit::kAutonomous:
+ return values.autonomous;
+ case ControlBit::kEnabled:
+ return values.enabled;
+ default:
+ __builtin_unreachable();
+ }
+}
+
+bool Data::IsPressed(const ButtonLocation location) const {
+ return GetButton(location, current_values_);
+}
+
+bool Data::PosEdge(const ButtonLocation location) const {
+ return !GetButton(location, old_values_) &&
+ GetButton(location, current_values_);
+}
+
+bool Data::NegEdge(const ButtonLocation location) const {
+ return GetButton(location, old_values_) &&
+ !GetButton(location, current_values_);
+}
+
+bool Data::IsPressed(const POVLocation location) const {
+ return DoGetPOV(location, current_values_);
+}
+
+bool Data::PosEdge(const POVLocation location) const {
+ return !DoGetPOV(location, old_values_) &&
+ DoGetPOV(location, current_values_);
+}
+
+bool Data::NegEdge(const POVLocation location) const {
+ return DoGetPOV(location, old_values_) &&
+ !DoGetPOV(location, current_values_);
+}
+
+int32_t Data::GetPOV(int joystick) const {
+ return current_values_.joysticks[joystick - 1].pov;
+}
+
+int32_t Data::GetOldPOV(int joystick) const {
+ return old_values_.joysticks[joystick - 1].pov;
+}
+
+bool Data::GetControlBit(const ControlBit bit) const {
+ return GetControlBitValue(bit, current_values_);
+}
+
+bool Data::PosEdge(const ControlBit bit) const {
+ return !GetControlBitValue(bit, old_values_) &&
+ GetControlBitValue(bit, current_values_);
+}
+
+bool Data::NegEdge(const ControlBit bit) const {
+ return GetControlBitValue(bit, old_values_) &&
+ !GetControlBitValue(bit, current_values_);
+}
+
+float Data::GetAxis(JoystickAxis axis) const {
+ return current_values_.joysticks[axis.joystick() - 1].axis[axis.number() - 1];
+}
+
+} // namespace driver_station
+} // namespace input
+} // namespace frc971
diff --git a/frc971/input/driver_station_data.h b/frc971/input/driver_station_data.h
new file mode 100644
index 0000000..c5c38b4
--- /dev/null
+++ b/frc971/input/driver_station_data.h
@@ -0,0 +1,133 @@
+#ifndef AOS_INPUT_DRIVER_STATION_DATA_H_
+#define AOS_INPUT_DRIVER_STATION_DATA_H_
+
+// This file defines several types to support nicely looking at the data
+// received from the driver's station.
+
+#include <array>
+#include <memory>
+
+#include "frc971/input/joystick_state_generated.h"
+
+namespace frc971 {
+namespace input {
+namespace driver_station {
+
+// Represents a feature of a joystick (a button or an axis).
+// All indices are 1-based.
+class JoystickFeature {
+ public:
+ JoystickFeature(int joystick, int number)
+ : joystick_(joystick), number_(number) {}
+
+ // How many joysticks there are.
+ static const int kJoysticks = 6;
+
+ // Which joystick number this is (1-based).
+ int joystick() const { return joystick_; }
+ // Which feature on joystick() this is (1-based).
+ int number() const { return number_; }
+
+ private:
+ const int joystick_, number_;
+};
+
+// Represents the location of a button.
+// Use Data to actually get the value.
+// Safe for static initialization.
+class ButtonLocation : public JoystickFeature {
+ public:
+ ButtonLocation(int joystick, int number)
+ : JoystickFeature(joystick, number) {}
+
+ // How many buttons there are available on each joystick.
+ static const int kButtons = 16;
+};
+
+// Represents the direction of a POV on a joystick.
+// Use Data to actually get the value.
+// Safe for static initialization.
+class POVLocation : public JoystickFeature {
+ public:
+ POVLocation(int joystick, int number) : JoystickFeature(joystick, number) {}
+};
+
+// Represents various bits of control information that the DS sends.
+// Use Data to actually get the value.
+enum class ControlBit { kTestMode, kFmsAttached, kAutonomous, kEnabled };
+
+// Represents a single axis of a joystick.
+// Use Data to actually get the value.
+// Safe for static initialization.
+class JoystickAxis : public JoystickFeature {
+ public:
+ JoystickAxis(int joystick, int number) : JoystickFeature(joystick, number) {}
+
+ // How many axes there are available on each joystick.
+ static const int kAxes = 6;
+};
+
+class Data {
+ public:
+ // Initializes the data to all buttons and control bits off and all joysticks
+ // at 0.
+ Data();
+
+ // Updates the current information with a new set of values.
+ void Update(const aos::JoystickState *new_values);
+
+ bool IsPressed(POVLocation location) const;
+ bool PosEdge(POVLocation location) const;
+ bool NegEdge(POVLocation location) const;
+
+ // Returns the current and previous "values" for the POV.
+ int32_t GetPOV(int joystick) const;
+ int32_t GetOldPOV(int joystick) const;
+
+ bool IsPressed(ButtonLocation location) const;
+ bool PosEdge(ButtonLocation location) const;
+ bool NegEdge(ButtonLocation location) const;
+
+ bool GetControlBit(ControlBit bit) const;
+ bool PosEdge(ControlBit bit) const;
+ bool NegEdge(ControlBit bit) const;
+
+ // Returns the value in the range [-1.0, 1.0].
+ float GetAxis(JoystickAxis axis) const;
+
+ private:
+ struct SavedJoystickState {
+ struct SavedJoystick {
+ uint16_t buttons = 0;
+ std::array<double, JoystickAxis::kAxes> axis;
+ int pov = 0;
+ };
+
+ std::array<SavedJoystick, JoystickFeature::kJoysticks> joysticks;
+ bool test_mode = false;
+ bool fms_attached = false;
+ bool enabled = false;
+ bool autonomous = false;
+ uint16_t team_id = 0;
+
+ // 2018 scale and switch positions.
+ bool switch_left = false;
+ bool scale_left = false;
+ };
+
+ static bool GetButton(const ButtonLocation location,
+ const Data::SavedJoystickState &values);
+ static bool DoGetPOV(const POVLocation location,
+ const Data::SavedJoystickState &values);
+
+ static bool GetControlBitValue(const ControlBit bit,
+ const Data::SavedJoystickState &values);
+
+ SavedJoystickState current_values_, old_values_;
+};
+
+} // namespace driver_station
+} // namespace input
+} // namespace frc971
+
+#endif // AOS_INPUT_DRIVER_STATION_DATA_H_
diff --git a/frc971/input/drivetrain_input.cc b/frc971/input/drivetrain_input.cc
new file mode 100644
index 0000000..cefb113
--- /dev/null
+++ b/frc971/input/drivetrain_input.cc
@@ -0,0 +1,348 @@
+#include "frc971/input/drivetrain_input.h"
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <cmath>
+
+#include "aos/commonmath.h"
+#include "aos/logging/logging.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/input/driver_station_data.h"
+
+using ::frc971::input::driver_station::ButtonLocation;
+using ::frc971::input::driver_station::ControlBit;
+using ::frc971::input::driver_station::JoystickAxis;
+using ::frc971::input::driver_station::POVLocation;
+
+namespace drivetrain = frc971::control_loops::drivetrain;
+
+namespace frc971 {
+namespace input {
+
+const ButtonLocation kShiftHigh(2, 3), kShiftHigh2(2, 2), kShiftLow(2, 1);
+
+void DrivetrainInputReader::HandleDrivetrain(
+ const ::frc971::input::driver_station::Data &data) {
+ const auto wheel_and_throttle = GetWheelAndThrottle(data);
+ const double wheel = wheel_and_throttle.wheel;
+ const double wheel_velocity = wheel_and_throttle.wheel_velocity;
+ const double wheel_torque = wheel_and_throttle.wheel_torque;
+ const double throttle = wheel_and_throttle.throttle;
+ const double throttle_velocity = wheel_and_throttle.throttle_velocity;
+ const double throttle_torque = wheel_and_throttle.throttle_torque;
+ const bool high_gear = wheel_and_throttle.high_gear;
+
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
+ robot_velocity_ = drivetrain_status_fetcher_->robot_speed();
+ }
+
+ // If we have a vision align function, and it is in control, don't run the
+ // normal driving code.
+ if (vision_align_fn_) {
+ if (vision_align_fn_(data)) {
+ return;
+ }
+ }
+
+ bool is_control_loop_driving = false;
+ bool is_line_following = false;
+
+ if (data.IsPressed(turn1_)) {
+ switch (turn1_use_) {
+ case TurnButtonUse::kControlLoopDriving:
+ is_control_loop_driving = true;
+ break;
+ case TurnButtonUse::kLineFollow:
+ is_line_following = true;
+ break;
+ }
+ }
+
+ if (data.IsPressed(turn2_)) {
+ switch (turn2_use_) {
+ case TurnButtonUse::kControlLoopDriving:
+ is_control_loop_driving = true;
+ break;
+ case TurnButtonUse::kLineFollow:
+ is_line_following = true;
+ break;
+ }
+ }
+
+ if (drivetrain_status_fetcher_.get()) {
+ if (is_control_loop_driving && !last_is_control_loop_driving_) {
+ left_goal_ = drivetrain_status_fetcher_->estimated_left_position() +
+ wheel * wheel_multiplier_;
+ right_goal_ = drivetrain_status_fetcher_->estimated_right_position() -
+ wheel * wheel_multiplier_;
+ }
+ }
+
+ 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;
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ frc971::ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<frc971::ProfileParameters>();
+
+ linear_builder.add_max_velocity(3.0);
+ linear_builder.add_max_acceleration(20.0);
+
+ flatbuffers::Offset<frc971::ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ auto goal_builder = builder.MakeBuilder<drivetrain::Goal>();
+ goal_builder.add_wheel(wheel);
+ goal_builder.add_wheel_velocity(wheel_velocity);
+ goal_builder.add_wheel_torque(wheel_torque);
+ goal_builder.add_throttle(throttle);
+ goal_builder.add_throttle_velocity(throttle_velocity);
+ goal_builder.add_throttle_torque(throttle_torque);
+ goal_builder.add_highgear(high_gear);
+ goal_builder.add_quickturn(data.IsPressed(quick_turn_));
+ goal_builder.add_controller_type(
+ is_line_following ? drivetrain::ControllerType::LINE_FOLLOWER
+ : (is_control_loop_driving
+ ? drivetrain::ControllerType::MOTION_PROFILE
+ : drivetrain::ControllerType::POLYDRIVE));
+ goal_builder.add_left_goal(current_left_goal);
+ goal_builder.add_right_goal(current_right_goal);
+ goal_builder.add_linear(linear_offset);
+
+ if (!builder.Send(goal_builder.Finish())) {
+ AOS_LOG(WARNING, "sending stick values failed\n");
+ }
+
+ last_is_control_loop_driving_ = is_control_loop_driving;
+}
+
+DrivetrainInputReader::WheelAndThrottle
+SteeringWheelDrivetrainInputReader::GetWheelAndThrottle(
+ const ::frc971::input::driver_station::Data &data) {
+ const double wheel = -data.GetAxis(wheel_);
+ const double throttle = -data.GetAxis(throttle_);
+
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ high_gear_ = default_high_gear_;
+ }
+
+ if (data.PosEdge(kShiftLow)) {
+ high_gear_ = false;
+ }
+
+ if (data.PosEdge(kShiftHigh) || data.PosEdge(kShiftHigh2)) {
+ high_gear_ = true;
+ }
+
+ return DrivetrainInputReader::WheelAndThrottle{
+ wheel, 0.0, 0.0, throttle, 0.0, 0.0, high_gear_};
+}
+
+double UnwrappedAxis(const ::frc971::input::driver_station::Data &data,
+ const JoystickAxis &high_bits,
+ const JoystickAxis &low_bits) {
+ const float high_bits_data = data.GetAxis(high_bits);
+ const float low_bits_data = data.GetAxis(low_bits);
+ const int16_t high_bits_data_int =
+ (high_bits_data < 0.0f ? high_bits_data * 128.0f
+ : high_bits_data * 127.0f);
+ const int16_t low_bits_data_int =
+ (low_bits_data < 0.0f ? low_bits_data * 128.0f : low_bits_data * 127.0f);
+
+ const uint16_t high_bits_data_uint =
+ ((static_cast<uint16_t>(high_bits_data_int) & 0xff) + 0x80) & 0xff;
+ const uint16_t low_bits_data_uint =
+ ((static_cast<uint16_t>(low_bits_data_int) & 0xff) + 0x80) & 0xff;
+
+ const uint16_t data_uint = (high_bits_data_uint << 8) | low_bits_data_uint;
+
+ const int32_t data_int = static_cast<int32_t>(data_uint) - 0x8000;
+
+ if (data_int < 0) {
+ return static_cast<double>(data_int) / static_cast<double>(0x8000);
+ } else {
+ return static_cast<double>(data_int) / static_cast<double>(0x7fff);
+ }
+}
+
+DrivetrainInputReader::WheelAndThrottle
+PistolDrivetrainInputReader::GetWheelAndThrottle(
+ const ::frc971::input::driver_station::Data &data) {
+ const double wheel = -UnwrappedAxis(data, wheel_, wheel_low_);
+ const double wheel_velocity =
+ -UnwrappedAxis(data, wheel_velocity_high_, wheel_velocity_low_) * 50.0;
+ const double wheel_torque =
+ -UnwrappedAxis(data, wheel_torque_high_, wheel_torque_low_) / 2.0;
+
+ double throttle = UnwrappedAxis(data, throttle_, throttle_low_);
+ const double throttle_velocity =
+ UnwrappedAxis(data, throttle_velocity_high_, throttle_velocity_low_) *
+ 50.0;
+ const double throttle_torque =
+ UnwrappedAxis(data, throttle_torque_high_, throttle_torque_low_) / 2.0;
+
+ // TODO(austin): Deal with haptics here.
+ if (throttle < 0) {
+ throttle = ::std::max(-1.0, throttle / 0.7);
+ }
+
+ if (data.IsPressed(slow_down_)) {
+ throttle *= 0.5;
+ }
+
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ high_gear_ = default_high_gear_;
+ }
+
+ if (data.PosEdge(shift_low_)) {
+ high_gear_ = false;
+ }
+
+ if (data.PosEdge(shift_high_)) {
+ high_gear_ = true;
+ }
+
+ return DrivetrainInputReader::WheelAndThrottle{
+ wheel, wheel_velocity, wheel_torque,
+ throttle, throttle_velocity, throttle_torque,
+ high_gear_};
+}
+
+DrivetrainInputReader::WheelAndThrottle
+XboxDrivetrainInputReader::GetWheelAndThrottle(
+ const ::frc971::input::driver_station::Data &data) {
+ // xbox
+ constexpr double kWheelDeadband = 0.05;
+ constexpr double kThrottleDeadband = 0.05;
+ const double wheel =
+ aos::Deadband(-data.GetAxis(wheel_), kWheelDeadband, 1.0);
+
+ const double unmodified_throttle =
+ aos::Deadband(-data.GetAxis(throttle_), 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, 0.0, 0.0, throttle,
+ 0.0, 0.0, true};
+}
+
+std::unique_ptr<SteeringWheelDrivetrainInputReader>
+SteeringWheelDrivetrainInputReader::Make(::aos::EventLoop *event_loop,
+ bool default_high_gear) {
+ 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(
+ event_loop, kSteeringWheel, kDriveThrottle, kQuickTurn, kTurn1,
+ TurnButtonUse::kControlLoopDriving, kTurn2,
+ TurnButtonUse::kControlLoopDriving));
+ result.get()->set_default_high_gear(default_high_gear);
+
+ return result;
+}
+
+std::unique_ptr<PistolDrivetrainInputReader> PistolDrivetrainInputReader::Make(
+ ::aos::EventLoop *event_loop, bool default_high_gear,
+ TopButtonUse top_button_use) {
+ // Pistol Grip controller
+ const JoystickAxis kTriggerHigh(1, 1), kTriggerLow(1, 4),
+ kTriggerVelocityHigh(1, 2), kTriggerVelocityLow(1, 5),
+ kTriggerTorqueHigh(1, 3), kTriggerTorqueLow(1, 6);
+
+ const JoystickAxis kWheelHigh(2, 1), kWheelLow(2, 4),
+ kWheelVelocityHigh(2, 2), kWheelVelocityLow(2, 5), kWheelTorqueHigh(2, 3),
+ kWheelTorqueLow(2, 6);
+
+ const ButtonLocation kQuickTurn(1, 3);
+
+ const ButtonLocation TopButton(1, 1);
+ const ButtonLocation SecondButton(1, 2);
+ const ButtonLocation BottomButton(1, 4);
+ // Non-existant button for nops.
+ const ButtonLocation DummyButton(1, 10);
+
+ // TODO(james): Make a copy assignment operator for ButtonLocation so we don't
+ // have to shoehorn in these ternary operators.
+ const ButtonLocation kTurn1 = (top_button_use == TopButtonUse::kLineFollow)
+ ? SecondButton
+ : DummyButton;
+ // Turn2 does closed loop driving.
+ const ButtonLocation kTurn2 =
+ (top_button_use == TopButtonUse::kLineFollow) ? TopButton : DummyButton;
+
+ const ButtonLocation kShiftHigh =
+ (top_button_use == TopButtonUse::kShift) ? TopButton : DummyButton;
+ const ButtonLocation kShiftLow =
+ (top_button_use == TopButtonUse::kShift) ? SecondButton : DummyButton;
+
+ std::unique_ptr<PistolDrivetrainInputReader> result(
+ new PistolDrivetrainInputReader(
+ event_loop, kWheelHigh, kWheelLow, kTriggerVelocityHigh,
+ kTriggerVelocityLow, kTriggerTorqueHigh, kTriggerTorqueLow,
+ kTriggerHigh, kTriggerLow, kWheelVelocityHigh, kWheelVelocityLow,
+ kWheelTorqueHigh, kWheelTorqueLow, kQuickTurn, kShiftHigh, kShiftLow,
+ kTurn1, kTurn2, BottomButton));
+
+ result->set_default_high_gear(default_high_gear);
+ return result;
+}
+
+std::unique_ptr<XboxDrivetrainInputReader> XboxDrivetrainInputReader::Make(
+ ::aos::EventLoop *event_loop) {
+ // 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(event_loop, kSteeringWheel, kDriveThrottle,
+ kQuickTurn, kTurn1,
+ TurnButtonUse::kControlLoopDriving, kTurn2,
+ TurnButtonUse::kControlLoopDriving));
+ return result;
+}
+::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
+ ::aos::EventLoop *event_loop, InputType type,
+ const drivetrain::DrivetrainConfig<double> &dt_config) {
+ std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader;
+
+ using InputType = DrivetrainInputReader::InputType;
+
+ switch (type) {
+ case InputType::kSteeringWheel:
+ drivetrain_input_reader = SteeringWheelDrivetrainInputReader::Make(
+ event_loop, dt_config.default_high_gear);
+ break;
+ case InputType::kPistol:
+ drivetrain_input_reader = PistolDrivetrainInputReader::Make(
+ event_loop, dt_config.default_high_gear,
+ dt_config.pistol_grip_shift_enables_line_follow
+ ? PistolDrivetrainInputReader::TopButtonUse::kLineFollow
+ : PistolDrivetrainInputReader::TopButtonUse::kShift);
+ break;
+ case InputType::kXbox:
+ drivetrain_input_reader = XboxDrivetrainInputReader::Make(event_loop);
+ break;
+ }
+ return drivetrain_input_reader;
+}
+
+} // namespace input
+} // namespace frc971
diff --git a/frc971/input/drivetrain_input.h b/frc971/input/drivetrain_input.h
new file mode 100644
index 0000000..9e450d5
--- /dev/null
+++ b/frc971/input/drivetrain_input.h
@@ -0,0 +1,276 @@
+#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/logging/logging.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/input/driver_station_data.h"
+
+namespace frc971 {
+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:
+ // What to use the turn1/2 buttons for.
+ enum class TurnButtonUse {
+ // Use the button to enable control loop driving.
+ kControlLoopDriving,
+ // Use the button to set line following mode.
+ kLineFollow,
+ };
+ // Inputs driver station button and joystick locations
+ DrivetrainInputReader(::aos::EventLoop *event_loop,
+ driver_station::JoystickAxis wheel,
+ driver_station::JoystickAxis throttle,
+ driver_station::ButtonLocation quick_turn,
+ driver_station::ButtonLocation turn1,
+ TurnButtonUse turn1_use,
+ driver_station::ButtonLocation turn2,
+ TurnButtonUse turn2_use)
+ : wheel_(wheel),
+ throttle_(throttle),
+ quick_turn_(quick_turn),
+ turn1_(turn1),
+ turn1_use_(turn1_use),
+ turn2_(turn2),
+ turn2_use_(turn2_use),
+ drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ drivetrain_goal_sender_(
+ event_loop->MakeSender<::frc971::control_loops::drivetrain::Goal>(
+ "/drivetrain")) {}
+
+ virtual ~DrivetrainInputReader() = default;
+
+ // List of driver joystick types.
+ enum class InputType {
+ kSteeringWheel,
+ kPistol,
+ kXbox,
+ };
+
+ // Constructs the appropriate DrivetrainInputReader.
+ static std::unique_ptr<DrivetrainInputReader> Make(
+ ::aos::EventLoop *event_loop, InputType type,
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config);
+
+ // Processes new joystick data and publishes drivetrain goal messages.
+ void HandleDrivetrain(const ::frc971::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_; }
+
+ void set_vision_align_fn(
+ ::std::function<bool(const ::frc971::input::driver_station::Data &data)>
+ vision_align_fn) {
+ vision_align_fn_ = vision_align_fn;
+ }
+
+ protected:
+ const driver_station::JoystickAxis wheel_;
+ const driver_station::JoystickAxis throttle_;
+ const driver_station::ButtonLocation quick_turn_;
+ // Button for enabling control loop driving.
+ const driver_station::ButtonLocation turn1_;
+ const TurnButtonUse turn1_use_;
+ // But for enabling line following.
+ const driver_station::ButtonLocation turn2_;
+ const TurnButtonUse turn2_use_;
+
+ bool last_is_control_loop_driving_ = false;
+
+ // Structure containing the (potentially adjusted) steering and throttle
+ // values from the joysticks.
+ struct WheelAndThrottle {
+ double wheel;
+ double wheel_velocity;
+ double wheel_torque;
+ double throttle;
+ double throttle_velocity;
+ double throttle_torque;
+ bool high_gear;
+ };
+
+ private:
+ // Computes the steering and throttle from the provided driverstation data.
+ virtual WheelAndThrottle GetWheelAndThrottle(
+ const ::frc971::input::driver_station::Data &data) = 0;
+
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
+ drivetrain_goal_sender_;
+
+ 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;
+
+ ::std::function<bool(const ::frc971::input::driver_station::Data &data)>
+ vision_align_fn_;
+};
+
+// 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(
+ ::aos::EventLoop *event_loop, bool default_high_gear);
+
+ // Sets the default shifter position
+ void set_default_high_gear(bool default_high_gear) {
+ high_gear_ = default_high_gear;
+ default_high_gear_ = default_high_gear;
+ }
+
+ private:
+ WheelAndThrottle GetWheelAndThrottle(
+ const ::frc971::input::driver_station::Data &data) override;
+ bool high_gear_;
+ bool default_high_gear_;
+};
+
+class PistolDrivetrainInputReader : public DrivetrainInputReader {
+ public:
+ using DrivetrainInputReader::DrivetrainInputReader;
+
+ // What to use the top two buttons for on the pistol grip.
+ enum class TopButtonUse {
+ // Normal shifting.
+ kShift,
+ // Line following (currently just uses top button).
+ kLineFollow,
+ };
+
+ // Creates a DrivetrainInputReader with the corresponding joystick ports and
+ // axis for the (cheap) pistol grip controller.
+ static std::unique_ptr<PistolDrivetrainInputReader> Make(
+ ::aos::EventLoop *event_loop, bool default_high_gear,
+ TopButtonUse top_button_use);
+
+ private:
+ PistolDrivetrainInputReader(
+ ::aos::EventLoop *event_loop, driver_station::JoystickAxis wheel_high,
+ driver_station::JoystickAxis wheel_low,
+ driver_station::JoystickAxis wheel_velocity_high,
+ driver_station::JoystickAxis wheel_velocity_low,
+ driver_station::JoystickAxis wheel_torque_high,
+ driver_station::JoystickAxis wheel_torque_low,
+ driver_station::JoystickAxis throttle_high,
+ driver_station::JoystickAxis throttle_low,
+ driver_station::JoystickAxis throttle_velocity_high,
+ driver_station::JoystickAxis throttle_velocity_low,
+ driver_station::JoystickAxis throttle_torque_high,
+ driver_station::JoystickAxis throttle_torque_low,
+ driver_station::ButtonLocation quick_turn,
+ driver_station::ButtonLocation shift_high,
+ driver_station::ButtonLocation shift_low,
+ driver_station::ButtonLocation turn1,
+ driver_station::ButtonLocation turn2,
+ driver_station::ButtonLocation slow_down)
+ : DrivetrainInputReader(event_loop, wheel_high, throttle_high, quick_turn,
+ turn1, TurnButtonUse::kLineFollow, turn2,
+ TurnButtonUse::kControlLoopDriving),
+ wheel_low_(wheel_low),
+ wheel_velocity_high_(wheel_velocity_high),
+ wheel_velocity_low_(wheel_velocity_low),
+ wheel_torque_high_(wheel_torque_high),
+ wheel_torque_low_(wheel_torque_low),
+ throttle_low_(throttle_low),
+ throttle_velocity_high_(throttle_velocity_high),
+ throttle_velocity_low_(throttle_velocity_low),
+ throttle_torque_high_(throttle_torque_high),
+ throttle_torque_low_(throttle_torque_low),
+ shift_high_(shift_high),
+ shift_low_(shift_low),
+ slow_down_(slow_down) {}
+
+ WheelAndThrottle GetWheelAndThrottle(
+ const ::frc971::input::driver_station::Data &data) override;
+
+ // Sets the default shifter position
+ void set_default_high_gear(bool default_high_gear) {
+ high_gear_ = default_high_gear;
+ default_high_gear_ = default_high_gear;
+ }
+
+ const driver_station::JoystickAxis wheel_low_;
+ const driver_station::JoystickAxis wheel_velocity_high_;
+ const driver_station::JoystickAxis wheel_velocity_low_;
+ const driver_station::JoystickAxis wheel_torque_high_;
+ const driver_station::JoystickAxis wheel_torque_low_;
+
+ const driver_station::JoystickAxis throttle_low_;
+ const driver_station::JoystickAxis throttle_velocity_high_;
+ const driver_station::JoystickAxis throttle_velocity_low_;
+ const driver_station::JoystickAxis throttle_torque_high_;
+ const driver_station::JoystickAxis throttle_torque_low_;
+
+ const driver_station::ButtonLocation shift_high_;
+ const driver_station::ButtonLocation shift_low_;
+ const driver_station::ButtonLocation slow_down_;
+
+ bool high_gear_;
+ bool default_high_gear_;
+};
+
+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(
+ ::aos::EventLoop *event_loop);
+
+ private:
+ WheelAndThrottle GetWheelAndThrottle(
+ const ::frc971::input::driver_station::Data &data) override;
+};
+
+} // namespace input
+} // namespace frc971
+
+#endif // AOS_INPUT_DRIVETRAIN_INPUT_H_
diff --git a/frc971/input/joystick_input.cc b/frc971/input/joystick_input.cc
new file mode 100644
index 0000000..c92d567
--- /dev/null
+++ b/frc971/input/joystick_input.cc
@@ -0,0 +1,65 @@
+#include "frc971/input/joystick_input.h"
+
+#include <string.h>
+
+#include <atomic>
+
+#include "aos/logging/logging.h"
+#include "frc971/input/robot_state_generated.h"
+
+namespace frc971 {
+namespace input {
+
+void JoystickInput::HandleData(const ::aos::JoystickState *joystick_state) {
+ data_.Update(joystick_state);
+
+ mode_ = static_cast<int>(joystick_state->switch_left()) |
+ (static_cast<int>(joystick_state->scale_left()) << 1);
+
+ {
+ using driver_station::ButtonLocation;
+ using driver_station::JoystickFeature;
+ for (int joystick = 1; joystick <= JoystickFeature::kJoysticks;
+ ++joystick) {
+ for (int button = 1; button <= ButtonLocation::kButtons; ++button) {
+ ButtonLocation location(joystick, button);
+ if (data_.PosEdge(location)) {
+ AOS_LOG(INFO, "PosEdge(%d, %d)\n", joystick, button);
+ }
+ if (data_.NegEdge(location)) {
+ AOS_LOG(INFO, "NegEdge(%d, %d)\n", joystick, button);
+ }
+ }
+ if (data_.GetPOV(joystick) != data_.GetOldPOV(joystick)) {
+ AOS_LOG(INFO, "POV %d %d->%d\n", joystick, data_.GetOldPOV(joystick),
+ data_.GetPOV(joystick));
+ }
+ }
+ }
+ {
+ using driver_station::ControlBit;
+ if (data_.PosEdge(ControlBit::kFmsAttached)) {
+ AOS_LOG(INFO, "PosEdge(kFmsAttached)\n");
+ }
+ if (data_.NegEdge(ControlBit::kFmsAttached)) {
+ AOS_LOG(INFO, "NegEdge(kFmsAttached)\n");
+ }
+ if (data_.PosEdge(ControlBit::kAutonomous)) {
+ AOS_LOG(INFO, "PosEdge(kAutonomous)\n");
+ }
+ if (data_.NegEdge(ControlBit::kAutonomous)) {
+ AOS_LOG(INFO, "NegEdge(kAutonomous)\n");
+ }
+ if (data_.PosEdge(ControlBit::kEnabled)) {
+ AOS_LOG(INFO, "PosEdge(kEnabled)\n");
+ }
+ if (data_.NegEdge(ControlBit::kEnabled)) {
+ AOS_LOG(INFO, "NegEdge(kEnabled)\n");
+ }
+ }
+
+ RunIteration(data_);
+}
+
+} // namespace input
+} // namespace frc971
diff --git a/frc971/input/joystick_input.h b/frc971/input/joystick_input.h
new file mode 100644
index 0000000..074908d
--- /dev/null
+++ b/frc971/input/joystick_input.h
@@ -0,0 +1,46 @@
+#ifndef AOS_INPUT_JOYSTICK_INPUT_H_
+#define AOS_INPUT_JOYSTICK_INPUT_H_
+
+#include <atomic>
+
+#include "aos/events/event_loop.h"
+#include "frc971/input/driver_station_data.h"
+
+namespace frc971 {
+namespace input {
+
+// A class for handling joystick packet values.
+// It will call RunIteration each time a new packet is received.
+//
+// This class automatically handles updating ::aos::joystick_state and logging
+// (at INFO) button edges.
+class JoystickInput {
+ public:
+ explicit JoystickInput(::aos::EventLoop *event_loop)
+ : event_loop_(event_loop) {
+ event_loop_->MakeWatcher(
+ "/aos", [this](const ::aos::JoystickState &joystick_state) {
+ this->HandleData(&joystick_state);
+ });
+ event_loop->SetRuntimeRealtimePriority(28);
+ }
+
+ protected:
+ int mode() const { return mode_; }
+
+ private:
+ void HandleData(const ::aos::JoystickState *joystick_state);
+
+ // Subclasses should do whatever they want with data here.
+ virtual void RunIteration(const driver_station::Data &data) = 0;
+
+ ::aos::EventLoop *event_loop_;
+ driver_station::Data data_;
+
+ int mode_;
+};
+
+} // namespace input
+} // namespace frc971
+
+#endif // AOS_INPUT_JOYSTICK_INPUT_H_
diff --git a/frc971/input/joystick_state.fbs b/frc971/input/joystick_state.fbs
new file mode 100644
index 0000000..2f48bdf
--- /dev/null
+++ b/frc971/input/joystick_state.fbs
@@ -0,0 +1,49 @@
+namespace aos;
+
+table Joystick {
+ // A bitmask of the butotn state.
+ buttons:ushort (id: 0);
+
+ // The 6 joystick axes.
+ // TODO: Should have size of 6
+ axis:[double] (id: 1);
+
+ // The POV axis.
+ pov:int (id: 2);
+}
+
+enum Alliance : byte { kRed, kBlue, kInvalid }
+
+// This message is checked by all control loops to make sure that the
+// joystick code hasn't died. It is published on "/aos"
+table JoystickState {
+ //TODO: should have fixed size.
+ joysticks:[Joystick] (id: 0);
+
+ test_mode:bool (id: 1);
+ fms_attached:bool (id: 2);
+ enabled:bool (id: 3);
+ autonomous:bool (id: 4);
+ team_id:ushort (id: 5);
+
+ // 2018 scale and switch positions.
+ // TODO(austin): Push these out to a new message?
+ switch_left:bool (id: 6);
+ scale_left:bool (id: 7);
+
+ // If this is true, then this message isn't actually from the control
+ // system and so should not be trusted as evidence that the button inputs
+ // etc are actually real and should be acted on.
+ // However, most things should ignore this so that sending fake messages is
+ // useful for testing. The only difference in behavior should be motors not
+ // actually turning on.
+ fake:bool (id: 8);
+
+ // Color of our current alliance.
+ alliance:Alliance (id: 9);
+
+ // String corresponding to the game data string
+ game_data:string (id: 10);
+}
+
+root_type JoystickState;
diff --git a/frc971/input/robot_state.fbs b/frc971/input/robot_state.fbs
new file mode 100644
index 0000000..7d0199b
--- /dev/null
+++ b/frc971/input/robot_state.fbs
@@ -0,0 +1,35 @@
+namespace aos;
+
+// This message is sent out on this queue when sensors are read. It contains
+// global robot state and information about whether the process reading sensors
+// has been restarted, along with all counters etc it keeps track of. It is
+// published on "/aos"
+table RobotState {
+ // The PID of the process reading sensors.
+ // This is here so control loops can tell when it changes.
+ reader_pid:int (id: 0);
+
+ // True when outputs are enabled.
+ // Motor controllers keep going for a bit after this goes to false.
+ outputs_enabled:bool (id: 1);
+ // Indicates whether something is browned out (I think motor controller
+ // outputs). IMPORTANT: This is NOT !outputs_enabled. outputs_enabled goes to
+ // false for other reasons too (disabled, e-stopped, maybe more).
+ browned_out:bool (id: 2);
+
+ // Whether the two sensor rails are currently working.
+ is_3v3_active:bool (id: 3);
+ is_5v_active:bool (id: 4);
+ // The current voltages measured on the two sensor rails.
+ voltage_3v3:double (id: 5);
+ voltage_5v:double (id: 6);
+
+ // The input voltage to the roboRIO.
+ voltage_roborio_in:double (id: 7);
+
+ // From the DriverStation object, aka what FMS sees and what shows up on the
+ // actual driver's station.
+ voltage_battery:double (id: 8);
+}
+
+root_type RobotState;
diff --git a/frc971/input/robot_state_config.json b/frc971/input/robot_state_config.json
new file mode 100644
index 0000000..781dd6c
--- /dev/null
+++ b/frc971/input/robot_state_config.json
@@ -0,0 +1,18 @@
+{
+ "channels":
+ [
+ {
+ "name": "/aos",
+ "type": "aos.JoystickState",
+ "frequency": 75
+ },
+ {
+ "name": "/aos",
+ "type": "aos.RobotState",
+ "frequency": 200
+ }
+ ],
+ "imports": [
+ "../../../org_frc971/aos/events/aos.json"
+ ]
+}
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 00896bb..4f8bca0 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -115,7 +115,7 @@
"//aos/events:event_loop",
"//aos/events:shm_event_loop",
"//aos/logging",
- "//aos/robot_state:robot_state_fbs",
+ "//frc971/input:robot_state_fbs",
"//aos/time",
"//aos/util:phased_loop",
"//frc971/queues:gyro_fbs",
@@ -167,7 +167,7 @@
deps = [
"//aos:init",
"//aos/events:event_loop",
- "//aos/robot_state:robot_state_fbs",
+ "//frc971/input:robot_state_fbs",
"//aos/scoped:scoped_fd",
"//aos/time",
"//aos/util:log_interval",
@@ -204,9 +204,9 @@
deps = [
"//aos:init",
"//aos/events:shm_event_loop",
- "//aos/input:driver_station_data",
+ "//frc971/input:driver_station_data",
"//aos/network:team_number",
- "//aos/robot_state:joystick_state_fbs",
+ "//frc971/input:joystick_state_fbs",
"//third_party:wpilib",
],
)
@@ -222,7 +222,7 @@
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
"//aos/events:event_loop",
- "//aos/robot_state:robot_state_fbs",
+ "//frc971/input:robot_state_fbs",
"//third_party:wpilib",
],
)
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 2bdcc76..3061c3b 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -10,9 +10,8 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
-#include "aos/robot_state/robot_state_generated.h"
#include "aos/time/time.h"
-
+#include "frc971/input/robot_state_generated.h"
#include "frc971/queues/gyro_generated.h"
#include "frc971/queues/gyro_uid_generated.h"
#include "frc971/zeroing/averager.h"
@@ -31,9 +30,9 @@
gyro_reading_sender_(
event_loop_->MakeSender<frc971::sensors::GyroReading>(
"/drivetrain")) {
- AOS_PCHECK(
- system("busybox ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
- "33") == 0);
+ AOS_PCHECK(system("busybox ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | "
+ "xargs chrt -f -p "
+ "33") == 0);
event_loop->set_name("Gyro");
event_loop_->SetRuntimeRealtimePriority(33);
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
index a0048fb..91257c9 100644
--- a/frc971/wpilib/gyro_sender.h
+++ b/frc971/wpilib/gyro_sender.h
@@ -7,7 +7,7 @@
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
-#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/input/robot_state_generated.h"
#include "frc971/queues/gyro_generated.h"
#include "frc971/queues/gyro_uid_generated.h"
#include "frc971/wpilib/gyro_interface.h"
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index a4634bf..5661933 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -1,11 +1,10 @@
#include "frc971/wpilib/joystick_sender.h"
-#include "aos/input/driver_station_data.h"
#include "aos/logging/logging.h"
#include "aos/network/team_number.h"
#include "aos/realtime.h"
-#include "aos/robot_state/joystick_state_generated.h"
-
+#include "frc971/input/driver_station_data.h"
+#include "frc971/input/joystick_state_generated.h"
#include "frc971/wpilib/ahal/DriverStation.h"
#include "hal/HAL.h"
@@ -35,15 +34,16 @@
auto status = HAL_GetMatchInfo(&match_info);
std::array<flatbuffers::Offset<Joystick>,
- aos::input::driver_station::JoystickFeature::kJoysticks>
+ frc971::input::driver_station::JoystickFeature::kJoysticks>
joysticks;
for (size_t i = 0;
- i < aos::input::driver_station::JoystickFeature::kJoysticks; ++i) {
- std::array<double, aos::input::driver_station::JoystickAxis::kAxes>
+ i < frc971::input::driver_station::JoystickFeature::kJoysticks;
+ ++i) {
+ std::array<double, frc971::input::driver_station::JoystickAxis::kAxes>
axis;
- for (int j = 0; j < aos::input::driver_station::JoystickAxis::kAxes;
- ++j) {
+ for (int j = 0;
+ j < frc971::input::driver_station::JoystickAxis::kAxes; ++j) {
axis[j] = ds->GetStickAxis(i, j);
}
diff --git a/frc971/wpilib/joystick_sender.h b/frc971/wpilib/joystick_sender.h
index 6622d85..c9eb4cd 100644
--- a/frc971/wpilib/joystick_sender.h
+++ b/frc971/wpilib/joystick_sender.h
@@ -4,7 +4,7 @@
#include <atomic>
#include "aos/events/shm_event_loop.h"
-#include "aos/robot_state/joystick_state_generated.h"
+#include "frc971/input/joystick_state_generated.h"
namespace frc971 {
namespace wpilib {
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 39b83fa..8efdf75 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -6,10 +6,10 @@
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
-#include "aos/robot_state/robot_state_generated.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/input/robot_state_generated.h"
#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/DriverStation.h"
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 8772af1..2a59019 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -2,8 +2,7 @@
#include "aos/events/event_loop.h"
#include "aos/logging/logging.h"
-#include "aos/robot_state/robot_state_generated.h"
-
+#include "frc971/input/robot_state_generated.h"
#include "hal/HAL.h"
namespace frc971 {
diff --git a/frc971/wpilib/wpilib_interface.h b/frc971/wpilib/wpilib_interface.h
index 104ab84..8ab92ef 100644
--- a/frc971/wpilib/wpilib_interface.h
+++ b/frc971/wpilib/wpilib_interface.h
@@ -4,7 +4,7 @@
#include <stdint.h>
#include "aos/events/event_loop.h"
-#include "aos/robot_state/robot_state_generated.h"
+#include "frc971/input/robot_state_generated.h"
namespace frc971 {
namespace wpilib {