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] Check for signedness in ArcadeDriveIK() (#4028)

Browse Source 
If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the
third quadrant instead of the first since +0.0 == -0.0.

Also added tests for inverse kinematic functions, fixed some
MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK()
and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle
that C++ already had.

Fixes #4022.
This commit is contained in:
Tyler Veness
2022-02-17 18:03:59 -08:00
committed by GitHub
parent a19d1133b1
commit 49adac9564
10 changed files with 844 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,6 +10,244 @@ import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
import org.junit.jupiter.api.Test;
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);
// 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);
// 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);
// Backward
speeds = DifferentialDrive.arcadeDriveIK(-1.0, 0.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.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);
// 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);
// 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 (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 (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 (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);
}
@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);
// 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);
// 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);
// Backward
speeds = DifferentialDrive.arcadeDriveIK(-1.0, 0.0, true);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.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);
// 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);
// 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 (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 (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 (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);
}
@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);
// 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);
// 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);
// Backward
speeds = DifferentialDrive.curvatureDriveIK(-1.0, 0.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.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);
// 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);
}
@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);
// 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);
// 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);
// Backward
speeds = DifferentialDrive.curvatureDriveIK(-1.0, 0.0, true);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.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);
// 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);
}
@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);
// 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);
// 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);
// Backward
speeds = DifferentialDrive.tankDriveIK(-1.0, -1.0, false);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.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);
// 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);
}
@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);
// 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);
// 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);
// Backward
speeds = DifferentialDrive.tankDriveIK(-1.0, -1.0, true);
assertEquals(-1.0, speeds.left, 1e-9);
assertEquals(-1.0, speeds.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);
// 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);
}
@Test
void testArcadeDrive() {
var left = new MockMotorController();

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

@@ -10,6 +10,82 @@ 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();
@@ -50,7 +126,7 @@ class KilloughDriveTest {
}
@Test
void testCartesiangyro90CW() {
void testCartesianGyro90CW() {
var left = new MockMotorController();
var right = new MockMotorController();
var back = new MockMotorController();

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

@@ -10,13 +10,89 @@ import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
import org.junit.jupiter.api.Test;
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);
// 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);
// 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);
// 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);
// 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);
}
@Test
void testCartesianIKGyro90CW() {
// Forward in global frame; left in robot frame
var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, 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);
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);
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);
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);
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);
}
@Test
void testCartesian() {
var fl = new MockMotorController();
var fr = new MockMotorController();
var rl = new MockMotorController();
var rr = new MockMotorController();
var drive = new MecanumDrive(fl, fr, rl, rr);
var drive = new MecanumDrive(fl, rl, fr, rr);
drive.setDeadband(0.0);
// Forward
@@ -43,25 +119,25 @@ class MecanumDriveTest {
// 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, 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);
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(1.0, fr.get(), 1e-9);
assertEquals(-1.0, rl.get(), 1e-9);
assertEquals(-1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);
assertEquals(-1.0, rr.get(), 1e-9);
}
@Test
void testCartesiangyro90CW() {
void testCartesianGyro90CW() {
var fl = new MockMotorController();
var fr = new MockMotorController();
var rl = new MockMotorController();
var rr = new MockMotorController();
var drive = new MecanumDrive(fl, fr, rl, rr);
var drive = new MecanumDrive(fl, rl, fr, rr);
drive.setDeadband(0.0);
// Forward in global frame; left in robot frame
@@ -88,15 +164,15 @@ class MecanumDriveTest {
// Rotate CCW
drive.driveCartesian(0.0, 0.0, -1.0, 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, 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);
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(1.0, fr.get(), 1e-9);
assertEquals(-1.0, rl.get(), 1e-9);
assertEquals(-1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);
assertEquals(-1.0, rr.get(), 1e-9);
}
@@ -106,7 +182,7 @@ class MecanumDriveTest {
var fr = new MockMotorController();
var rl = new MockMotorController();
var rr = new MockMotorController();
var drive = new MecanumDrive(fl, fr, rl, rr);
var drive = new MecanumDrive(fl, rl, fr, rr);
drive.setDeadband(0.0);
// Forward
@@ -133,15 +209,15 @@ class MecanumDriveTest {
// Rotate CCW
drive.drivePolar(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, 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);
assertEquals(1.0, fl.get(), 1e-9);
assertEquals(1.0, fr.get(), 1e-9);
assertEquals(-1.0, rl.get(), 1e-9);
assertEquals(-1.0, fr.get(), 1e-9);
assertEquals(1.0, rl.get(), 1e-9);
assertEquals(-1.0, rr.get(), 1e-9);
}
}