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

[wpimath] Make various classes constexpr (#7237)

Browse Source
This commit is contained in:
Tyler Veness
2024-10-22 06:58:06 -07:00
committed by GitHub
parent 0c824bd447
commit 05c7fd929b
21 changed files with 235 additions and 307 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -45,7 +45,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*
* @return Twist2d.
*/
Twist2d ToTwist2d(units::second_t dt) const {
constexpr Twist2d ToTwist2d(units::second_t dt) const {
return Twist2d{vx * dt, vy * dt, omega * dt};
}
@@ -68,10 +68,10 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*
* @return Discretized ChassisSpeeds.
*/
static ChassisSpeeds Discretize(units::meters_per_second_t vx,
units::meters_per_second_t vy,
units::radians_per_second_t omega,
units::second_t dt) {
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) {
// 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};
@@ -101,8 +101,8 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
*
* @return Discretized ChassisSpeeds.
*/
static ChassisSpeeds Discretize(const ChassisSpeeds& continuousSpeeds,
units::second_t dt) {
static constexpr ChassisSpeeds Discretize(
const ChassisSpeeds& continuousSpeeds, units::second_t dt) {
return Discretize(continuousSpeeds.vx, continuousSpeeds.vy,
continuousSpeeds.omega, dt);
}
@@ -123,7 +123,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
*/
static ChassisSpeeds FromFieldRelativeSpeeds(
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) {
// CW rotation into chassis frame
@@ -148,7 +148,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* @return ChassisSpeeds object representing the speeds in the robot's frame
* of reference.
*/
static ChassisSpeeds FromFieldRelativeSpeeds(
static constexpr ChassisSpeeds FromFieldRelativeSpeeds(
const ChassisSpeeds& fieldRelativeSpeeds, const Rotation2d& robotAngle) {
return FromFieldRelativeSpeeds(fieldRelativeSpeeds.vx,
fieldRelativeSpeeds.vy,
@@ -171,7 +171,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* @return ChassisSpeeds object representing the speeds in the field's frame
* of reference.
*/
static ChassisSpeeds FromRobotRelativeSpeeds(
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) {
// CCW rotation out of chassis frame
@@ -196,7 +196,7 @@ struct WPILIB_DLLEXPORT ChassisSpeeds {
* @return ChassisSpeeds object representing the speeds in the field's frame
* of reference.
*/
static ChassisSpeeds FromRobotRelativeSpeeds(
static constexpr ChassisSpeeds FromRobotRelativeSpeeds(
const ChassisSpeeds& robotRelativeSpeeds, const Rotation2d& robotAngle) {
return FromRobotRelativeSpeeds(robotRelativeSpeeds.vx,
robotRelativeSpeeds.vy,

17
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
View File

@@ -4,6 +4,8 @@
#pragma once
#include <type_traits>
#include <wpi/SymbolExports.h>
#include "frc/geometry/Twist2d.h"
@@ -36,10 +38,12 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
explicit DifferentialDriveKinematics(units::meter_t trackWidth)
constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth)
: trackWidth(trackWidth) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1);
if (!std::is_constant_evaluated()) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kKinematics_DifferentialDrive, 1);
}
}
/**
@@ -83,12 +87,13 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
(rightDistance - leftDistance) / trackWidth * 1_rad};
}
Twist2d ToTwist2d(const DifferentialDriveWheelPositions& start,
const DifferentialDriveWheelPositions& end) const override {
constexpr Twist2d ToTwist2d(
const DifferentialDriveWheelPositions& start,
const DifferentialDriveWheelPositions& end) const override {
return ToTwist2d(end.left - start.left, end.right - start.right);
}
DifferentialDriveWheelPositions Interpolate(
constexpr DifferentialDriveWheelPositions Interpolate(
const DifferentialDriveWheelPositions& start,
const DifferentialDriveWheelPositions& end, double t) const override {
return start.Interpolate(end, t);

14
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h
View File

@@ -31,18 +31,10 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const DifferentialDriveWheelPositions& other) const = default;
constexpr bool operator==(
const DifferentialDriveWheelPositions& other) const = default;
/**
* Checks inequality between this DifferentialDriveWheelPositions and another
* object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const DifferentialDriveWheelPositions& other) const = default;
DifferentialDriveWheelPositions Interpolate(
constexpr DifferentialDriveWheelPositions Interpolate(
const DifferentialDriveWheelPositions& endValue, double t) const {
return {wpi::Lerp(left, endValue.left, t),
wpi::Lerp(right, endValue.right, t)};

11
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
View File

@@ -6,6 +6,7 @@
#include <wpi/SymbolExports.h>
#include "units/math.h"
#include "units/velocity.h"
namespace frc {
@@ -35,7 +36,15 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Desaturate(units::meters_per_second_t attainableMaxSpeed);
constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) {
auto realMaxSpeed =
units::math::max(units::math::abs(left), units::math::abs(right));
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;
right = right / realMaxSpeed * attainableMaxSpeed;
}
}
/**
* Adds two DifferentialDriveWheelSpeeds and returns the sum.

14
wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h
View File

@@ -40,18 +40,10 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const MecanumDriveWheelPositions& other) const = default;
constexpr bool operator==(const MecanumDriveWheelPositions& other) const =
default;
/**
* Checks inequality between this MecanumDriveWheelPositions and another
* object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const MecanumDriveWheelPositions& other) const = default;
MecanumDriveWheelPositions Interpolate(
constexpr MecanumDriveWheelPositions Interpolate(
const MecanumDriveWheelPositions& endValue, double t) const {
return {wpi::Lerp(frontLeft, endValue.frontLeft, t),
wpi::Lerp(frontRight, endValue.frontRight, t),

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

@@ -4,8 +4,13 @@
#pragma once
#include <algorithm>
#include <array>
#include <cmath>
#include <wpi/SymbolExports.h>
#include "units/math.h"
#include "units/velocity.h"
namespace frc {
@@ -45,7 +50,25 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
*/
void Desaturate(units::meters_per_second_t attainableMaxSpeed);
constexpr void Desaturate(units::meters_per_second_t attainableMaxSpeed) {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
units::meters_per_second_t realMaxSpeed = units::math::abs(
*std::max_element(wheelSpeeds.begin(), wheelSpeeds.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a) < units::math::abs(b);
}));
if (realMaxSpeed > attainableMaxSpeed) {
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];
}
}
/**
* Adds two MecanumDriveWheelSpeeds and returns the sum.

10
wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h
View File

@@ -8,7 +8,6 @@
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation2d.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/math.h"
@@ -33,10 +32,13 @@ struct WPILIB_DLLEXPORT SwerveModulePosition {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const SwerveModulePosition& other) const;
constexpr bool operator==(const SwerveModulePosition& other) const {
return units::math::abs(distance - other.distance) < 1E-9_m &&
angle == other.angle;
}
SwerveModulePosition Interpolate(const SwerveModulePosition& endValue,
double t) const {
constexpr SwerveModulePosition Interpolate(
const SwerveModulePosition& endValue, double t) const {
return {wpi::Lerp(distance, endValue.distance, t),
wpi::Lerp(angle, endValue.angle, t)};
}

20
wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
View File

@@ -32,7 +32,10 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const SwerveModuleState& other) const;
constexpr bool operator==(const SwerveModuleState& other) const {
return units::math::abs(speed - other.speed) < 1E-9_mps &&
angle == other.angle;
}
/**
* Minimize the change in the heading this swerve module state would
@@ -42,7 +45,7 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
*
* @param currentAngle The current module angle.
*/
void Optimize(const Rotation2d& currentAngle) {
constexpr void Optimize(const Rotation2d& currentAngle) {
auto delta = angle - currentAngle;
if (units::math::abs(delta.Degrees()) > 90_deg) {
speed *= -1;
@@ -60,8 +63,15 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
* @param currentAngle The current module angle.
*/
[[deprecated("Use instance method instead.")]]
static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
const Rotation2d& currentAngle);
constexpr static SwerveModuleState Optimize(
const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
auto delta = desiredState.angle - currentAngle;
if (units::math::abs(delta.Degrees()) > 90_deg) {
return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
} else {
return {desiredState.speed, desiredState.angle};
}
}
/**
* Scales speed by cosine of angle error. This scales down movement
@@ -70,7 +80,7 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
*
* @param currentAngle The current module angle.
*/
void CosineScale(const Rotation2d& currentAngle) {
constexpr void CosineScale(const Rotation2d& currentAngle) {
speed *= (angle - currentAngle).Cos();
}
};