Add logic to read out of order joysticks
If joysticks get out of order on the driverstation, we won't
get the wrong inputs.
The id is encoded as a two bit number in buttons 14 and 15.
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I569ad6a991c4fda1596aba85b4b697da9c2ecab9
diff --git a/frc971/input/BUILD b/frc971/input/BUILD
index ddb1f43..8515153 100644
--- a/frc971/input/BUILD
+++ b/frc971/input/BUILD
@@ -58,12 +58,28 @@
)
cc_library(
+ name = "redundant_joystick_data",
+ srcs = [
+ "redundant_joystick_data.cc",
+ ],
+ hdrs = [
+ "redundant_joystick_data.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":driver_station_data",
+ "//aos/events:event_loop",
+ ],
+)
+
+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",
+ ":redundant_joystick_data",
"//aos:init",
"//aos/actions:action_lib",
"//aos/logging",
diff --git a/frc971/input/action_joystick_input.cc b/frc971/input/action_joystick_input.cc
index 4f1d655..dfd229e 100644
--- a/frc971/input/action_joystick_input.cc
+++ b/frc971/input/action_joystick_input.cc
@@ -4,6 +4,7 @@
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/input/driver_station_data.h"
+#include "frc971/input/redundant_joystick_data.h"
using ::frc971::input::driver_station::ControlBit;
@@ -11,6 +12,16 @@
namespace input {
void ActionJoystickInput::RunIteration(
+ const ::frc971::input::driver_station::Data &unsorted_data) {
+ if (input_config_.use_redundant_joysticks) {
+ driver_station::RedundantData redundant_data_storage(unsorted_data);
+ DoRunIteration(redundant_data_storage);
+ } else {
+ DoRunIteration(unsorted_data);
+ }
+}
+
+void ActionJoystickInput::DoRunIteration(
const ::frc971::input::driver_station::Data &data) {
const bool last_auto_running = auto_running_;
auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
diff --git a/frc971/input/action_joystick_input.h b/frc971/input/action_joystick_input.h
index 3284b15..f77f416 100644
--- a/frc971/input/action_joystick_input.h
+++ b/frc971/input/action_joystick_input.h
@@ -25,6 +25,11 @@
// 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};
+
+ // Use button 14 and 15 to encode the id of the joystick and remap the
+ // joysticks so that their ids are independent of their order on the
+ // driverstation.
+ bool use_redundant_joysticks = false;
};
ActionJoystickInput(
::aos::EventLoop *event_loop,
@@ -95,6 +100,8 @@
void RunIteration(const ::frc971::input::driver_station::Data &data) override;
+ void DoRunIteration(const ::frc971::input::driver_station::Data &data);
+
void StartAuto();
void StopAuto();
diff --git a/frc971/input/driver_station_data.h b/frc971/input/driver_station_data.h
index c5c38b4..5ebf8f4 100644
--- a/frc971/input/driver_station_data.h
+++ b/frc971/input/driver_station_data.h
@@ -76,24 +76,24 @@
// 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;
+ virtual bool IsPressed(POVLocation location) const;
+ virtual bool PosEdge(POVLocation location) const;
+ virtual 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;
+ virtual int32_t GetPOV(int joystick) const;
+ virtual int32_t GetOldPOV(int joystick) const;
- bool IsPressed(ButtonLocation location) const;
- bool PosEdge(ButtonLocation location) const;
- bool NegEdge(ButtonLocation location) const;
+ virtual bool IsPressed(ButtonLocation location) const;
+ virtual bool PosEdge(ButtonLocation location) const;
+ virtual bool NegEdge(ButtonLocation location) const;
- bool GetControlBit(ControlBit bit) const;
- bool PosEdge(ControlBit bit) const;
- bool NegEdge(ControlBit bit) const;
+ virtual bool GetControlBit(ControlBit bit) const;
+ virtual bool PosEdge(ControlBit bit) const;
+ virtual bool NegEdge(ControlBit bit) const;
// Returns the value in the range [-1.0, 1.0].
- float GetAxis(JoystickAxis axis) const;
+ virtual float GetAxis(JoystickAxis axis) const;
private:
struct SavedJoystickState {
diff --git a/frc971/input/drivetrain_input.cc b/frc971/input/drivetrain_input.cc
index 239eddf..5aedd0e 100644
--- a/frc971/input/drivetrain_input.cc
+++ b/frc971/input/drivetrain_input.cc
@@ -280,7 +280,7 @@
const ButtonLocation kSecondButton(1, 2);
const ButtonLocation kBottomButton(1, 4);
// Non-existant button for nops.
- const ButtonLocation kDummyButton(1, 10);
+ const ButtonLocation kDummyButton(1, 15);
// TODO(james): Make a copy assignment operator for ButtonLocation so we don't
// have to shoehorn in these ternary operators.
diff --git a/frc971/input/redundant_joystick_data.cc b/frc971/input/redundant_joystick_data.cc
new file mode 100644
index 0000000..3274856
--- /dev/null
+++ b/frc971/input/redundant_joystick_data.cc
@@ -0,0 +1,100 @@
+#include "frc971/input/redundant_joystick_data.h"
+
+#include "aos/logging/logging.h"
+
+namespace frc971 {
+namespace input {
+namespace driver_station {
+
+RedundantData::RedundantData(const Data &data) : joystick_map_(), data_(data) {
+ // Start with a naive map.
+ for (int i = 0; i < JoystickFeature::kJoysticks; i++) {
+ joystick_map_.at(i) = i;
+ }
+
+ for (int i = 0; i < JoystickFeature::kJoysticks; i++) {
+ ButtonLocation id_bit0_location(i + 1, kIdBit0Button);
+ ButtonLocation id_bit1_location(i + 1, kIdBit1Button);
+ ButtonLocation redundant_bit_location(i + 1, kRedundantBitButton);
+
+ int id_bit0 = data_.IsPressed(id_bit0_location);
+ int id_bit1 = data_.IsPressed(id_bit1_location);
+ int is_redundant = data_.IsPressed(redundant_bit_location);
+
+ // We don't care if this is the redundant or primary one. Pick the second
+ // one.
+ (void)is_redundant;
+
+ int packed_joystick_number = (id_bit1 << 1u) | (id_bit0 << 0u);
+
+ joystick_map_.at(packed_joystick_number) = i + 1;
+ }
+};
+
+int RedundantData::MapRedundantJoystick(int joystick) const {
+ return joystick_map_.at(joystick);
+}
+
+bool RedundantData::IsPressed(POVLocation location) const {
+ POVLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.IsPressed(mapped_location);
+}
+
+bool RedundantData::PosEdge(POVLocation location) const {
+ POVLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.PosEdge(mapped_location);
+}
+
+bool RedundantData::NegEdge(POVLocation location) const {
+ POVLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.NegEdge(mapped_location);
+}
+
+// Returns the current and previous "values" for the POV.
+int32_t RedundantData::GetPOV(int joystick) const {
+ return data_.GetPOV(MapRedundantJoystick(joystick));
+}
+
+int32_t RedundantData::GetOldPOV(int joystick) const {
+ return data_.GetOldPOV(MapRedundantJoystick(joystick));
+}
+
+bool RedundantData::IsPressed(ButtonLocation location) const {
+ ButtonLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.IsPressed(mapped_location);
+}
+
+bool RedundantData::PosEdge(ButtonLocation location) const {
+ ButtonLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.PosEdge(mapped_location);
+}
+
+bool RedundantData::NegEdge(ButtonLocation location) const {
+ ButtonLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.NegEdge(mapped_location);
+}
+
+bool RedundantData::GetControlBit(ControlBit bit) const {
+ return data_.GetControlBit(bit);
+}
+
+bool RedundantData::PosEdge(ControlBit bit) const { return data_.PosEdge(bit); }
+
+bool RedundantData::NegEdge(ControlBit bit) const { return data_.NegEdge(bit); }
+
+// Returns the value in the range [-1.0, 1.0].
+float RedundantData::GetAxis(JoystickAxis axis) const {
+ JoystickAxis mapped_location(MapRedundantJoystick(axis.joystick()),
+ axis.number());
+ return data_.GetAxis(mapped_location);
+}
+
+} // namespace driver_station
+} // namespace input
+} // namespace frc971
diff --git a/frc971/input/redundant_joystick_data.h b/frc971/input/redundant_joystick_data.h
new file mode 100644
index 0000000..c6cdb78
--- /dev/null
+++ b/frc971/input/redundant_joystick_data.h
@@ -0,0 +1,58 @@
+#ifndef AOS_INPUT_REDUNDANT_JOYSTICK_DATA_H_
+#define AOS_INPUT_REDUNDANT_JOYSTICK_DATA_H_
+
+#include "frc971/input/driver_station_data.h"
+
+namespace frc971 {
+namespace input {
+namespace driver_station {
+
+// A class to wrap driver_station::Data and map logical joystick numbers to
+// their actual numbers in the order they are on the driverstation.
+//
+// Bits 13 and 14 of the joystick bitmap are defined to be a two bit number
+// corresponding to the joystick's logical joystick number.
+class RedundantData : public Data {
+ public:
+ RedundantData(const Data &data);
+
+ bool IsPressed(POVLocation location) const override;
+ bool PosEdge(POVLocation location) const override;
+ bool NegEdge(POVLocation location) const override;
+
+ // Returns the current and previous "values" for the POV.
+ int32_t GetPOV(int joystick) const override;
+ int32_t GetOldPOV(int joystick) const override;
+
+ bool IsPressed(ButtonLocation location) const override;
+ bool PosEdge(ButtonLocation location) const override;
+ bool NegEdge(ButtonLocation location) const override;
+
+ bool GetControlBit(ControlBit bit) const override;
+ bool PosEdge(ControlBit bit) const override;
+ bool NegEdge(ControlBit bit) const override;
+
+ // Returns the value in the range [-1.0, 1.0].
+ float GetAxis(JoystickAxis axis) const override;
+
+ private:
+ static constexpr int kIdBit0Button = 14;
+ static constexpr int kIdBit1Button = 15;
+ static constexpr int kRedundantBitButton = 16;
+
+ int MapRedundantJoystick(int joystick) const;
+
+ // A mapping from logical joystick numbers to their actual order on the
+ // driverstation.
+ //
+ // Index is logical joystick number, Value is mapped joystick number.
+ std::array<int, JoystickFeature::kJoysticks> joystick_map_;
+
+ const Data &data_;
+};
+
+} // namespace driver_station
+} // namespace input
+} // namespace frc971
+
+#endif // AOS_INPUT_REDUNDANT_JOYSTICK_DATA_H_