Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +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

232
wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
View File

@@ -6,6 +6,238 @@
#include "frc/drive/DifferentialDrive.h"
#include "gtest/gtest.h"
TEST(DifferentialDriveTest, ArcadeDriveIK) {
// Forward
auto speeds = frc::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(0.5, speeds.right);
// Forward right turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.5, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward
speeds = frc::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward right turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-0.5, speeds.right);
// Left turn (xSpeed with negative sign)
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Left turn (xSpeed with positive sign)
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Right turn (xSpeed with negative sign)
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Right turn (xSpeed with positive sign)
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
}
TEST(DifferentialDriveTest, ArcadeDriveIKSquared) {
// Forward
auto speeds = frc::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
// Forward right turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward
speeds = frc::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward right turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-0.25, speeds.right);
// Left turn (xSpeed with negative sign)
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Left turn (xSpeed with positive sign)
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Right turn (xSpeed with negative sign)
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Right turn (xSpeed with positive sign)
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
}
TEST(DifferentialDriveTest, CurvatureDriveIK) {
// Forward
auto speeds = frc::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.75, speeds.right);
// Forward right turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.75, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
// Backward
speeds = frc::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(-0.75, speeds.left);
EXPECT_DOUBLE_EQ(-0.25, speeds.right);
// Backward right turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(-0.75, speeds.right);
}
TEST(DifferentialDriveTest, CurvatureDriveIKTurnInPlace) {
// Forward
auto speeds = frc::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward right turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward
speeds = frc::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
// Backward right turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
}
TEST(DifferentialDriveTest, TankDriveIK) {
// Forward
auto speeds = frc::DifferentialDrive::TankDriveIK(1.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::TankDriveIK(0.5, 1.0, false);
EXPECT_DOUBLE_EQ(0.5, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward right turn
speeds = frc::DifferentialDrive::TankDriveIK(1.0, 0.5, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.5, speeds.right);
// Backward
speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::TankDriveIK(-0.5, -1.0, false);
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward right turn
speeds = frc::DifferentialDrive::TankDriveIK(-0.5, 1.0, false);
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
}
TEST(DifferentialDriveTest, TankDriveIKSquared) {
// Forward
auto speeds = frc::DifferentialDrive::TankDriveIK(1.0, 1.0, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::TankDriveIK(0.5, 1.0, true);
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward right turn
speeds = frc::DifferentialDrive::TankDriveIK(1.0, 0.5, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
// Backward
speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -1.0, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::TankDriveIK(-0.5, -1.0, true);
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward right turn
speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -0.5, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-0.25, speeds.right);
}
TEST(DifferentialDriveTest, ArcadeDrive) {
frc::MockMotorController left;
frc::MockMotorController right;

74
wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
View File

@@ -8,6 +8,80 @@
#include "frc/drive/KilloughDrive.h"
#include "gtest/gtest.h"
TEST(KilloughDriveTest, CartesianIK) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::MockMotorController back;
frc::KilloughDrive drive{left, right, back};
// Forward
auto speeds = drive.DriveCartesianIK(1.0, 0.0, 0.0);
EXPECT_DOUBLE_EQ(0.5, speeds.left);
EXPECT_DOUBLE_EQ(-0.5, speeds.right);
EXPECT_NEAR(0.0, speeds.back, 1e-9);
// Left
speeds = drive.DriveCartesianIK(0.0, -1.0, 0.0);
EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.left);
EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.right);
EXPECT_DOUBLE_EQ(1.0, speeds.back);
// Right
speeds = drive.DriveCartesianIK(0.0, 1.0, 0.0);
EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, speeds.left);
EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, speeds.right);
EXPECT_DOUBLE_EQ(-1.0, speeds.back);
// Rotate CCW
speeds = drive.DriveCartesianIK(0.0, 0.0, -1.0);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
EXPECT_DOUBLE_EQ(-1.0, speeds.back);
// Rotate CW
speeds = drive.DriveCartesianIK(0.0, 0.0, 1.0);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
EXPECT_DOUBLE_EQ(1.0, speeds.back);
}
TEST(KilloughDriveTest, CartesianIKGyro90CW) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::MockMotorController back;
frc::KilloughDrive drive{left, right, back};
// Forward in global frame; left in robot frame
auto speeds = drive.DriveCartesianIK(1.0, 0.0, 0.0, 90.0);
EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.left);
EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.right);
EXPECT_DOUBLE_EQ(1.0, speeds.back);
// Left in global frame; backward in robot frame
speeds = drive.DriveCartesianIK(0.0, -1.0, 0.0, 90.0);
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_NEAR(0.5, speeds.right, 1e-9);
EXPECT_NEAR(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);
EXPECT_DOUBLE_EQ(0.5, speeds.left);
EXPECT_NEAR(-0.5, speeds.right, 1e-9);
EXPECT_NEAR(0.0, speeds.back, 1e-9);
// Rotate CCW
speeds = drive.DriveCartesianIK(0.0, 0.0, -1.0, 90.0);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
EXPECT_DOUBLE_EQ(-1.0, speeds.back);
// Rotate CW
speeds = drive.DriveCartesianIK(0.0, 0.0, 1.0, 90.0);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
EXPECT_DOUBLE_EQ(1.0, speeds.back);
}
TEST(KilloughDriveTest, Cartesian) {
frc::MockMotorController left;
frc::MockMotorController right;

106
wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
View File

@@ -6,12 +6,86 @@
#include "frc/drive/MecanumDrive.h"
#include "gtest/gtest.h"
TEST(MecanumDriveTest, CartesianIK) {
// Forward
auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0);
EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
// Left
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
// Right
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0);
EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
// Rotate CCW
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
// Rotate CW
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0);
EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
}
TEST(MecanumDriveTest, CartesianIKGyro90CW) {
// Forward in global frame; left in robot frame
auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0, 90.0);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
// Left in global frame; backward in robot frame
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0, 90.0);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
// Right in global frame; forward in robot frame
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0, 90.0);
EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
// Rotate CCW
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0, 90.0);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
// Rotate CW
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0, 90.0);
EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
}
TEST(MecanumDriveTest, Cartesian) {
frc::MockMotorController fl;
frc::MockMotorController fr;
frc::MockMotorController rl;
frc::MockMotorController fr;
frc::MockMotorController rr;
frc::MecanumDrive drive{fl, fr, rl, rr};
frc::MecanumDrive drive{fl, rl, fr, rr};
drive.SetDeadband(0.0);
// Forward
@@ -38,15 +112,15 @@ TEST(MecanumDriveTest, Cartesian) {
// Rotate CCW
drive.DriveCartesian(0.0, 0.0, -1.0);
EXPECT_DOUBLE_EQ(-1.0, fl.Get());
EXPECT_DOUBLE_EQ(-1.0, fr.Get());
EXPECT_DOUBLE_EQ(1.0, rl.Get());
EXPECT_DOUBLE_EQ(1.0, fr.Get());
EXPECT_DOUBLE_EQ(-1.0, rl.Get());
EXPECT_DOUBLE_EQ(1.0, rr.Get());
// Rotate CW
drive.DriveCartesian(0.0, 0.0, 1.0);
EXPECT_DOUBLE_EQ(1.0, fl.Get());
EXPECT_DOUBLE_EQ(1.0, fr.Get());
EXPECT_DOUBLE_EQ(-1.0, rl.Get());
EXPECT_DOUBLE_EQ(-1.0, fr.Get());
EXPECT_DOUBLE_EQ(1.0, rl.Get());
EXPECT_DOUBLE_EQ(-1.0, rr.Get());
}
@@ -55,7 +129,7 @@ TEST(MecanumDriveTest, CartesianGyro90CW) {
frc::MockMotorController fr;
frc::MockMotorController rl;
frc::MockMotorController rr;
frc::MecanumDrive drive{fl, fr, rl, rr};
frc::MecanumDrive drive{fl, rl, fr, rr};
drive.SetDeadband(0.0);
// Forward in global frame; left in robot frame
@@ -82,15 +156,15 @@ TEST(MecanumDriveTest, CartesianGyro90CW) {
// Rotate CCW
drive.DriveCartesian(0.0, 0.0, -1.0, 90.0);
EXPECT_DOUBLE_EQ(-1.0, fl.Get());
EXPECT_DOUBLE_EQ(-1.0, fr.Get());
EXPECT_DOUBLE_EQ(1.0, rl.Get());
EXPECT_DOUBLE_EQ(1.0, fr.Get());
EXPECT_DOUBLE_EQ(-1.0, rl.Get());
EXPECT_DOUBLE_EQ(1.0, rr.Get());
// Rotate CW
drive.DriveCartesian(0.0, 0.0, 1.0, 90.0);
EXPECT_DOUBLE_EQ(1.0, fl.Get());
EXPECT_DOUBLE_EQ(1.0, fr.Get());
EXPECT_DOUBLE_EQ(-1.0, rl.Get());
EXPECT_DOUBLE_EQ(-1.0, fr.Get());
EXPECT_DOUBLE_EQ(1.0, rl.Get());
EXPECT_DOUBLE_EQ(-1.0, rr.Get());
}
@@ -99,7 +173,7 @@ TEST(MecanumDriveTest, Polar) {
frc::MockMotorController fr;
frc::MockMotorController rl;
frc::MockMotorController rr;
frc::MecanumDrive drive{fl, fr, rl, rr};
frc::MecanumDrive drive{fl, rl, fr, rr};
drive.SetDeadband(0.0);
// Forward
@@ -126,14 +200,14 @@ TEST(MecanumDriveTest, Polar) {
// Rotate CCW
drive.DrivePolar(0.0, 0.0, -1.0);
EXPECT_DOUBLE_EQ(-1.0, fl.Get());
EXPECT_DOUBLE_EQ(-1.0, fr.Get());
EXPECT_DOUBLE_EQ(1.0, rl.Get());
EXPECT_DOUBLE_EQ(1.0, fr.Get());
EXPECT_DOUBLE_EQ(-1.0, rl.Get());
EXPECT_DOUBLE_EQ(1.0, rr.Get());
// Rotate CW
drive.DrivePolar(0.0, 0.0, 1.0);
EXPECT_DOUBLE_EQ(1.0, fl.Get());
EXPECT_DOUBLE_EQ(1.0, fr.Get());
EXPECT_DOUBLE_EQ(-1.0, rl.Get());
EXPECT_DOUBLE_EQ(-1.0, fr.Get());
EXPECT_DOUBLE_EQ(1.0, rl.Get());
EXPECT_DOUBLE_EQ(-1.0, rr.Get());
}