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_
diff --git a/y2023/BUILD b/y2023/BUILD
index 08713b7..11600de 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -322,6 +322,7 @@
"//frc971/input:action_joystick_input",
"//frc971/input:drivetrain_input",
"//frc971/input:joystick_input",
+ "//frc971/input:redundant_joystick_data",
"//y2023/control_loops/drivetrain:drivetrain_base",
"//y2023/control_loops/drivetrain:target_selector_hint_fbs",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
diff --git a/y2023/constants.h b/y2023/constants.h
index f404472..2f16f1a 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -121,9 +121,7 @@
// Wrist
static constexpr double kWristEncoderCountsPerRevolution() { return 4096.0; }
- static constexpr double kCompWristEncoderRatio() {
- return 1.0;
- }
+ static constexpr double kCompWristEncoderRatio() { return 1.0; }
static constexpr double kPracticeWristEncoderRatio() {
return (24.0 / 36.0) * (36.0 / 60.0);
}
@@ -172,7 +170,7 @@
}
// if true, tune down all the arm constants for testing.
- static constexpr bool kArmGrannyMode() { return false; }
+ static constexpr bool kArmGrannyMode() { return true; }
// the operating voltage.
static constexpr double kArmOperatingVoltage() {
diff --git a/y2023/control_loops/superstructure/superstructure_main.cc b/y2023/control_loops/superstructure/superstructure_main.cc
index 10a9ca9..4a044b6 100644
--- a/y2023/control_loops/superstructure/superstructure_main.cc
+++ b/y2023/control_loops/superstructure/superstructure_main.cc
@@ -16,6 +16,8 @@
::aos::ShmEventLoop event_loop(&config.message());
+ frc971::constants::WaitForConstants<y2023::Constants>(&config.message());
+
auto trajectories =
y2023::control_loops::superstructure::Superstructure::GetArmTrajectories(
FLAGS_arm_trajectories);
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index efd7537..dcba253 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -16,6 +16,7 @@
#include "frc971/input/driver_station_data.h"
#include "frc971/input/drivetrain_input.h"
#include "frc971/input/joystick_input.h"
+#include "frc971/input/redundant_joystick_data.h"
#include "frc971/zeroing/wrap.h"
#include "y2023/constants.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
@@ -46,7 +47,7 @@
constexpr double kCubeWrist = 1.0;
// TODO(milind): add correct locations
-const ButtonLocation kDriverSpit(2, 1);
+const ButtonLocation kDriverSpit(1, 1);
const ButtonLocation kSpit(4, 13);
const ButtonLocation kHighConeScoreLeft(4, 14);
@@ -369,7 +370,8 @@
: ::frc971::input::ActionJoystickInput(
event_loop,
::y2023::control_loops::drivetrain::GetDrivetrainConfig(),
- ::frc971::input::DrivetrainInputReader::InputType::kPistol, {}),
+ ::frc971::input::DrivetrainInputReader::InputType::kPistol,
+ {.use_redundant_joysticks = true}),
superstructure_goal_sender_(
event_loop->MakeSender<superstructure::Goal>("/superstructure")),
target_selector_hint_sender_(
@@ -519,7 +521,7 @@
superstructure::Goal::Builder superstructure_goal_builder =
builder.MakeBuilder<superstructure::Goal>();
- superstructure_goal_builder.add_arm_goal_position(arm_goal_position_);
+ superstructure_goal_builder.add_arm_goal_position(arm::NeutralIndex());
superstructure_goal_builder.add_roller_goal(roller_goal);
superstructure_goal_builder.add_wrist(wrist_offset);
if (builder.Send(superstructure_goal_builder.Finish()) !=