Add Basic Swerve Joystick Reader
Signed-off-by: Nikolai Sohmers <nikolai@sohmers.com>
Change-Id: I97f6af6b978b8383d97f5549ca2d3ea82def5359
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_