Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use immutable member functions in ChassisSpeeds (#7545)

Browse Source
This commit is contained in:
Tyler Veness
2024-12-15 16:09:34 -08:00
committed by GitHub
parent 945d416d07
commit 03f0fc4dea
24 changed files with 163 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
View File

@@ -115,8 +115,8 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
m_rotationError = desiredHeading - currentPose.Rotation();
if (!m_enabled) {
return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
currentPose.Rotation());
return ChassisSpeeds{xFF, yFF, thetaFF}.ToRobotRelative(
currentPose.Rotation());
}
// Calculate feedback velocities (based on position error).
@@ -126,8 +126,8 @@ class WPILIB_DLLEXPORT HolonomicDriveController {
currentPose.Y().value(), trajectoryPose.Y().value())};
// Return next output.
return ChassisSpeeds::FromFieldRelativeSpeeds(
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation());
return ChassisSpeeds{xFF + xFeedback, yFF + yFeedback, thetaFF}
.ToRobotRelative(currentPose.Rotation());
}
/**

112
wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
View File

@@ -61,17 +61,10 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* This is useful for compensating for translational skew when translating and
* rotating a swerve drivetrain.
*
* @param vx Forward velocity.
* @param vy Sideways velocity.
* @param omega Angular velocity.
* @param dt The duration of the timestep the speeds should be applied for.
*
* @return Discretized ChassisSpeeds.
*/
static constexpr ChassisSpeeds Discretize(units::meters_per_second_t vx,
units::meters_per_second_t vy,
units::radians_per_second_t omega,
units::second_t dt) {
constexpr ChassisSpeeds Discretize(units::second_t dt) const {
// Construct the desired pose after a timestep, relative to the current
// pose. The desired pose has decoupled translation and rotation.
Pose2d desiredDeltaPose{vx * dt, vy * dt, omega * dt};
@@ -85,47 +78,17 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
}
/**
* Discretizes a continuous-time chassis speed.
*
* This function converts a continuous-time chassis speed into a discrete-time
* one such that when the discrete-time chassis speed is applied for one
* timestep, the robot moves as if the velocity components are independent
* (i.e., the robot moves v_x * dt along the x-axis, v_y * dt along the
* y-axis, and omega * dt around the z-axis).
*
* This is useful for compensating for translational skew when translating and
* rotating a swerve drivetrain.
*
* @param continuousSpeeds The continuous speeds.
* @param dt The duration of the timestep the speeds should be applied for.
*
* @return Discretized ChassisSpeeds.
*/
static constexpr ChassisSpeeds Discretize(
const ChassisSpeeds& continuousSpeeds, units::second_t dt) {
return Discretize(continuousSpeeds.vx, continuousSpeeds.vy,
continuousSpeeds.omega, dt);
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative
* Converts this field-relative set of speeds into a robot-relative
* ChassisSpeeds object.
*
* @param vx The component of speed in the x direction relative to the field.
* Positive x is away from your alliance wall.
* @param vy The component of speed in the y direction relative to the field.
* Positive y is to your left when standing behind the alliance wall.
* @param omega The angular rate of the robot.
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away from
* your alliance station wall. Remember that this should be CCW positive.
*
* robot's angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should be CCW
* positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
* of reference.
*/
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
constexpr ChassisSpeeds ToRobotRelative(const Rotation2d& robotAngle) const {
// CW rotation into chassis frame
auto rotated =
Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
@@ -135,45 +98,17 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
}
/**
* Converts a user provided field-relative ChassisSpeeds object into a
* robot-relative ChassisSpeeds object.
*
* @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds
* in the field frame of reference. Positive x is away from your alliance
* wall. Positive y is to your left when standing behind the alliance wall.
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should be CCW
* positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
*/
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(
const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) {
return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx,
fieldRelativeSpeeds.vy,
fieldRelativeSpeeds.omega, robotAngle);
}
/**
* Converts a user provided robot-relative set of speeds into a field-relative
* Converts this robot-relative set of speeds into a field-relative
* ChassisSpeeds object.
*
* @param vx The component of speed in the x direction relative to the robot.
* Positive x is towards the robot's front.
* @param vy The component of speed in the y direction relative to the robot.
* Positive y is towards the robot's left.
* @param omega The angular rate of the robot.
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away from
* your alliance station wall. Remember that this should be CCW positive.
*
* robot's angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should be CCW
* positive.
* @return ChassisSpeeds object representing the speeds in the field's frame
* of reference.
* of reference.
*/
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(
units::meters_per_second_t vx, units::meters_per_second_t vy,
units::radians_per_second_t omega, const Rotation2d& robotAngle) {
constexpr ChassisSpeeds ToFieldRelative(const Rotation2d& robotAngle) const {
// CCW rotation out of chassis frame
auto rotated =
Translation2d{units::meter_t{vx.value()}, units::meter_t{vy.value()}}
@@ -182,27 +117,6 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
units::meters_per_second_t{rotated.Y().value()}, omega};
}
/**
* Converts a user provided robot-relative ChassisSpeeds object into a
* field-relative ChassisSpeeds object.
*
* @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds
* in the robot frame of reference. Positive x is the towards robot's
* front. Positive y is towards the robot's left.
* @param robotAngle The angle of the robot as measured by a gyroscope. The
* robot's angle is considered to be zero when it is facing directly away
* from your alliance station wall. Remember that this should be CCW
* positive.
* @return ChassisSpeeds object representing the speeds in the field's frame
* of reference.
*/
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(
const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) {
return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx,
robotRelativeSpeeds.vy,
robotRelativeSpeeds.omega, robotAngle);
}
/**
* Adds two ChassisSpeeds and returns the sum.
*

12
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
View File

@@ -49,8 +49,10 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
* threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
* @return Desaturated MecanumDriveWheelSpeeds.
*/
constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) {
constexpr MecanumDriveWheelSpeeds Desaturate(
units::meters_per_second_t attainableMaxSpeed) const {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
units::meters_per_second_t realMaxSpeed = units::math::abs(
@@ -63,11 +65,11 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
for (int i = 0; i < 4; ++i) {
wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
}
frontLeft = wheelSpeeds[0];
frontRight = wheelSpeeds[1];
rearLeft = wheelSpeeds[2];
rearRight = wheelSpeeds[3];
return MecanumDriveWheelSpeeds{wheelSpeeds[0], wheelSpeeds[1],
wheelSpeeds[2], wheelSpeeds[3]};
}
return *this;
}
/**

6
wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
View File

@@ -32,9 +32,9 @@ class WPILIB_DLLEXPORT MecanumDriveKinematicsConstraint
units::meters_per_second_t velocity) const override {
auto xVelocity = velocity * pose.Rotation().Cos();
auto yVelocity = velocity * pose.Rotation().Sin();
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
{xVelocity, yVelocity, velocity * curvature});
wheelSpeeds.Desaturate(m_maxSpeed);
auto wheelSpeeds =
m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature})
.Desaturate(m_maxSpeed);
auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);