7#include <frc/kinematics/SwerveModuleState.h>
8#include <units/angle.h>
9#include <units/angular_velocity.h>
10#include <units/velocity.h>
27 units::degree_t
NearestAngle(units::degree_t desiredAngle, units::degree_t referenceAngle);
37 units::degree_t
InvertedAngle(units::degree_t desiredAngle, units::degree_t referenceAngle);
47 units::degree_t
ConstrainAngle(units::degree_t inVal, units::degree_t minVal, units::degree_t maxVal);
68 frc::SwerveModuleState
Optimize(frc::SwerveModuleState desiredState,
69 units::degree_t currentModuleAngle,
70 units::degrees_per_second_t currentModuleAngularRate,
71 units::feet_per_second_t currentModuleDriveVel,
72 units::feet_per_second_t maxVelocity);
103 template <
class T,
int size,
class V>
108 const double mappedMagnitude = interpMap(magnitude);
109 return TranslationSpeeds{mappedMagnitude * std::cos(angle), mappedMagnitude * std::sin(angle)};
132 [[nodiscard]]
virtual std::optional<SwerveModulePositions>
Load() = 0;
Performs linear interpolation of a value based on a set of input->output mapping points.
Definition: interpolation.h:46
Interface capable of saving and loading module home positions from persistent storage.
Definition: swerve_utils.h:115
virtual std::optional< SwerveModulePositions > Load()=0
Load home position from persistent storage.
virtual bool Save(const SwerveModulePositions &homePosition)=0
Save home position to persistent storage.
units::degree_t NearestAngle(units::degree_t desiredAngle, units::degree_t referenceAngle)
Finds closest angle alias of desiredAngle relative to referencedAngle. All inputs are normalized so r...
Definition: swerve_utils.cpp:14
units::degree_t ConstrainAngle(units::degree_t inVal, units::degree_t minVal, units::degree_t maxVal)
Normalize angle to specified range.
Definition: swerve_utils.cpp:35
units::degree_t InvertedAngle(units::degree_t desiredAngle, units::degree_t referenceAngle)
Finds closest angle alias of a vector 180-degrees offset from desiredAngle relative to referencedAngl...
Definition: swerve_utils.cpp:28
constexpr TranslationSpeeds CircularInterpolate(const TranslationSpeeds rawSpeeds, const argos_lib::InterpolationMap< T, size, V > interpMap)
Use argos_lib::InterpolationMap to apply mapping according to joystick vector magnitude.
Definition: swerve_utils.h:104
frc::SwerveModuleState Optimize(frc::SwerveModuleState desiredState, units::degree_t currentModuleAngle, units::degrees_per_second_t currentModuleAngularRate, units::feet_per_second_t currentModuleDriveVel, units::feet_per_second_t maxVelocity)
Optimize swerve module to minimize rotations and drive direction changes.
Definition: swerve_utils.cpp:53
Definition: swap_controllers_command.h:12
Representation of the absolute encoder position of each module at home position.
Definition: swerve_utils.h:77
units::degree_t FrontRight
Definition: swerve_utils.h:79
units::degree_t RearRight
Definition: swerve_utils.h:80
units::degree_t FrontLeft
Definition: swerve_utils.h:78
units::degree_t RearLeft
Definition: swerve_utils.h:81
Translation speeds as percent max output.
Definition: swerve_utils.h:87
double leftSpeedPct
Speed with positive left in range [-1, 1].
Definition: swerve_utils.h:89
double forwardSpeedPct
Speed with positive forward in range [-1, 1].
Definition: swerve_utils.h:88