Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix swerve kinematics util classes equals function (#4907)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
ohowe
2023-01-08 17:29:35 -07:00
committed by GitHub
parent 51272ef6b3
commit 330ba45f9c
8 changed files with 102 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,15 @@
// 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 Normal file
View File

@@ -0,0 +1,24 @@
// 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};
}
}

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

@@ -25,5 +25,13 @@ struct WPILIB_DLLEXPORT SwerveModulePosition {
* Angle of the module.
*/
Rotation2d angle;
/**
* Checks equality between this SwerveModulePosition and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const SwerveModulePosition& other) const;
};
} // namespace frc

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

@@ -26,6 +26,14 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
*/
Rotation2d angle;
/**
* Checks equality between this SwerveModuleState and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const SwerveModuleState& other) const;
/**
* Minimize the change in heading the desired swerve module state would
* require by potentially reversing the direction the wheel spins. If this is
@@ -36,13 +44,6 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
* @param currentAngle The current module angle.
*/
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};
}
}
const Rotation2d& currentAngle);
};
} // namespace frc