Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

19
wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
View File

@@ -1,19 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/ComputerVisionUtil.h"
#include "units/math.h"
namespace frc {
frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
const frc::Transform3d& cameraToObject,
const frc::Transform3d& robotToCamera) {
const auto objectToCamera = cameraToObject.Inverse();
const auto cameraToRobot = robotToCamera.Inverse();
return objectInField + objectToCamera + cameraToRobot;
}
} // namespace frc

17
wpimath/src/main/native/cpp/StateSpaceUtil.cpp
View File

@@ -6,18 +6,6 @@
namespace frc {
Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
return Eigen::Vector3d{pose.Translation().X().value(),
pose.Translation().Y().value(),
pose.Rotation().Radians().value()};
}
Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
return Eigen::Vector4d{pose.Translation().X().value(),
pose.Translation().Y().value(), pose.Rotation().Cos(),
pose.Rotation().Sin()};
}
template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
const Matrixd<1, 1>& B);
template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
@@ -25,9 +13,4 @@ template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
template bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
Eigen::Vector3d PoseToVector(const Pose2d& pose) {
return Eigen::Vector3d{pose.X().value(), pose.Y().value(),
pose.Rotation().Radians().value()};
}
} // namespace frc

20
wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
View File

@@ -1,20 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
#include "units/math.h"
using namespace frc;
void DifferentialDriveWheelSpeeds::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;
}
}

33
wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
View File

@@ -1,33 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
#include <algorithm>
#include <array>
#include <cmath>
#include "units/math.h"
using namespace frc;
void MecanumDriveWheelSpeeds::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];
}
}

15
wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp
View File

@@ -1,15 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/kinematics/SwerveModulePosition.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "units/math.h"
using namespace frc;
bool SwerveModulePosition::operator==(const SwerveModulePosition& other) const {
return units::math::abs(distance - other.distance) < 1E-9_m &&
angle == other.angle;
}

24
wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp
View File

@@ -1,24 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/kinematics/SwerveModuleState.h"
#include "units/math.h"
using namespace frc;
bool SwerveModuleState::operator==(const SwerveModuleState& other) const {
return units::math::abs(speed - other.speed) < 1E-9_mps &&
angle == other.angle;
}
SwerveModuleState 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};
}
}