Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +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
wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
View File

@@ -13,12 +13,12 @@ TEST(DifferentialDriveTest, ArcadeDriveIK) {
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false);
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);
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.5, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
@@ -28,32 +28,32 @@ TEST(DifferentialDriveTest, ArcadeDriveIK) {
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, false);
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);
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);
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);
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);
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);
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
}
@@ -65,12 +65,12 @@ TEST(DifferentialDriveTest, ArcadeDriveIKSquared) {
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true);
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);
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
@@ -80,32 +80,32 @@ TEST(DifferentialDriveTest, ArcadeDriveIKSquared) {
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, true);
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);
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);
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);
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);
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);
speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
}
@@ -117,12 +117,12 @@ TEST(DifferentialDriveTest, CurvatureDriveIK) {
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false);
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);
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.75, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
@@ -132,12 +132,12 @@ TEST(DifferentialDriveTest, CurvatureDriveIK) {
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false);
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);
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(-0.75, speeds.right);
}
@@ -149,12 +149,12 @@ TEST(DifferentialDriveTest, CurvatureDriveIKTurnInPlace) {
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Forward left turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true);
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);
speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
@@ -164,12 +164,12 @@ TEST(DifferentialDriveTest, CurvatureDriveIKTurnInPlace) {
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Backward left turn
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true);
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);
speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
}
@@ -250,12 +250,12 @@ TEST(DifferentialDriveTest, ArcadeDrive) {
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
drive.ArcadeDrive(0.5, -0.5, false);
drive.ArcadeDrive(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(0.5, right.Get());
// Forward right turn
drive.ArcadeDrive(0.5, 0.5, false);
drive.ArcadeDrive(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.5, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
@@ -265,12 +265,12 @@ TEST(DifferentialDriveTest, ArcadeDrive) {
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
drive.ArcadeDrive(-0.5, -0.5, false);
drive.ArcadeDrive(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(-0.5, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
// Backward right turn
drive.ArcadeDrive(-0.5, 0.5, false);
drive.ArcadeDrive(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(-0.5, right.Get());
}
@@ -287,12 +287,12 @@ TEST(DifferentialDriveTest, ArcadeDriveSquared) {
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
drive.ArcadeDrive(0.5, -0.5, true);
drive.ArcadeDrive(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(0.25, right.Get());
// Forward right turn
drive.ArcadeDrive(0.5, 0.5, true);
drive.ArcadeDrive(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.25, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
@@ -302,12 +302,12 @@ TEST(DifferentialDriveTest, ArcadeDriveSquared) {
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
drive.ArcadeDrive(-0.5, -0.5, true);
drive.ArcadeDrive(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(-0.25, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
// Backward right turn
drive.ArcadeDrive(-0.5, 0.5, true);
drive.ArcadeDrive(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(-0.25, right.Get());
}
@@ -324,12 +324,12 @@ TEST(DifferentialDriveTest, CurvatureDrive) {
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
drive.CurvatureDrive(0.5, -0.5, false);
drive.CurvatureDrive(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.25, left.Get());
EXPECT_DOUBLE_EQ(0.75, right.Get());
// Forward right turn
drive.CurvatureDrive(0.5, 0.5, false);
drive.CurvatureDrive(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.75, left.Get());
EXPECT_DOUBLE_EQ(0.25, right.Get());
@@ -339,12 +339,12 @@ TEST(DifferentialDriveTest, CurvatureDrive) {
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
drive.CurvatureDrive(-0.5, -0.5, false);
drive.CurvatureDrive(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(-0.75, left.Get());
EXPECT_DOUBLE_EQ(-0.25, right.Get());
// Backward right turn
drive.CurvatureDrive(-0.5, 0.5, false);
drive.CurvatureDrive(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(-0.25, left.Get());
EXPECT_DOUBLE_EQ(-0.75, right.Get());
}
@@ -361,12 +361,12 @@ TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) {
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward left turn
drive.CurvatureDrive(0.5, -0.5, true);
drive.CurvatureDrive(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
// Forward right turn
drive.CurvatureDrive(0.5, 0.5, true);
drive.CurvatureDrive(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
@@ -376,12 +376,12 @@ TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) {
EXPECT_DOUBLE_EQ(-1.0, right.Get());
// Backward left turn
drive.CurvatureDrive(-0.5, -0.5, true);
drive.CurvatureDrive(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
// Backward right turn
drive.CurvatureDrive(-0.5, 0.5, true);
drive.CurvatureDrive(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
}

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

@@ -1,197 +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.
#include <cmath>
#include "frc/drive/KilloughDrive.h"
#include "gtest/gtest.h"
#include "motorcontrol/MockMotorController.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;
frc::MockMotorController back;
frc::KilloughDrive drive{left, right, back};
drive.SetDeadband(0.0);
// Forward
drive.DriveCartesian(1.0, 0.0, 0.0);
EXPECT_DOUBLE_EQ(0.5, left.Get());
EXPECT_DOUBLE_EQ(-0.5, right.Get());
EXPECT_NEAR(0.0, back.Get(), 1e-9);
// Left
drive.DriveCartesian(0.0, -1.0, 0.0);
EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, left.Get());
EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, right.Get());
EXPECT_DOUBLE_EQ(1.0, back.Get());
// Right
drive.DriveCartesian(0.0, 1.0, 0.0);
EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, left.Get());
EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, right.Get());
EXPECT_DOUBLE_EQ(-1.0, back.Get());
// Rotate CCW
drive.DriveCartesian(0.0, 0.0, -1.0);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, back.Get());
// Rotate CW
drive.DriveCartesian(0.0, 0.0, 1.0);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, back.Get());
}
TEST(KilloughDriveTest, CartesianGyro90CW) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::MockMotorController back;
frc::KilloughDrive drive{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);
EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, left.Get());
EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, right.Get());
EXPECT_DOUBLE_EQ(1.0, back.Get());
// Left in global frame; backward in robot frame
drive.DriveCartesian(0.0, -1.0, 0.0, 90.0);
EXPECT_DOUBLE_EQ(-0.5, left.Get());
EXPECT_NEAR(0.5, right.Get(), 1e-9);
EXPECT_NEAR(0.0, back.Get(), 1e-9);
// Right in global frame; forward in robot frame
drive.DriveCartesian(0.0, 1.0, 0.0, 90.0);
EXPECT_DOUBLE_EQ(0.5, left.Get());
EXPECT_NEAR(-0.5, right.Get(), 1e-9);
EXPECT_NEAR(0.0, back.Get(), 1e-9);
// Rotate CCW
drive.DriveCartesian(0.0, 0.0, -1.0, 90.0);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, back.Get());
// Rotate CW
drive.DriveCartesian(0.0, 0.0, 1.0, 90.0);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, back.Get());
}
TEST(KilloughDriveTest, Polar) {
frc::MockMotorController left;
frc::MockMotorController right;
frc::MockMotorController back;
frc::KilloughDrive drive{left, right, back};
drive.SetDeadband(0.0);
// Forward
drive.DrivePolar(1.0, 0.0, 0.0);
EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, left.Get());
EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, right.Get());
EXPECT_DOUBLE_EQ(-1.0, back.Get());
// Left
drive.DrivePolar(1.0, -90.0, 0.0);
EXPECT_DOUBLE_EQ(-0.5, left.Get());
EXPECT_DOUBLE_EQ(0.5, right.Get());
EXPECT_NEAR(0.0, back.Get(), 1e-9);
// Right
drive.DrivePolar(1.0, 90.0, 0.0);
EXPECT_DOUBLE_EQ(0.5, left.Get());
EXPECT_NEAR(-0.5, right.Get(), 1e-9);
EXPECT_NEAR(0.0, back.Get(), 1e-9);
// Rotate CCW
drive.DrivePolar(0.0, 0.0, -1.0);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, back.Get());
// Rotate CW
drive.DrivePolar(0.0, 0.0, 1.0);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, back.Get());
}

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

@@ -45,35 +45,35 @@ TEST(MecanumDriveTest, CartesianIK) {
TEST(MecanumDriveTest, CartesianIKGyro90CW) {
// Forward in global frame; left in robot frame
auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0, 90.0);
auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0, 90_deg);
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);
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0, 90_deg);
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);
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0, 90_deg);
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);
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0, 90_deg);
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);
speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0, 90_deg);
EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
@@ -133,35 +133,35 @@ TEST(MecanumDriveTest, CartesianGyro90CW) {
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, 90_deg);
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, rr.Get());
// 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, 90_deg);
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, rr.Get());
// 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, 90_deg);
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, rr.Get());
// Rotate CCW
drive.DriveCartesian(0.0, 0.0, -1.0, 90.0);
drive.DriveCartesian(0.0, 0.0, -1.0, 90_deg);
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, rr.Get());
// Rotate CW
drive.DriveCartesian(0.0, 0.0, 1.0, 90.0);
drive.DriveCartesian(0.0, 0.0, 1.0, 90_deg);
EXPECT_DOUBLE_EQ(1.0, fl.Get());
EXPECT_DOUBLE_EQ(-1.0, fr.Get());
EXPECT_DOUBLE_EQ(1.0, rl.Get());
@@ -177,35 +177,35 @@ TEST(MecanumDriveTest, Polar) {
drive.SetDeadband(0.0);
// Forward
drive.DrivePolar(1.0, 0.0, 0.0);
drive.DrivePolar(1.0, 0_deg, 0.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, rr.Get());
// Left
drive.DrivePolar(1.0, -90.0, 0.0);
drive.DrivePolar(1.0, -90_deg, 0.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, rr.Get());
// Right
drive.DrivePolar(1.0, 90.0, 0.0);
drive.DrivePolar(1.0, 90_deg, 0.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, rr.Get());
// Rotate CCW
drive.DrivePolar(0.0, 0.0, -1.0);
drive.DrivePolar(0.0, 0_deg, -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, rr.Get());
// Rotate CW
drive.DrivePolar(0.0, 0.0, 1.0);
drive.DrivePolar(0.0, 0_deg, 1.0);
EXPECT_DOUBLE_EQ(1.0, fl.Get());
EXPECT_DOUBLE_EQ(-1.0, fr.Get());
EXPECT_DOUBLE_EQ(1.0, rl.Get());