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

[wpimath] Replace Speeds with Velocities (#8479)

Browse Source 
I left "free speed" alone since that's the technical term for it. In
general, velocity is a vector quantity, and speed is a magnitude (i.e.,
a strictly positive value).

This PR also replaces the speed verbiage in MotorController with duty
cycle.

Fixes #8423.
This commit is contained in:
Tyler Veness
2026-03-06 14:19:15 -08:00
committed by GitHub
parent 1e39f39128
commit 9bd9656871
594 changed files with 8073 additions and 7875 deletions
Download Patch File Download Diff File Expand all files Collapse all files

436
wpilibj/src/test/java/org/wpilib/drive/DifferentialDriveTest.java
View File

@@ -14,466 +14,466 @@ class DifferentialDriveTest {
@Test
void testArcadeDriveIK() {
// Forward
var speeds = DifferentialDrive.arcadeDriveIK(1.0, 0.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
var velocities = DifferentialDrive.arcadeDriveIK(1.0, 0.0, false);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, false);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(0.5, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(0.5, 0.5, false);
assertEquals(0.0, velocities.left, 1e-9);
assertEquals(0.5, velocities.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, false);
assertEquals(0.5, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(0.5, -0.5, false);
assertEquals(0.5, velocities.left, 1e-9);
assertEquals(0.0, velocities.right, 1e-9);
// Backward
speeds = DifferentialDrive.arcadeDriveIK(-1.0, 0.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(-1.0, 0.0, false);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, false);
assertEquals(-0.5, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, false);
assertEquals(-0.5, velocities.left, 1e-9);
assertEquals(0.0, velocities.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, false);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(-0.5, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, false);
assertEquals(0.0, velocities.left, 1e-9);
assertEquals(-0.5, velocities.right, 1e-9);
// Left turn (xSpeed with negative sign)
speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
// Left turn (xVelocity with negative sign)
velocities = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Left turn (xSpeed with positive sign)
speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
// Left turn (xVelocity with positive sign)
velocities = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Right turn (xSpeed with negative sign)
speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
// Right turn (xVelocity with negative sign)
velocities = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Right turn (xSpeed with positive sign)
speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
// Right turn (xVelocity with positive sign)
velocities = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
}
@Test
void testArcadeDriveIKSquared() {
// Forward
var speeds = DifferentialDrive.arcadeDriveIK(1.0, 0.0, true);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
var velocities = DifferentialDrive.arcadeDriveIK(1.0, 0.0, true);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.arcadeDriveIK(0.5, 0.5, true);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(0.25, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(0.5, 0.5, true);
assertEquals(0.0, velocities.left, 1e-9);
assertEquals(0.25, velocities.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.arcadeDriveIK(0.5, -0.5, true);
assertEquals(0.25, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(0.5, -0.5, true);
assertEquals(0.25, velocities.left, 1e-9);
assertEquals(0.0, velocities.right, 1e-9);
// Backward
speeds = DifferentialDrive.arcadeDriveIK(-1.0, 0.0, true);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(-1.0, 0.0, true);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, true);
assertEquals(-0.25, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(-0.5, 0.5, true);
assertEquals(-0.25, velocities.left, 1e-9);
assertEquals(0.0, velocities.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, true);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(-0.25, speeds.right, 1e-9);
velocities = DifferentialDrive.arcadeDriveIK(-0.5, -0.5, true);
assertEquals(0.0, velocities.left, 1e-9);
assertEquals(-0.25, velocities.right, 1e-9);
// Left turn (xSpeed with negative sign)
speeds = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
// Left turn (xVelocity with negative sign)
velocities = DifferentialDrive.arcadeDriveIK(-0.0, 1.0, false);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Left turn (xSpeed with positive sign)
speeds = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
// Left turn (xVelocity with positive sign)
velocities = DifferentialDrive.arcadeDriveIK(0.0, 1.0, false);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Right turn (xSpeed with negative sign)
speeds = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
// Right turn (xVelocity with negative sign)
velocities = DifferentialDrive.arcadeDriveIK(-0.0, -1.0, false);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Right turn (xSpeed with positive sign)
speeds = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
// Right turn (xVelocity with positive sign)
velocities = DifferentialDrive.arcadeDriveIK(0.0, -1.0, false);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
}
@Test
void testCurvatureDriveIK() {
// Forward
var speeds = DifferentialDrive.curvatureDriveIK(1.0, 0.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
var velocities = DifferentialDrive.curvatureDriveIK(1.0, 0.0, false);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, false);
assertEquals(0.25, speeds.left, 1e-9);
assertEquals(0.75, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(0.5, 0.5, false);
assertEquals(0.25, velocities.left, 1e-9);
assertEquals(0.75, velocities.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, false);
assertEquals(0.75, speeds.left, 1e-9);
assertEquals(0.25, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(0.5, -0.5, false);
assertEquals(0.75, velocities.left, 1e-9);
assertEquals(0.25, velocities.right, 1e-9);
// Backward
speeds = DifferentialDrive.curvatureDriveIK(-1.0, 0.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(-1.0, 0.0, false);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, false);
assertEquals(-0.75, speeds.left, 1e-9);
assertEquals(-0.25, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, false);
assertEquals(-0.75, velocities.left, 1e-9);
assertEquals(-0.25, velocities.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, false);
assertEquals(-0.25, speeds.left, 1e-9);
assertEquals(-0.75, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, false);
assertEquals(-0.25, velocities.left, 1e-9);
assertEquals(-0.75, velocities.right, 1e-9);
}
@Test
void testCurvatureDriveIKTurnInPlace() {
// Forward
var speeds = DifferentialDrive.curvatureDriveIK(1.0, 0.0, true);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
var velocities = DifferentialDrive.curvatureDriveIK(1.0, 0.0, true);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.curvatureDriveIK(0.5, 0.5, true);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(0.5, 0.5, true);
assertEquals(0.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.curvatureDriveIK(0.5, -0.5, true);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(0.5, -0.5, true);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(0.0, velocities.right, 1e-9);
// Backward
speeds = DifferentialDrive.curvatureDriveIK(-1.0, 0.0, true);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(-1.0, 0.0, true);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, true);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(0.0, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(-0.5, 0.5, true);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(0.0, velocities.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, true);
assertEquals(0.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.curvatureDriveIK(-0.5, -0.5, true);
assertEquals(0.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
}
@Test
void testTankDriveIK() {
// Forward
var speeds = DifferentialDrive.tankDriveIK(1.0, 1.0, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
var velocities = DifferentialDrive.tankDriveIK(1.0, 1.0, false);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.tankDriveIK(0.5, 1.0, false);
assertEquals(0.5, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(0.5, 1.0, false);
assertEquals(0.5, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.tankDriveIK(1.0, 0.5, false);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(0.5, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(1.0, 0.5, false);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(0.5, velocities.right, 1e-9);
// Backward
speeds = DifferentialDrive.tankDriveIK(-1.0, -1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(-1.0, -1.0, false);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.tankDriveIK(-0.5, -1.0, false);
assertEquals(-0.5, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(-0.5, -1.0, false);
assertEquals(-0.5, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.tankDriveIK(-0.5, 1.0, false);
assertEquals(-0.5, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(-0.5, 1.0, false);
assertEquals(-0.5, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
}
@Test
void testTankDriveIKSquared() {
// Forward
var speeds = DifferentialDrive.tankDriveIK(1.0, 1.0, true);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
var velocities = DifferentialDrive.tankDriveIK(1.0, 1.0, true);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Forward left turn
speeds = DifferentialDrive.tankDriveIK(0.5, 1.0, true);
assertEquals(0.25, speeds.left, 1e-9);
assertEquals(1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(0.5, 1.0, true);
assertEquals(0.25, velocities.left, 1e-9);
assertEquals(1.0, velocities.right, 1e-9);
// Forward right turn
speeds = DifferentialDrive.tankDriveIK(1.0, 0.5, true);
assertEquals(1.0, speeds.left, 1e-9);
assertEquals(0.25, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(1.0, 0.5, true);
assertEquals(1.0, velocities.left, 1e-9);
assertEquals(0.25, velocities.right, 1e-9);
// Backward
speeds = DifferentialDrive.tankDriveIK(-1.0, -1.0, true);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(-1.0, -1.0, true);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Backward left turn
speeds = DifferentialDrive.tankDriveIK(-0.5, -1.0, true);
assertEquals(-0.25, speeds.left, 1e-9);
assertEquals(-1.0, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(-0.5, -1.0, true);
assertEquals(-0.25, velocities.left, 1e-9);
assertEquals(-1.0, velocities.right, 1e-9);
// Backward right turn
speeds = DifferentialDrive.tankDriveIK(-1.0, -0.5, true);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-0.25, speeds.right, 1e-9);
velocities = DifferentialDrive.tankDriveIK(-1.0, -0.5, true);
assertEquals(-1.0, velocities.left, 1e-9);
assertEquals(-0.25, velocities.right, 1e-9);
}
@Test
void testArcadeDrive() {
var left = new MockPWMMotorController();
var right = new MockPWMMotorController();
var drive = new DifferentialDrive(left::set, right::set);
var drive = new DifferentialDrive(left::setDutyCycle, right::setDutyCycle);
drive.setDeadband(0.0);
// Forward
drive.arcadeDrive(1.0, 0.0, false);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(1.0, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
// Forward left turn
drive.arcadeDrive(0.5, 0.5, false);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(0.5, right.get(), 1e-9);
assertEquals(0.0, left.getDutyCycle(), 1e-9);
assertEquals(0.5, right.getDutyCycle(), 1e-9);
// Forward right turn
drive.arcadeDrive(0.5, -0.5, false);
assertEquals(0.5, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
assertEquals(0.5, left.getDutyCycle(), 1e-9);
assertEquals(0.0, right.getDutyCycle(), 1e-9);
// Backward
drive.arcadeDrive(-1.0, 0.0, false);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-1.0, left.getDutyCycle(), 1e-9);
assertEquals(-1.0, right.getDutyCycle(), 1e-9);
// Backward left turn
drive.arcadeDrive(-0.5, 0.5, false);
assertEquals(-0.5, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
assertEquals(-0.5, left.getDutyCycle(), 1e-9);
assertEquals(0.0, right.getDutyCycle(), 1e-9);
// Backward right turn
drive.arcadeDrive(-0.5, -0.5, false);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(-0.5, right.get(), 1e-9);
assertEquals(0.0, left.getDutyCycle(), 1e-9);
assertEquals(-0.5, right.getDutyCycle(), 1e-9);
}
@Test
void testArcadeDriveSquared() {
var left = new MockPWMMotorController();
var right = new MockPWMMotorController();
var drive = new DifferentialDrive(left::set, right::set);
var drive = new DifferentialDrive(left::setDutyCycle, right::setDutyCycle);
drive.setDeadband(0.0);
// Forward
drive.arcadeDrive(1.0, 0.0, true);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(1.0, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
// Forward left turn
drive.arcadeDrive(0.5, 0.5, true);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(0.25, right.get(), 1e-9);
assertEquals(0.0, left.getDutyCycle(), 1e-9);
assertEquals(0.25, right.getDutyCycle(), 1e-9);
// Forward right turn
drive.arcadeDrive(0.5, -0.5, true);
assertEquals(0.25, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
assertEquals(0.25, left.getDutyCycle(), 1e-9);
assertEquals(0.0, right.getDutyCycle(), 1e-9);
// Backward
drive.arcadeDrive(-1.0, 0.0, true);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-1.0, left.getDutyCycle(), 1e-9);
assertEquals(-1.0, right.getDutyCycle(), 1e-9);
// Backward left turn
drive.arcadeDrive(-0.5, 0.5, true);
assertEquals(-0.25, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
assertEquals(-0.25, left.getDutyCycle(), 1e-9);
assertEquals(0.0, right.getDutyCycle(), 1e-9);
// Backward right turn
drive.arcadeDrive(-0.5, -0.5, true);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(-0.25, right.get(), 1e-9);
assertEquals(0.0, left.getDutyCycle(), 1e-9);
assertEquals(-0.25, right.getDutyCycle(), 1e-9);
}
@Test
void testCurvatureDrive() {
var left = new MockPWMMotorController();
var right = new MockPWMMotorController();
var drive = new DifferentialDrive(left::set, right::set);
var drive = new DifferentialDrive(left::setDutyCycle, right::setDutyCycle);
drive.setDeadband(0.0);
// Forward
drive.curvatureDrive(1.0, 0.0, false);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(1.0, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
// Forward left turn
drive.curvatureDrive(0.5, 0.5, false);
assertEquals(0.25, left.get(), 1e-9);
assertEquals(0.75, right.get(), 1e-9);
assertEquals(0.25, left.getDutyCycle(), 1e-9);
assertEquals(0.75, right.getDutyCycle(), 1e-9);
// Forward right turn
drive.curvatureDrive(0.5, -0.5, false);
assertEquals(0.75, left.get(), 1e-9);
assertEquals(0.25, right.get(), 1e-9);
assertEquals(0.75, left.getDutyCycle(), 1e-9);
assertEquals(0.25, right.getDutyCycle(), 1e-9);
// Backward
drive.curvatureDrive(-1.0, 0.0, false);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-1.0, left.getDutyCycle(), 1e-9);
assertEquals(-1.0, right.getDutyCycle(), 1e-9);
// Backward left turn
drive.curvatureDrive(-0.5, 0.5, false);
assertEquals(-0.75, left.get(), 1e-9);
assertEquals(-0.25, right.get(), 1e-9);
assertEquals(-0.75, left.getDutyCycle(), 1e-9);
assertEquals(-0.25, right.getDutyCycle(), 1e-9);
// Backward right turn
drive.curvatureDrive(-0.5, -0.5, false);
assertEquals(-0.25, left.get(), 1e-9);
assertEquals(-0.75, right.get(), 1e-9);
assertEquals(-0.25, left.getDutyCycle(), 1e-9);
assertEquals(-0.75, right.getDutyCycle(), 1e-9);
}
@Test
void testCurvatureDriveTurnInPlace() {
var left = new MockPWMMotorController();
var right = new MockPWMMotorController();
var drive = new DifferentialDrive(left::set, right::set);
var drive = new DifferentialDrive(left::setDutyCycle, right::setDutyCycle);
drive.setDeadband(0.0);
// Forward
drive.curvatureDrive(1.0, 0.0, true);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(1.0, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
// Forward left turn
drive.curvatureDrive(0.5, 0.5, true);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(0.0, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
// Forward right turn
drive.curvatureDrive(0.5, -0.5, true);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
assertEquals(1.0, left.getDutyCycle(), 1e-9);
assertEquals(0.0, right.getDutyCycle(), 1e-9);
// Backward
drive.curvatureDrive(-1.0, 0.0, true);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-1.0, left.getDutyCycle(), 1e-9);
assertEquals(-1.0, right.getDutyCycle(), 1e-9);
// Backward left turn
drive.curvatureDrive(-0.5, 0.5, true);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(0.0, right.get(), 1e-9);
assertEquals(-1.0, left.getDutyCycle(), 1e-9);
assertEquals(0.0, right.getDutyCycle(), 1e-9);
// Backward right turn
drive.curvatureDrive(-0.5, -0.5, true);
assertEquals(0.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(0.0, left.getDutyCycle(), 1e-9);
assertEquals(-1.0, right.getDutyCycle(), 1e-9);
}
@Test
void testTankDrive() {
var left = new MockPWMMotorController();
var right = new MockPWMMotorController();
var drive = new DifferentialDrive(left::set, right::set);
var drive = new DifferentialDrive(left::setDutyCycle, right::setDutyCycle);
drive.setDeadband(0.0);
// Forward
drive.tankDrive(1.0, 1.0, false);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(1.0, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
// Forward left turn
drive.tankDrive(0.5, 1.0, false);
assertEquals(0.5, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(0.5, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
// Forward right turn
drive.tankDrive(1.0, 0.5, false);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(0.5, right.get(), 1e-9);
assertEquals(1.0, left.getDutyCycle(), 1e-9);
assertEquals(0.5, right.getDutyCycle(), 1e-9);
// Backward
drive.tankDrive(-1.0, -1.0, false);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-1.0, left.getDutyCycle(), 1e-9);
assertEquals(-1.0, right.getDutyCycle(), 1e-9);
// Backward left turn
drive.tankDrive(-0.5, -1.0, false);
assertEquals(-0.5, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-0.5, left.getDutyCycle(), 1e-9);
assertEquals(-1.0, right.getDutyCycle(), 1e-9);
// Backward right turn
drive.tankDrive(-0.5, 1.0, false);
assertEquals(-0.5, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(-0.5, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
}
@Test
void testTankDriveSquared() {
var left = new MockPWMMotorController();
var right = new MockPWMMotorController();
var drive = new DifferentialDrive(left::set, right::set);
var drive = new DifferentialDrive(left::setDutyCycle, right::setDutyCycle);
drive.setDeadband(0.0);
// Forward
drive.tankDrive(1.0, 1.0, true);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(1.0, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
// Forward left turn
drive.tankDrive(0.5, 1.0, true);
assertEquals(0.25, left.get(), 1e-9);
assertEquals(1.0, right.get(), 1e-9);
assertEquals(0.25, left.getDutyCycle(), 1e-9);
assertEquals(1.0, right.getDutyCycle(), 1e-9);
// Forward right turn
drive.tankDrive(1.0, 0.5, true);
assertEquals(1.0, left.get(), 1e-9);
assertEquals(0.25, right.get(), 1e-9);
assertEquals(1.0, left.getDutyCycle(), 1e-9);
assertEquals(0.25, right.getDutyCycle(), 1e-9);
// Backward
drive.tankDrive(-1.0, -1.0, true);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-1.0, left.getDutyCycle(), 1e-9);
assertEquals(-1.0, right.getDutyCycle(), 1e-9);
// Backward left turn
drive.tankDrive(-0.5, -1.0, true);
assertEquals(-0.25, left.get(), 1e-9);
assertEquals(-1.0, right.get(), 1e-9);
assertEquals(-0.25, left.getDutyCycle(), 1e-9);
assertEquals(-1.0, right.getDutyCycle(), 1e-9);
// Backward right turn
drive.tankDrive(-1.0, -0.5, true);
assertEquals(-1.0, left.get(), 1e-9);
assertEquals(-0.25, right.get(), 1e-9);
assertEquals(-1.0, left.getDutyCycle(), 1e-9);
assertEquals(-0.25, right.getDutyCycle(), 1e-9);
}
}

229
wpilibj/src/test/java/org/wpilib/drive/MecanumDriveTest.java
View File

@@ -15,77 +15,77 @@ class MecanumDriveTest {
@Test
void testCartesianIK() {
// Forward
var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.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);
var velocities = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0);
assertEquals(1.0, velocities.frontLeft, 1e-9);
assertEquals(1.0, velocities.frontRight, 1e-9);
assertEquals(1.0, velocities.rearLeft, 1e-9);
assertEquals(1.0, velocities.rearRight, 1e-9);
// Left
speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.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);
velocities = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0);
assertEquals(-1.0, velocities.frontLeft, 1e-9);
assertEquals(1.0, velocities.frontRight, 1e-9);
assertEquals(1.0, velocities.rearLeft, 1e-9);
assertEquals(-1.0, velocities.rearRight, 1e-9);
// Right
speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.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);
velocities = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0);
assertEquals(1.0, velocities.frontLeft, 1e-9);
assertEquals(-1.0, velocities.frontRight, 1e-9);
assertEquals(-1.0, velocities.rearLeft, 1e-9);
assertEquals(1.0, velocities.rearRight, 1e-9);
// Rotate CCW
speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.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);
velocities = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0);
assertEquals(-1.0, velocities.frontLeft, 1e-9);
assertEquals(1.0, velocities.frontRight, 1e-9);
assertEquals(-1.0, velocities.rearLeft, 1e-9);
assertEquals(1.0, velocities.rearRight, 1e-9);
// Rotate CW
speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.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);
velocities = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0);
assertEquals(1.0, velocities.frontLeft, 1e-9);
assertEquals(-1.0, velocities.frontRight, 1e-9);
assertEquals(1.0, velocities.rearLeft, 1e-9);
assertEquals(-1.0, velocities.rearRight, 1e-9);
}
@Test
void testCartesianIKGyro90CW() {
// Forward in global frame; left in robot frame
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);
var velocities = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, Rotation2d.kCCW_Pi_2);
assertEquals(-1.0, velocities.frontLeft, 1e-9);
assertEquals(1.0, velocities.frontRight, 1e-9);
assertEquals(1.0, velocities.rearLeft, 1e-9);
assertEquals(-1.0, velocities.rearRight, 1e-9);
// Left in global frame; backward in robot frame
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);
velocities = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, Rotation2d.kCCW_Pi_2);
assertEquals(-1.0, velocities.frontLeft, 1e-9);
assertEquals(-1.0, velocities.frontRight, 1e-9);
assertEquals(-1.0, velocities.rearLeft, 1e-9);
assertEquals(-1.0, velocities.rearRight, 1e-9);
// Right in global frame; forward in robot frame
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);
velocities = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, Rotation2d.kCCW_Pi_2);
assertEquals(1.0, velocities.frontLeft, 1e-9);
assertEquals(1.0, velocities.frontRight, 1e-9);
assertEquals(1.0, velocities.rearLeft, 1e-9);
assertEquals(1.0, velocities.rearRight, 1e-9);
// Rotate CCW
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);
velocities = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, Rotation2d.kCCW_Pi_2);
assertEquals(-1.0, velocities.frontLeft, 1e-9);
assertEquals(1.0, velocities.frontRight, 1e-9);
assertEquals(-1.0, velocities.rearLeft, 1e-9);
assertEquals(1.0, velocities.rearRight, 1e-9);
// Rotate CW
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);
velocities = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, Rotation2d.kCCW_Pi_2);
assertEquals(1.0, velocities.frontLeft, 1e-9);
assertEquals(-1.0, velocities.frontRight, 1e-9);
assertEquals(1.0, velocities.rearLeft, 1e-9);
assertEquals(-1.0, velocities.rearRight, 1e-9);
}
@Test
@@ -94,43 +94,44 @@ class MecanumDriveTest {
var rl = new MockPWMMotorController();
var fr = new MockPWMMotorController();
var rr = new MockPWMMotorController();
var drive = new MecanumDrive(fl::set, rl::set, fr::set, rr::set);
var drive =
new MecanumDrive(fl::setDutyCycle, rl::setDutyCycle, fr::setDutyCycle, rr::setDutyCycle);
drive.setDeadband(0.0);
// Forward
drive.driveCartesian(1.0, 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);
assertEquals(1.0, fl.getDutyCycle(), 1e-9);
assertEquals(1.0, fr.getDutyCycle(), 1e-9);
assertEquals(1.0, rl.getDutyCycle(), 1e-9);
assertEquals(1.0, rr.getDutyCycle(), 1e-9);
// Left
drive.driveCartesian(0.0, -1.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);
assertEquals(-1.0, fl.getDutyCycle(), 1e-9);
assertEquals(1.0, fr.getDutyCycle(), 1e-9);
assertEquals(1.0, rl.getDutyCycle(), 1e-9);
assertEquals(-1.0, rr.getDutyCycle(), 1e-9);
// Right
drive.driveCartesian(0.0, 1.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);
assertEquals(1.0, fl.getDutyCycle(), 1e-9);
assertEquals(-1.0, fr.getDutyCycle(), 1e-9);
assertEquals(-1.0, rl.getDutyCycle(), 1e-9);
assertEquals(1.0, rr.getDutyCycle(), 1e-9);
// Rotate CCW
drive.driveCartesian(0.0, 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);
assertEquals(-1.0, fl.getDutyCycle(), 1e-9);
assertEquals(1.0, fr.getDutyCycle(), 1e-9);
assertEquals(-1.0, rl.getDutyCycle(), 1e-9);
assertEquals(1.0, rr.getDutyCycle(), 1e-9);
// Rotate CW
drive.driveCartesian(0.0, 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);
assertEquals(1.0, fl.getDutyCycle(), 1e-9);
assertEquals(-1.0, fr.getDutyCycle(), 1e-9);
assertEquals(1.0, rl.getDutyCycle(), 1e-9);
assertEquals(-1.0, rr.getDutyCycle(), 1e-9);
}
@Test
@@ -139,43 +140,44 @@ class MecanumDriveTest {
var rl = new MockPWMMotorController();
var fr = new MockPWMMotorController();
var rr = new MockPWMMotorController();
var drive = new MecanumDrive(fl::set, rl::set, fr::set, rr::set);
var drive =
new MecanumDrive(fl::setDutyCycle, rl::setDutyCycle, fr::setDutyCycle, rr::setDutyCycle);
drive.setDeadband(0.0);
// Forward in global frame; left in robot frame
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);
assertEquals(-1.0, fl.getDutyCycle(), 1e-9);
assertEquals(1.0, fr.getDutyCycle(), 1e-9);
assertEquals(1.0, rl.getDutyCycle(), 1e-9);
assertEquals(-1.0, rr.getDutyCycle(), 1e-9);
// Left in global frame; backward in robot frame
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);
assertEquals(-1.0, fl.getDutyCycle(), 1e-9);
assertEquals(-1.0, fr.getDutyCycle(), 1e-9);
assertEquals(-1.0, rl.getDutyCycle(), 1e-9);
assertEquals(-1.0, rr.getDutyCycle(), 1e-9);
// Right in global frame; forward in robot frame
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);
assertEquals(1.0, fl.getDutyCycle(), 1e-9);
assertEquals(1.0, fr.getDutyCycle(), 1e-9);
assertEquals(1.0, rl.getDutyCycle(), 1e-9);
assertEquals(1.0, rr.getDutyCycle(), 1e-9);
// Rotate CCW
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);
assertEquals(-1.0, fl.getDutyCycle(), 1e-9);
assertEquals(1.0, fr.getDutyCycle(), 1e-9);
assertEquals(-1.0, rl.getDutyCycle(), 1e-9);
assertEquals(1.0, rr.getDutyCycle(), 1e-9);
// Rotate CW
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);
assertEquals(1.0, fl.getDutyCycle(), 1e-9);
assertEquals(-1.0, fr.getDutyCycle(), 1e-9);
assertEquals(1.0, rl.getDutyCycle(), 1e-9);
assertEquals(-1.0, rr.getDutyCycle(), 1e-9);
}
@Test
@@ -184,42 +186,43 @@ class MecanumDriveTest {
var rl = new MockPWMMotorController();
var fr = new MockPWMMotorController();
var rr = new MockPWMMotorController();
var drive = new MecanumDrive(fl::set, rl::set, fr::set, rr::set);
var drive =
new MecanumDrive(fl::setDutyCycle, rl::setDutyCycle, fr::setDutyCycle, rr::setDutyCycle);
drive.setDeadband(0.0);
// Forward
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);
assertEquals(1.0, fl.getDutyCycle(), 1e-9);
assertEquals(1.0, fr.getDutyCycle(), 1e-9);
assertEquals(1.0, rl.getDutyCycle(), 1e-9);
assertEquals(1.0, rr.getDutyCycle(), 1e-9);
// Left
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);
assertEquals(-1.0, fl.getDutyCycle(), 1e-9);
assertEquals(1.0, fr.getDutyCycle(), 1e-9);
assertEquals(1.0, rl.getDutyCycle(), 1e-9);
assertEquals(-1.0, rr.getDutyCycle(), 1e-9);
// Right
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);
assertEquals(1.0, fl.getDutyCycle(), 1e-9);
assertEquals(-1.0, fr.getDutyCycle(), 1e-9);
assertEquals(-1.0, rl.getDutyCycle(), 1e-9);
assertEquals(1.0, rr.getDutyCycle(), 1e-9);
// Rotate CCW
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);
assertEquals(-1.0, fl.getDutyCycle(), 1e-9);
assertEquals(1.0, fr.getDutyCycle(), 1e-9);
assertEquals(-1.0, rl.getDutyCycle(), 1e-9);
assertEquals(1.0, rr.getDutyCycle(), 1e-9);
// Rotate CW
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);
assertEquals(1.0, fl.getDutyCycle(), 1e-9);
assertEquals(-1.0, fr.getDutyCycle(), 1e-9);
assertEquals(1.0, rl.getDutyCycle(), 1e-9);
assertEquals(-1.0, rr.getDutyCycle(), 1e-9);
}
}

16
wpilibj/src/test/java/org/wpilib/hardware/led/LEDPatternTest.java
View File

@@ -220,7 +220,7 @@ class LEDPatternTest {
};
// scroll forwards 1/256th (1 LED) per microsecond - this makes mock time easier
var scroll = base.scrollAtRelativeSpeed(Value.per(Microsecond).of(1 / 256.0));
var scroll = base.scrollAtRelativeVelocity(Value.per(Microsecond).of(1 / 256.0));
for (int time = 0; time < 500; time++) {
m_mockTime = time;
@@ -254,7 +254,7 @@ class LEDPatternTest {
};
// scroll backwards 1/256th (1 LED) per microsecond - this makes mock time easier
var scroll = base.scrollAtRelativeSpeed(Value.per(Microsecond).of(-1 / 256.0));
var scroll = base.scrollAtRelativeVelocity(Value.per(Microsecond).of(-1 / 256.0));
for (int time = 0; time < 500; time++) {
m_mockTime = time;
@@ -277,7 +277,7 @@ class LEDPatternTest {
}
@Test
void scrollAbsoluteSpeedForward() {
void scrollAbsoluteVelocityForward() {
var buffer = new AddressableLEDBuffer(256);
LEDPattern base =
@@ -290,7 +290,7 @@ class LEDPatternTest {
// scroll at 16 m/s, LED spacing = 2cm
// buffer is 256 LEDs, so total length = 512cm = 5.12m
// scrolling at 16 m/s yields a period of 0.32 seconds, or 0.00125 seconds per LED (800 LEDs/s)
var scroll = base.scrollAtAbsoluteSpeed(MetersPerSecond.of(16), Centimeters.of(2));
var scroll = base.scrollAtAbsoluteVelocity(MetersPerSecond.of(16), Centimeters.of(2));
for (int time = 0; time < 500; time++) {
m_mockTime = time * 1_250; // 1.25ms per LED
@@ -313,7 +313,7 @@ class LEDPatternTest {
}
@Test
void scrollAbsoluteSpeedBackward() {
void scrollAbsoluteVelocityBackward() {
var buffer = new AddressableLEDBuffer(256);
LEDPattern base =
@@ -326,7 +326,7 @@ class LEDPatternTest {
// scroll at 16 m/s, LED spacing = 2cm
// buffer is 256 LEDs, so total length = 512cm = 5.12m
// scrolling at 16 m/s yields a period of 0.32 seconds, or 0.00125 seconds per LED (800 LEDs/s)
var scroll = base.scrollAtAbsoluteSpeed(MetersPerSecond.of(-16), Centimeters.of(2));
var scroll = base.scrollAtAbsoluteVelocity(MetersPerSecond.of(-16), Centimeters.of(2));
for (int time = 0; time < 500; time++) {
m_mockTime = time * 1_250; // 1.25ms per LED
@@ -867,7 +867,7 @@ class LEDPatternTest {
var pattern =
LEDPattern.steps(Map.of(0, kRed, 0.25, kBlue, 0.5, kYellow, 0.75, kGreen))
.mask(LEDPattern.steps(Map.of(0, kWhite, 0.5, kBlack)))
.scrollAtRelativeSpeed(Percent.per(Microsecond).of(12.5));
.scrollAtRelativeVelocity(Percent.per(Microsecond).of(12.5));
var buffer = new AddressableLEDBuffer(8);
{
@@ -932,7 +932,7 @@ class LEDPatternTest {
var pattern =
LEDPattern.steps(Map.of(0, kRed, 0.25, kBlue, 0.5, kYellow, 0.75, kGreen))
.mask(LEDPattern.steps(Map.of(0, kWhite, 0.5, kBlack)))
.scrollAtAbsoluteSpeed(Meters.per(Microsecond).of(1), Meters.one());
.scrollAtAbsoluteVelocity(Meters.per(Microsecond).of(1), Meters.one());
var buffer = new AddressableLEDBuffer(8);
{

12
wpilibj/src/test/java/org/wpilib/hardware/motor/MockMotorController.java
View File

@@ -6,17 +6,17 @@ package org.wpilib.hardware.motor;
@SuppressWarnings("removal")
public class MockMotorController implements MotorController {
private double m_speed;
private double m_dutyCycle;
private boolean m_isInverted;
@Override
public void set(double speed) {
m_speed = m_isInverted ? -speed : speed;
public void setDutyCycle(double dutyCycle) {
m_dutyCycle = m_isInverted ? -dutyCycle : dutyCycle;
}
@Override
public double get() {
return m_speed;
public double getDutyCycle() {
return m_dutyCycle;
}
@Override
@@ -31,6 +31,6 @@ public class MockMotorController implements MotorController {
@Override
public void disable() {
m_speed = 0;
m_dutyCycle = 0;
}
}

12
wpilibj/src/test/java/org/wpilib/hardware/motor/MockPWMMotorController.java
View File

@@ -5,15 +5,15 @@
package org.wpilib.hardware.motor;
public class MockPWMMotorController {
private double m_speed;
private double m_dutyCycle;
private boolean m_isInverted;
public void set(double speed) {
m_speed = m_isInverted ? -speed : speed;
public void setDutyCycle(double dutyCycle) {
m_dutyCycle = m_isInverted ? -dutyCycle : dutyCycle;
}
public double get() {
return m_speed;
public double getDutyCycle() {
return m_dutyCycle;
}
public void setInverted(boolean isInverted) {
@@ -25,7 +25,7 @@ public class MockPWMMotorController {
}
public void disable() {
m_speed = 0;
m_dutyCycle = 0;
}
public void stopMotor() {

8
wpilibj/src/test/java/org/wpilib/simulation/DCMotorSimTest.java
View File

@@ -38,7 +38,7 @@ class DCMotorSimTest {
// ------ SimulationPeriodic() happens after user code -------
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw()));
sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
sim.setInputVoltage(motor.getDutyCycle() * RobotController.getBatteryVoltage());
sim.update(0.020);
encoderSim.setRate(sim.getAngularVelocity());
}
@@ -51,7 +51,7 @@ class DCMotorSimTest {
// ------ SimulationPeriodic() happens after user code -------
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw()));
sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
sim.setInputVoltage(motor.getDutyCycle() * RobotController.getBatteryVoltage());
sim.update(0.020);
encoderSim.setRate(sim.getAngularVelocity());
}
@@ -76,12 +76,12 @@ class DCMotorSimTest {
encoderSim.resetData();
for (int i = 0; i < 140; i++) {
motor.set(controller.calculate(encoder.getDistance(), 750));
motor.setDutyCycle(controller.calculate(encoder.getDistance(), 750));
// ------ SimulationPeriodic() happens after user code -------
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw()));
sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
sim.setInputVoltage(motor.getDutyCycle() * RobotController.getBatteryVoltage());
sim.update(0.020);
encoderSim.setDistance(sim.getAngularPosition());
encoderSim.setRate(sim.getAngularVelocity());

5
wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java
View File

@@ -65,9 +65,10 @@ class DifferentialDrivetrainSimTest {
var state = traj.sample(t);
var feedbackOut = feedback.calculate(sim.getPose(), state);
var wheelSpeeds = kinematics.toWheelSpeeds(feedbackOut);
var wheelVelocities = kinematics.toWheelVelocities(feedbackOut);
var voltages = feedforward.calculate(VecBuilder.fill(wheelSpeeds.left, wheelSpeeds.right));
var voltages =
feedforward.calculate(VecBuilder.fill(wheelVelocities.left, wheelVelocities.right));
// Sim periodic code
sim.setInputs(voltages.get(0, 0), voltages.get(1, 0));

4
wpilibj/src/test/java/org/wpilib/simulation/ElevatorSimTest.java
View File

@@ -48,11 +48,11 @@ class ElevatorSimTest {
double nextVoltage = controller.calculate(encoderSim.getDistance());
double currentBatteryVoltage = RobotController.getBatteryVoltage();
motor.set(nextVoltage / currentBatteryVoltage);
motor.setDutyCycle(nextVoltage / currentBatteryVoltage);
// ------ SimulationPeriodic() happens after user code -------
var u = VecBuilder.fill(motor.get() * currentBatteryVoltage);
var u = VecBuilder.fill(motor.getDutyCycle() * currentBatteryVoltage);
sim.setInput(u);
sim.update(0.020);
var y = sim.getOutput();

12
wpilibj/src/test/java/org/wpilib/simulation/PWMMotorControllerSimTest.java
View File

@@ -18,14 +18,14 @@ class PWMMotorControllerSimTest {
try (Spark spark = new Spark(0)) {
PWMMotorControllerSim sim = new PWMMotorControllerSim(spark);
spark.set(0);
assertEquals(0, sim.getSpeed());
spark.setDutyCycle(0);
assertEquals(0, sim.getDutyCycle());
spark.set(0.354);
assertEquals(0.354, sim.getSpeed());
spark.setDutyCycle(0.354);
assertEquals(0.354, sim.getDutyCycle());
spark.set(-0.785);
assertEquals(-0.785, sim.getSpeed());
spark.setDutyCycle(-0.785);
assertEquals(-0.785, sim.getDutyCycle());
}
}
}