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

[wpimath] Java: add static instantiations of common rotations (#6563)

Browse Source 
C++ doesn't need this because it supports value types, which are much
cheaper to construct. constexpr is also available to make construction
zero-cost.
This commit is contained in:
Tyler Veness
2024-05-03 12:39:35 -07:00
committed by GitHub
parent 9c7120e6bf
commit e172aa66f7
75 changed files with 499 additions and 429 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java
View File

@@ -53,35 +53,35 @@ class MecanumDriveTest {
@Test
void testCartesianIKGyro90CW() {
// Forward in global frame; left in robot frame
var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, Rotation2d.fromDegrees(90.0));
var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, Rotation2d.kCCW_Pi_2);
assertEquals(-1.0, speeds.frontLeft, 1e-9);
assertEquals(1.0, speeds.frontRight, 1e-9);
assertEquals(1.0, speeds.rearLeft, 1e-9);
assertEquals(-1.0, speeds.rearRight, 1e-9);
// Left in global frame; backward in robot frame
speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, Rotation2d.fromDegrees(90.0));
speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, Rotation2d.kCCW_Pi_2);
assertEquals(-1.0, speeds.frontLeft, 1e-9);
assertEquals(-1.0, speeds.frontRight, 1e-9);
assertEquals(-1.0, speeds.rearLeft, 1e-9);
assertEquals(-1.0, speeds.rearRight, 1e-9);
// Right in global frame; forward in robot frame
speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, Rotation2d.fromDegrees(90.0));
speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, Rotation2d.kCCW_Pi_2);
assertEquals(1.0, speeds.frontLeft, 1e-9);
assertEquals(1.0, speeds.frontRight, 1e-9);
assertEquals(1.0, speeds.rearLeft, 1e-9);
assertEquals(1.0, speeds.rearRight, 1e-9);
// Rotate CCW
speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, Rotation2d.fromDegrees(90.0));
speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, Rotation2d.kCCW_Pi_2);
assertEquals(-1.0, speeds.frontLeft, 1e-9);
assertEquals(1.0, speeds.frontRight, 1e-9);
assertEquals(-1.0, speeds.rearLeft, 1e-9);
assertEquals(1.0, speeds.rearRight, 1e-9);
// Rotate CW
speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, Rotation2d.fromDegrees(90.0));
speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, Rotation2d.kCCW_Pi_2);
assertEquals(1.0, speeds.frontLeft, 1e-9);
assertEquals(-1.0, speeds.frontRight, 1e-9);
assertEquals(1.0, speeds.rearLeft, 1e-9);
@@ -143,35 +143,35 @@ class MecanumDriveTest {
drive.setDeadband(0.0);
// Forward in global frame; left in robot frame
drive.driveCartesian(1.0, 0.0, 0.0, Rotation2d.fromDegrees(90.0));
drive.driveCartesian(1.0, 0.0, 0.0, Rotation2d.kCCW_Pi_2);
assertEquals(-1.0, fl.get(), 1e-9);
assertEquals(1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);
assertEquals(-1.0, rr.get(), 1e-9);
// Left in global frame; backward in robot frame
drive.driveCartesian(0.0, -1.0, 0.0, Rotation2d.fromDegrees(90.0));
drive.driveCartesian(0.0, -1.0, 0.0, Rotation2d.kCCW_Pi_2);
assertEquals(-1.0, fl.get(), 1e-9);
assertEquals(-1.0, fr.get(), 1e-9);
assertEquals(-1.0, rl.get(), 1e-9);
assertEquals(-1.0, rr.get(), 1e-9);
// Right in global frame; forward in robot frame
drive.driveCartesian(0.0, 1.0, 0.0, Rotation2d.fromDegrees(90.0));
drive.driveCartesian(0.0, 1.0, 0.0, Rotation2d.kCCW_Pi_2);
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);
assertEquals(1.0, rr.get(), 1e-9);
// Rotate CCW
drive.driveCartesian(0.0, 0.0, -1.0, Rotation2d.fromDegrees(90.0));
drive.driveCartesian(0.0, 0.0, -1.0, Rotation2d.kCCW_Pi_2);
assertEquals(-1.0, fl.get(), 1e-9);
assertEquals(1.0, fr.get(), 1e-9);
assertEquals(-1.0, rl.get(), 1e-9);
assertEquals(1.0, rr.get(), 1e-9);
// Rotate CW
drive.driveCartesian(0.0, 0.0, 1.0, Rotation2d.fromDegrees(90.0));
drive.driveCartesian(0.0, 0.0, 1.0, Rotation2d.kCCW_Pi_2);
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(-1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);
@@ -188,35 +188,35 @@ class MecanumDriveTest {
drive.setDeadband(0.0);
// Forward
drive.drivePolar(1.0, Rotation2d.fromDegrees(0.0), 0.0);
drive.drivePolar(1.0, Rotation2d.kZero, 0.0);
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);
assertEquals(1.0, rr.get(), 1e-9);
// Left
drive.drivePolar(1.0, Rotation2d.fromDegrees(-90.0), 0.0);
drive.drivePolar(1.0, Rotation2d.kCW_Pi_2, 0.0);
assertEquals(-1.0, fl.get(), 1e-9);
assertEquals(1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);
assertEquals(-1.0, rr.get(), 1e-9);
// Right
drive.drivePolar(1.0, Rotation2d.fromDegrees(90.0), 0.0);
drive.drivePolar(1.0, Rotation2d.kCCW_Pi_2, 0.0);
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(-1.0, fr.get(), 1e-9);
assertEquals(-1.0, rl.get(), 1e-9);
assertEquals(1.0, rr.get(), 1e-9);
// Rotate CCW
drive.drivePolar(0.0, Rotation2d.fromDegrees(0.0), -1.0);
drive.drivePolar(0.0, Rotation2d.kZero, -1.0);
assertEquals(-1.0, fl.get(), 1e-9);
assertEquals(1.0, fr.get(), 1e-9);
assertEquals(-1.0, rl.get(), 1e-9);
assertEquals(1.0, rr.get(), 1e-9);
// Rotate CW
drive.drivePolar(0.0, Rotation2d.fromDegrees(0.0), 1.0);
drive.drivePolar(0.0, Rotation2d.kZero, 1.0);
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(-1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);

2
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java
View File

@@ -18,7 +18,7 @@ class AnalogEncoderSimTest {
var analogEncoder = new AnalogEncoder(analogInput)) {
var encoderSim = new AnalogEncoderSim(analogEncoder);
encoderSim.setPosition(Rotation2d.fromDegrees(180));
encoderSim.setPosition(Rotation2d.kPi);
assertEquals(analogEncoder.get(), 0.5, 1E-8);
assertEquals(encoderSim.getTurns(), 0.5, 1E-8);
assertEquals(encoderSim.getPosition().getRadians(), Math.PI, 1E-8);

4
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
View File

@@ -55,9 +55,9 @@ class DifferentialDrivetrainSimTest {
var traj =
TrajectoryGenerator.generateTrajectory(
new Pose2d(),
Pose2d.kZero,
List.of(),
new Pose2d(2, 2, new Rotation2d()),
new Pose2d(2, 2, Rotation2d.kZero),
new TrajectoryConfig(1, 1)
.addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));