Merge "Add Basic Swerve Joystick Reader" into main
diff --git a/frc971/input/BUILD b/frc971/input/BUILD
index c992932..34fa70b 100644
--- a/frc971/input/BUILD
+++ b/frc971/input/BUILD
@@ -38,6 +38,8 @@
"//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/control_loops/drivetrain:spline_goal_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_goal_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_joystick_goal_fbs",
"//frc971/input:driver_station_data",
],
)
@@ -91,6 +93,21 @@
],
)
+cc_library(
+ name = "swerve_joystick_input",
+ srcs = ["swerve_joystick_input.cc"],
+ hdrs = ["swerve_joystick_input.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":drivetrain_input",
+ ":redundant_joystick_data",
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/logging",
+ "//frc971/input:joystick_input",
+ ],
+)
+
static_flatbuffer(
name = "robot_state_fbs",
srcs = ["robot_state.fbs"],
diff --git a/frc971/input/drivetrain_input.cc b/frc971/input/drivetrain_input.cc
index be61cc4..d93d140 100644
--- a/frc971/input/drivetrain_input.cc
+++ b/frc971/input/drivetrain_input.cc
@@ -6,6 +6,7 @@
#include "aos/commonmath.h"
#include "aos/logging/logging.h"
+#include "drivetrain_input.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"
@@ -120,6 +121,56 @@
last_is_control_loop_driving_ = is_control_loop_driving;
}
+void SwerveDrivetrainInputReader::HandleDrivetrain(
+ const ::frc971::input::driver_station::Data &data) {
+ const auto swerve_goals = GetSwerveGoals(data);
+ const double vx = swerve_goals.vx;
+ const double vy = swerve_goals.vy;
+ const double omega = swerve_goals.omega;
+
+ auto builder = goal_sender_.MakeStaticBuilder();
+
+ auto joystick_goal = builder->add_joystick_goal();
+
+ joystick_goal->set_vx(vx);
+ joystick_goal->set_vy(vy);
+ joystick_goal->set_omega(omega);
+
+ builder.CheckOk(builder.Send());
+}
+
+std::unique_ptr<SwerveDrivetrainInputReader> SwerveDrivetrainInputReader::Make(
+ ::aos::EventLoop *event_loop) {
+ // Swerve Controller
+ // axis (2, 2) will give you alternative omega axis (controls with vertical
+ // movement)
+ const JoystickAxis kVxAxis(2, 1), kVyAxis(1, 1), kOmegaAxis(1, 2);
+
+ std::unique_ptr<SwerveDrivetrainInputReader> result(
+ new SwerveDrivetrainInputReader(event_loop, kVxAxis, kVyAxis,
+ kOmegaAxis));
+ return result;
+}
+
+SwerveDrivetrainInputReader::SwerveGoals
+SwerveDrivetrainInputReader::GetSwerveGoals(
+ const ::frc971::input::driver_station::Data &data) {
+ // xbox
+ constexpr double kMovementDeadband = 0.05;
+ constexpr double kRotationDeadband = 0.05;
+
+ const double omega =
+ -aos::Deadband(-data.GetAxis(omega_axis_), kRotationDeadband, 1.0);
+
+ const double vx =
+ aos::Deadband(-data.GetAxis(vx_axis_), kMovementDeadband, 1.0);
+
+ const double vy =
+ aos::Deadband(-data.GetAxis(vy_axis_), kMovementDeadband, 1.0);
+
+ return SwerveDrivetrainInputReader::SwerveGoals{vx, vy, omega};
+}
+
DrivetrainInputReader::WheelAndThrottle
SteeringWheelDrivetrainInputReader::GetWheelAndThrottle(
const ::frc971::input::driver_station::Data &data) {
diff --git a/frc971/input/drivetrain_input.h b/frc971/input/drivetrain_input.h
index 173c551..e135f2c 100644
--- a/frc971/input/drivetrain_input.h
+++ b/frc971/input/drivetrain_input.h
@@ -11,6 +11,8 @@
#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/control_loops/swerve/swerve_drivetrain_goal_static.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_joystick_goal_static.h"
#include "frc971/input/driver_station_data.h"
namespace frc971::input {
@@ -155,6 +157,57 @@
vision_align_fn_;
};
+class SwerveDrivetrainInputReader {
+ SwerveDrivetrainInputReader(::aos::EventLoop *event_loop,
+ driver_station::JoystickAxis vx_axis,
+ driver_station::JoystickAxis vy_axis,
+ driver_station::JoystickAxis omega_axis)
+ : vx_axis_(vx_axis),
+ vy_axis_(vy_axis),
+ omega_axis_(omega_axis),
+ goal_sender_(event_loop->MakeSender<control_loops::swerve::GoalStatic>(
+ "/drivetrain")) {}
+
+ public:
+ virtual ~SwerveDrivetrainInputReader() = default;
+
+ // Constructs the appropriate DrivetrainInputReader.
+ static std::unique_ptr<SwerveDrivetrainInputReader> Make(
+ ::aos::EventLoop *event_loop);
+
+ // 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;
+ }
+
+ protected:
+ const driver_station::JoystickAxis vx_axis_;
+ const driver_station::JoystickAxis vy_axis_;
+ const driver_station::JoystickAxis omega_axis_;
+
+ // Structure containing the (potentially adjusted) steering and throttle
+ // values from the joysticks.
+ struct SwerveGoals {
+ double vx;
+ double vy;
+ double omega;
+ };
+
+ private:
+ // Computes the steering and throttle from the provided driverstation data.
+ SwerveGoals GetSwerveGoals(const ::frc971::input::driver_station::Data &data);
+
+ ::aos::Sender<control_loops::swerve::GoalStatic> goal_sender_;
+
+ // 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:
diff --git a/frc971/input/swerve_joystick_input.cc b/frc971/input/swerve_joystick_input.cc
new file mode 100644
index 0000000..fbd7f01
--- /dev/null
+++ b/frc971/input/swerve_joystick_input.cc
@@ -0,0 +1,28 @@
+#include "frc971/input/swerve_joystick_input.h"
+
+#include "frc971/input/driver_station_data.h"
+#include "frc971/input/redundant_joystick_data.h"
+
+using frc971::input::driver_station::ControlBit;
+
+namespace frc971::input {
+
+void SwerveJoystickInput::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 SwerveJoystickInput::DoRunIteration(
+ const ::frc971::input::driver_station::Data &data) {
+ drivetrain_input_reader_->HandleDrivetrain(data);
+ HandleTeleop(data);
+ action_queue_.Tick();
+ was_running_ = action_queue_.Running();
+}
+
+} // namespace frc971::input
diff --git a/frc971/input/swerve_joystick_input.h b/frc971/input/swerve_joystick_input.h
new file mode 100644
index 0000000..532bd9b
--- /dev/null
+++ b/frc971/input/swerve_joystick_input.h
@@ -0,0 +1,76 @@
+#ifndef AOS_INPUT_SWERVE_JOYSTICK_INPUT_H_
+#define AOS_INPUT_SWERVE_JOYSTICK_INPUT_H_
+
+#include "aos/actions/actions.h"
+#include "frc971/input/driver_station_data.h"
+#include "frc971/input/drivetrain_input.h"
+#include "frc971/input/joystick_input.h"
+
+using frc971::control_loops::drivetrain::PistolBottomButtonUse;
+using frc971::control_loops::drivetrain::PistolSecondButtonUse;
+using frc971::control_loops::drivetrain::PistolTopButtonUse;
+
+namespace frc971::input {
+
+class SwerveJoystickInput : public ::frc971::input::JoystickInput {
+ public:
+ // Configuration parameters that don't really belong in the DrivetrainConfig.
+ struct InputConfig {
+ // 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;
+ };
+ SwerveJoystickInput(::aos::EventLoop *event_loop,
+ const InputConfig &input_config)
+ : ::frc971::input::JoystickInput(event_loop),
+ input_config_(input_config),
+ drivetrain_input_reader_(SwerveDrivetrainInputReader::Make(event_loop)),
+ goal_sender_(event_loop->MakeSender<control_loops::swerve::GoalStatic>(
+ "/drivetrain")) {}
+
+ virtual ~SwerveJoystickInput() {}
+
+ 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));
+ }
+
+ private:
+ // 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 DoRunIteration(const ::frc971::input::driver_station::Data &data);
+
+ void HandleDrivetrain(const ::frc971::input::driver_station::Data &data);
+
+ // True if an action was running last cycle.
+ bool was_running_ = false;
+
+ // Bool to track if auto was running the last cycle through. This lets us
+ // call AutoEnded when the auto mode function stops.
+
+ const InputConfig input_config_;
+
+ ::std::unique_ptr<SwerveDrivetrainInputReader> drivetrain_input_reader_;
+ ::aos::Sender<control_loops::swerve::GoalStatic> goal_sender_;
+
+ ::aos::common::actions::ActionQueue action_queue_;
+};
+
+} // namespace frc971::input
+
+#endif // AOS_SWERVE_ACTION_JOYSTICK_INPUT_H_
diff --git a/y2024_swerve/BUILD b/y2024_swerve/BUILD
index 2357c6e..a114728 100644
--- a/y2024_swerve/BUILD
+++ b/y2024_swerve/BUILD
@@ -27,6 +27,7 @@
"//y2024/www:www_files",
],
start_binaries = [
+ ":joystick_reader",
"//aos/events/logging:logger_main",
"//aos/network:web_proxy_main",
"//aos/starter:irq_affinity",
@@ -43,6 +44,7 @@
robot_downloader(
name = "orin_download",
binaries = [
+ "//frc971/wpilib:joystick_republish",
"//aos/events:aos_timing_report_streamer",
"//aos/events/logging:log_cat",
"//aos:aos_jitter",
@@ -275,3 +277,21 @@
"//frc971/input:aos_config",
],
)
+
+cc_binary(
+ name = "joystick_reader",
+ srcs = [
+ ":joystick_reader.cc",
+ ],
+ deps = [
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/events:shm_event_loop",
+ "//aos/logging",
+ "//frc971/input:drivetrain_input",
+ "//frc971/input:joystick_input",
+ "//frc971/input:redundant_joystick_data",
+ "//frc971/input:swerve_joystick_input",
+ "//y2024/control_loops/drivetrain:drivetrain_base",
+ ],
+)
diff --git a/y2024_swerve/joystick_reader.cc b/y2024_swerve/joystick_reader.cc
new file mode 100644
index 0000000..6a1eb46
--- /dev/null
+++ b/y2024_swerve/joystick_reader.cc
@@ -0,0 +1,60 @@
+#include <unistd.h>
+
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+
+#include "absl/flags/flag.h"
+
+#include "aos/actions/actions.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
+#include "aos/util/log_interval.h"
+#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/input/swerve_joystick_input.h"
+
+using frc971::CreateProfileParameters;
+using frc971::input::driver_station::ButtonLocation;
+using frc971::input::driver_station::ControlBit;
+using frc971::input::driver_station::JoystickAxis;
+using frc971::input::driver_station::POVLocation;
+using Side = frc971::control_loops::drivetrain::RobotSide;
+
+namespace y2024_swerve::input::joysticks {
+
+namespace swerve = frc971::control_loops::swerve;
+
+class Reader : public ::frc971::input::SwerveJoystickInput {
+ public:
+ Reader(::aos::EventLoop *event_loop)
+ : ::frc971::input::SwerveJoystickInput(
+ event_loop, {.use_redundant_joysticks = true}) {}
+
+ void HandleTeleop(
+ const ::frc971::input::driver_station::Data &data) override {
+ // Where teleop logic will eventually go when there is superstructure code
+ (void)data;
+ }
+};
+
+} // namespace y2024_swerve::input::joysticks
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2024_swerve::input::joysticks::Reader reader(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024_swerve/y2024_swerve_roborio.json b/y2024_swerve/y2024_swerve_roborio.json
index adf528d..6d933f7 100644
--- a/y2024_swerve/y2024_swerve_roborio.json
+++ b/y2024_swerve/y2024_swerve_roborio.json
@@ -302,6 +302,16 @@
],
"applications": [
{
+ "name": "joystick_reader",
+ "executable_name": "joystick_reader",
+ "args": [
+ "--nodie_on_malloc"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
"name": "wpilib_interface",
"executable_name": "wpilib_interface",
"args": [