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

[wpilib] Make drive classes follow NWU axes convention (#4079)

Browse Source 
All trigonometric functions and vector classes assume North-West-Up axes
convention, so using North-East-Down convention with them is really
error-prone. We've broken something every time we touched the drive
classes.

We originally used North-East-Down to match the joystick convention, but
the volume of long-lived bugs has made this not worth it in retrospect.

The rest of WPILib also uses North-West-Up, so this makes things
consistent.

KilloughDrive was removed since no one uses it.
This commit is contained in:
Tyler Veness
2022-10-27 21:59:11 -07:00
committed by GitHub
parent 9e22ffbebf
commit 66157397c1
24 changed files with 264 additions and 1463 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java
View File

@@ -18,12 +18,12 @@ class DifferentialDriveTest {
assertEquals(1.0, speeds.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, false);
speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, false);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(0.5, speeds.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, false);
speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, false);
assertEquals(0.5, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
@@ -33,32 +33,32 @@ class DifferentialDriveTest {
assertEquals(-1.0, speeds.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, false);
speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, false);
assertEquals(-0.5, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, false);
speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, false);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(-0.5, speeds.right, 1e-9);
// Left turn (xSpeed with negative sign)
speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
// Left turn (xSpeed with positive sign)
speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
// Right turn (xSpeed with negative sign)
speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
// Right turn (xSpeed with positive sign)
speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
}
@@ -71,12 +71,12 @@ class DifferentialDriveTest {
assertEquals(1.0, speeds.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, true);
speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, true);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(0.25, speeds.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, true);
speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, true);
assertEquals(0.25, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
@@ -86,32 +86,32 @@ class DifferentialDriveTest {
assertEquals(-1.0, speeds.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, true);
speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, true);
assertEquals(-0.25, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, true);
speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, true);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(-0.25, speeds.right, 1e-9);
// Left turn (xSpeed with negative sign)
speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
// Left turn (xSpeed with positive sign)
speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
// Right turn (xSpeed with negative sign)
speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
// Right turn (xSpeed with positive sign)
speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
}
@@ -124,12 +124,12 @@ class DifferentialDriveTest {
assertEquals(1.0, speeds.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, false);
speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, false);
assertEquals(0.25, speeds.left, 1e-9);
assertEquals(0.75, speeds.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, false);
speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, false);
assertEquals(0.75, speeds.left, 1e-9);
assertEquals(0.25, speeds.right, 1e-9);
@@ -139,12 +139,12 @@ class DifferentialDriveTest {
assertEquals(-1.0, speeds.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, false);
speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, false);
assertEquals(-0.75, speeds.left, 1e-9);
assertEquals(-0.25, speeds.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, false);
speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, false);
assertEquals(-0.25, speeds.left, 1e-9);
assertEquals(-0.75, speeds.right, 1e-9);
}
@@ -157,12 +157,12 @@ class DifferentialDriveTest {
assertEquals(1.0, speeds.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, true);
speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, true);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, true);
speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, true);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
@@ -172,12 +172,12 @@ class DifferentialDriveTest {
assertEquals(-1.0, speeds.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, true);
speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, true);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, true);
speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, true);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
}
@@ -261,12 +261,12 @@ class DifferentialDriveTest {
assertEquals(1.0, right.get(), 1e-9);
// Forward left turn
drive.arcadeDrive(0.5, -0.5, false);
drive.arcadeDrive(0.5, 0.5, false);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(0.5, right.get(), 1e-9);
// Forward right turn
drive.arcadeDrive(0.5, 0.5, false);
drive.arcadeDrive(0.5, -0.5, false);
assertEquals(0.5, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
@@ -276,12 +276,12 @@ class DifferentialDriveTest {
assertEquals(-1.0, right.get(), 1e-9);
// Backward left turn
drive.arcadeDrive(-0.5, -0.5, false);
drive.arcadeDrive(-0.5, 0.5, false);
assertEquals(-0.5, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
// Backward right turn
drive.arcadeDrive(-0.5, 0.5, false);
drive.arcadeDrive(-0.5, -0.5, false);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(-0.5, right.get(), 1e-9);
}
@@ -299,12 +299,12 @@ class DifferentialDriveTest {
assertEquals(1.0, right.get(), 1e-9);
// Forward left turn
drive.arcadeDrive(0.5, -0.5, true);
drive.arcadeDrive(0.5, 0.5, true);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(0.25, right.get(), 1e-9);
// Forward right turn
drive.arcadeDrive(0.5, 0.5, true);
drive.arcadeDrive(0.5, -0.5, true);
assertEquals(0.25, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
@@ -314,12 +314,12 @@ class DifferentialDriveTest {
assertEquals(-1.0, right.get(), 1e-9);
// Backward left turn
drive.arcadeDrive(-0.5, -0.5, true);
drive.arcadeDrive(-0.5, 0.5, true);
assertEquals(-0.25, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
// Backward right turn
drive.arcadeDrive(-0.5, 0.5, true);
drive.arcadeDrive(-0.5, -0.5, true);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(-0.25, right.get(), 1e-9);
}
@@ -337,12 +337,12 @@ class DifferentialDriveTest {
assertEquals(1.0, right.get(), 1e-9);
// Forward left turn
drive.curvatureDrive(0.5, -0.5, false);
drive.curvatureDrive(0.5, 0.5, false);
assertEquals(0.25, left.get(), 1e-9);
assertEquals(0.75, right.get(), 1e-9);
// Forward right turn
drive.curvatureDrive(0.5, 0.5, false);
drive.curvatureDrive(0.5, -0.5, false);
assertEquals(0.75, left.get(), 1e-9);
assertEquals(0.25, right.get(), 1e-9);
@@ -352,12 +352,12 @@ class DifferentialDriveTest {
assertEquals(-1.0, right.get(), 1e-9);
// Backward left turn
drive.curvatureDrive(-0.5, -0.5, false);
drive.curvatureDrive(-0.5, 0.5, false);
assertEquals(-0.75, left.get(), 1e-9);
assertEquals(-0.25, right.get(), 1e-9);
// Backward right turn
drive.curvatureDrive(-0.5, 0.5, false);
drive.curvatureDrive(-0.5, -0.5, false);
assertEquals(-0.25, left.get(), 1e-9);
assertEquals(-0.75, right.get(), 1e-9);
}
@@ -375,12 +375,12 @@ class DifferentialDriveTest {
assertEquals(1.0, right.get(), 1e-9);
// Forward left turn
drive.curvatureDrive(0.5, -0.5, true);
drive.curvatureDrive(0.5, 0.5, true);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
// Forward right turn
drive.curvatureDrive(0.5, 0.5, true);
drive.curvatureDrive(0.5, -0.5, true);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
@@ -390,12 +390,12 @@ class DifferentialDriveTest {
assertEquals(-1.0, right.get(), 1e-9);
// Backward left turn
drive.curvatureDrive(-0.5, -0.5, true);
drive.curvatureDrive(-0.5, 0.5, true);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
// Backward right turn
drive.curvatureDrive(-0.5, 0.5, true);
drive.curvatureDrive(-0.5, -0.5, true);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
}

205
wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/KilloughDriveTest.java
View File

@@ -1,205 +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.
package edu.wpi.first.wpilibj.drive;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
import org.junit.jupiter.api.Test;
class KilloughDriveTest {
@Test
void testCartesianIK() {
var left = new MockMotorController();
var right = new MockMotorController();
var back = new MockMotorController();
var drive = new KilloughDrive(left, right, back);
// Forward
var speeds = drive.driveCartesianIK(1.0, 0.0, 0.0);
assertEquals(0.5, speeds.left, 1e-9);
assertEquals(-0.5, speeds.right, 1e-9);
assertEquals(0.0, speeds.back, 1e-9);
// Left
speeds = drive.driveCartesianIK(0.0, -1.0, 0.0);
assertEquals(-Math.sqrt(3) / 2, speeds.left, 1e-9);
assertEquals(-Math.sqrt(3) / 2, speeds.right, 1e-9);
assertEquals(1.0, speeds.back, 1e-9);
// Right
speeds = drive.driveCartesianIK(0.0, 1.0, 0.0);
assertEquals(Math.sqrt(3) / 2, speeds.left, 1e-9);
assertEquals(Math.sqrt(3) / 2, speeds.right, 1e-9);
assertEquals(-1.0, speeds.back, 1e-9);
// Rotate CCW
speeds = drive.driveCartesianIK(0.0, 0.0, -1.0);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
assertEquals(-1.0, speeds.back, 1e-9);
// Rotate CW
speeds = drive.driveCartesianIK(0.0, 0.0, 1.0);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
assertEquals(1.0, speeds.back, 1e-9);
}
@Test
void testCartesianIKGyro90CW() {
var left = new MockMotorController();
var right = new MockMotorController();
var back = new MockMotorController();
var drive = new KilloughDrive(left, right, back);
// Forward in global frame; left in robot frame
var speeds = drive.driveCartesianIK(1.0, 0.0, 0.0, 90.0);
assertEquals(-Math.sqrt(3) / 2, speeds.left, 1e-9);
assertEquals(-Math.sqrt(3) / 2, speeds.right, 1e-9);
assertEquals(1.0, speeds.back, 1e-9);
// Left in global frame; backward in robot frame
speeds = drive.driveCartesianIK(0.0, -1.0, 0.0, 90.0);
assertEquals(-0.5, speeds.left, 1e-9);
assertEquals(0.5, speeds.right, 1e-9);
assertEquals(0.0, speeds.back, 1e-9);
// Right in global frame; forward in robot frame
speeds = drive.driveCartesianIK(0.0, 1.0, 0.0, 90.0);
assertEquals(0.5, speeds.left, 1e-9);
assertEquals(-0.5, speeds.right, 1e-9);
assertEquals(0.0, speeds.back, 1e-9);
// Rotate CCW
speeds = drive.driveCartesianIK(0.0, 0.0, -1.0, 90.0);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
assertEquals(-1.0, speeds.back, 1e-9);
// Rotate CW
speeds = drive.driveCartesianIK(0.0, 0.0, 1.0, 90.0);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
assertEquals(1.0, speeds.back, 1e-9);
}
@Test
void testCartesian() {
var left = new MockMotorController();
var right = new MockMotorController();
var back = new MockMotorController();
var drive = new KilloughDrive(left, right, back);
drive.setDeadband(0.0);
// Forward
drive.driveCartesian(1.0, 0.0, 0.0);
assertEquals(0.5, left.get(), 1e-9);
assertEquals(-0.5, right.get(), 1e-9);
assertEquals(0.0, back.get(), 1e-9);
// Left
drive.driveCartesian(0.0, -1.0, 0.0);
assertEquals(-Math.sqrt(3) / 2, left.get(), 1e-9);
assertEquals(-Math.sqrt(3) / 2, right.get(), 1e-9);
assertEquals(1.0, back.get(), 1e-9);
// Right
drive.driveCartesian(0.0, 1.0, 0.0);
assertEquals(Math.sqrt(3) / 2, left.get(), 1e-9);
assertEquals(Math.sqrt(3) / 2, right.get(), 1e-9);
assertEquals(-1.0, back.get(), 1e-9);
// Rotate CCW
drive.driveCartesian(0.0, 0.0, -1.0);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-1.0, back.get(), 1e-9);
// Rotate CW
drive.driveCartesian(0.0, 0.0, 1.0);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(1.0, back.get(), 1e-9);
}
@Test
void testCartesianGyro90CW() {
var left = new MockMotorController();
var right = new MockMotorController();
var back = new MockMotorController();
var drive = new KilloughDrive(left, right, back);
drive.setDeadband(0.0);
// Forward in global frame; left in robot frame
drive.driveCartesian(1.0, 0.0, 0.0, 90.0);
assertEquals(-Math.sqrt(3) / 2, left.get(), 1e-9);
assertEquals(-Math.sqrt(3) / 2, right.get(), 1e-9);
assertEquals(1.0, back.get(), 1e-9);
// Left in global frame; backward in robot frame
drive.driveCartesian(0.0, -1.0, 0.0, 90.0);
assertEquals(-0.5, left.get(), 1e-9);
assertEquals(0.5, right.get(), 1e-9);
assertEquals(0.0, back.get(), 1e-9);
// Right in global frame; forward in robot frame
drive.driveCartesian(0.0, 1.0, 0.0, 90.0);
assertEquals(0.5, left.get(), 1e-9);
assertEquals(-0.5, right.get(), 1e-9);
assertEquals(0.0, back.get(), 1e-9);
// Rotate CCW
drive.driveCartesian(0.0, 0.0, -1.0, 90.0);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-1.0, back.get(), 1e-9);
// Rotate CW
drive.driveCartesian(0.0, 0.0, 1.0, 90.0);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(1.0, back.get(), 1e-9);
}
@Test
void testPolar() {
var left = new MockMotorController();
var right = new MockMotorController();
var back = new MockMotorController();
var drive = new KilloughDrive(left, right, back);
drive.setDeadband(0.0);
// Forward
drive.drivePolar(1.0, 0.0, 0.0);
assertEquals(Math.sqrt(3) / 2, left.get(), 1e-9);
assertEquals(Math.sqrt(3) / 2, right.get(), 1e-9);
assertEquals(-1.0, back.get(), 1e-9);
// Left
drive.drivePolar(1.0, -90.0, 0.0);
assertEquals(-0.5, left.get(), 1e-9);
assertEquals(0.5, right.get(), 1e-9);
assertEquals(0.0, back.get(), 1e-9);
// Right
drive.drivePolar(1.0, 90.0, 0.0);
assertEquals(0.5, left.get(), 1e-9);
assertEquals(-0.5, right.get(), 1e-9);
assertEquals(0.0, back.get(), 1e-9);
// Rotate CCW
drive.drivePolar(0.0, 0.0, -1.0);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-1.0, back.get(), 1e-9);
// Rotate CW
drive.drivePolar(0.0, 0.0, 1.0);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(1.0, back.get(), 1e-9);
}
}

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

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj.drive;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
import org.junit.jupiter.api.Test;
@@ -51,35 +52,35 @@ class MecanumDriveTest {
@Test
void testCartesianIKGyro90CW() {
// Forward in global frame; left in robot frame
var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, 90.0);
var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, Rotation2d.fromDegrees(90.0));
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, 90.0);
speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, Rotation2d.fromDegrees(90.0));
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, 90.0);
speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, Rotation2d.fromDegrees(90.0));
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, 90.0);
speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, Rotation2d.fromDegrees(90.0));
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, 90.0);
speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, Rotation2d.fromDegrees(90.0));
assertEquals(1.0, speeds.frontLeft, 1e-9);
assertEquals(-1.0, speeds.frontRight, 1e-9);
assertEquals(1.0, speeds.rearLeft, 1e-9);
@@ -141,35 +142,35 @@ class MecanumDriveTest {
drive.setDeadband(0.0);
// Forward in global frame; left in robot frame
drive.driveCartesian(1.0, 0.0, 0.0, 90.0);
drive.driveCartesian(1.0, 0.0, 0.0, Rotation2d.fromDegrees(90.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 in global frame; backward in robot frame
drive.driveCartesian(0.0, -1.0, 0.0, 90.0);
drive.driveCartesian(0.0, -1.0, 0.0, Rotation2d.fromDegrees(90.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 in global frame; forward in robot frame
drive.driveCartesian(0.0, 1.0, 0.0, 90.0);
drive.driveCartesian(0.0, 1.0, 0.0, Rotation2d.fromDegrees(90.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.driveCartesian(0.0, 0.0, -1.0, 90.0);
drive.driveCartesian(0.0, 0.0, -1.0, Rotation2d.fromDegrees(90.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.driveCartesian(0.0, 0.0, 1.0, 90.0);
drive.driveCartesian(0.0, 0.0, 1.0, Rotation2d.fromDegrees(90.0));
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(-1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);
@@ -186,35 +187,35 @@ class MecanumDriveTest {
drive.setDeadband(0.0);
// Forward
drive.drivePolar(1.0, 0.0, 0.0);
drive.drivePolar(1.0, Rotation2d.fromDegrees(0.0), 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, -90.0, 0.0);
drive.drivePolar(1.0, Rotation2d.fromDegrees(-90.0), 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, 90.0, 0.0);
drive.drivePolar(1.0, Rotation2d.fromDegrees(90.0), 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, 0.0, -1.0);
drive.drivePolar(0.0, Rotation2d.fromDegrees(0.0), -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, 0.0, 1.0);
drive.drivePolar(0.0, Rotation2d.fromDegrees(0.0), 1.0);
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(-1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);