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

14
wpilibc/src/test/native/cpp/LEDPatternTest.cpp
View File

@@ -208,7 +208,8 @@ TEST(LEDPatternTest, ScrollRelativeForward) {
// Scrolling at 1/256th of the buffer per second,
// or 1 individual diode per second
auto scroll = pattern.ScrollAtRelativeSpeed(wpi::units::hertz_t{1 / 256.0});
auto scroll =
pattern.ScrollAtRelativeVelocity(wpi::units::hertz_t{1 / 256.0});
static uint64_t now = 0ull;
WPI_SetNowImpl([] { return now; });
@@ -251,7 +252,8 @@ TEST(LEDPatternTest, ScrollRelativeBackward) {
// Scrolling at 1/256th of the buffer per second,
// or 1 individual diode per second
auto scroll = pattern.ScrollAtRelativeSpeed(wpi::units::hertz_t{-1 / 256.0});
auto scroll =
pattern.ScrollAtRelativeVelocity(wpi::units::hertz_t{-1 / 256.0});
static uint64_t now = 0ull;
WPI_SetNowImpl([] { return now; });
@@ -295,7 +297,7 @@ TEST(LEDPatternTest, ScrollAbsoluteForward) {
// 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)
auto scroll = pattern.ScrollAtAbsoluteSpeed(16_mps, 2_cm);
auto scroll = pattern.ScrollAtAbsoluteVelocity(16_mps, 2_cm);
static uint64_t now = 0ull;
WPI_SetNowImpl([] { return now; });
@@ -339,7 +341,7 @@ TEST(LEDPatternTest, ScrollAbsoluteBackward) {
// 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)
auto scroll = pattern.ScrollAtAbsoluteSpeed(-16_mps, 2_cm);
auto scroll = pattern.ScrollAtAbsoluteVelocity(-16_mps, 2_cm);
static uint64_t now = 0ull;
WPI_SetNowImpl([] { return now; });
@@ -912,7 +914,7 @@ TEST(LEDPatternTest, RelativeScrollingMask) {
auto pattern = LEDPattern::Steps(colorSteps)
.Mask(LEDPattern::Steps(maskSteps))
.ScrollAtRelativeSpeed(wpi::units::hertz_t{1e6 / 8.0});
.ScrollAtRelativeVelocity(wpi::units::hertz_t{1e6 / 8.0});
pattern.ApplyTo(buffer);
@@ -997,7 +999,7 @@ TEST(LEDPatternTest, AbsoluteScrollingMask) {
auto pattern = LEDPattern::Steps(colorSteps)
.Mask(LEDPattern::Steps(maskSteps))
.ScrollAtAbsoluteSpeed(1_mps, 1_m);
.ScrollAtAbsoluteVelocity(1_mps, 1_m);
pattern.ApplyTo(buffer);

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

@@ -10,460 +10,466 @@
TEST(DifferentialDriveTest, ArcadeDriveIK) {
// Forward
auto speeds = wpi::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
auto velocities = wpi::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Forward left turn
speeds = wpi::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(0.5, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.0, velocities.left);
EXPECT_DOUBLE_EQ(0.5, velocities.right);
// Forward right turn
speeds = wpi::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.5, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.5, velocities.left);
EXPECT_DOUBLE_EQ(0.0, velocities.right);
// Backward
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Backward left turn
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(-0.5, velocities.left);
EXPECT_DOUBLE_EQ(0.0, velocities.right);
// Backward right turn
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-0.5, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.0, velocities.left);
EXPECT_DOUBLE_EQ(-0.5, velocities.right);
// Left turn (xSpeed with negative sign)
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Left turn (xVelocity with negative sign)
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Left turn (xSpeed with positive sign)
speeds = wpi::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Left turn (xVelocity with positive sign)
velocities = wpi::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Right turn (xSpeed with negative sign)
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Right turn (xVelocity with negative sign)
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Right turn (xSpeed with positive sign)
speeds = wpi::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Right turn (xVelocity with positive sign)
velocities = wpi::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
}
TEST(DifferentialDriveTest, ArcadeDriveIKSquared) {
// Forward
auto speeds = wpi::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
auto velocities = wpi::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Forward left turn
speeds = wpi::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, velocities.left);
EXPECT_DOUBLE_EQ(0.25, velocities.right);
// Forward right turn
speeds = wpi::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.25, velocities.left);
EXPECT_DOUBLE_EQ(0.0, velocities.right);
// Backward
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Backward left turn
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(-0.25, velocities.left);
EXPECT_DOUBLE_EQ(0.0, velocities.right);
// Backward right turn
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-0.25, speeds.right);
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, velocities.left);
EXPECT_DOUBLE_EQ(-0.25, velocities.right);
// Left turn (xSpeed with negative sign)
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Left turn (xVelocity with negative sign)
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Left turn (xSpeed with positive sign)
speeds = wpi::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
// Left turn (xVelocity with positive sign)
velocities = wpi::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Right turn (xSpeed with negative sign)
speeds = wpi::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Right turn (xVelocity with negative sign)
velocities = wpi::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Right turn (xSpeed with positive sign)
speeds = wpi::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
// Right turn (xVelocity with positive sign)
velocities = wpi::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
}
TEST(DifferentialDriveTest, CurvatureDriveIK) {
// Forward
auto speeds = wpi::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
auto velocities = wpi::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Forward left turn
speeds = wpi::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(0.75, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.25, velocities.left);
EXPECT_DOUBLE_EQ(0.75, velocities.right);
// Forward right turn
speeds = wpi::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.75, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.75, velocities.left);
EXPECT_DOUBLE_EQ(0.25, velocities.right);
// Backward
speeds = wpi::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Backward left turn
speeds = wpi::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(-0.75, speeds.left);
EXPECT_DOUBLE_EQ(-0.25, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(-0.75, velocities.left);
EXPECT_DOUBLE_EQ(-0.25, velocities.right);
// Backward right turn
speeds = wpi::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(-0.75, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(-0.25, velocities.left);
EXPECT_DOUBLE_EQ(-0.75, velocities.right);
}
TEST(DifferentialDriveTest, CurvatureDriveIKTurnInPlace) {
// Forward
auto speeds = wpi::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
auto velocities = wpi::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Forward left turn
speeds = wpi::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Forward right turn
speeds = wpi::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(0.0, velocities.right);
// Backward
speeds = wpi::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Backward left turn
speeds = wpi::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.0, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(0.0, velocities.right);
// Backward right turn
speeds = wpi::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
velocities = wpi::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
}
TEST(DifferentialDriveTest, TankDriveIK) {
// Forward
auto speeds = wpi::DifferentialDrive::TankDriveIK(1.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
auto velocities = wpi::DifferentialDrive::TankDriveIK(1.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Forward left turn
speeds = wpi::DifferentialDrive::TankDriveIK(0.5, 1.0, false);
EXPECT_DOUBLE_EQ(0.5, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(0.5, 1.0, false);
EXPECT_DOUBLE_EQ(0.5, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Forward right turn
speeds = wpi::DifferentialDrive::TankDriveIK(1.0, 0.5, false);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.5, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(1.0, 0.5, false);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(0.5, velocities.right);
// Backward
speeds = wpi::DifferentialDrive::TankDriveIK(-1.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(-1.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Backward left turn
speeds = wpi::DifferentialDrive::TankDriveIK(-0.5, -1.0, false);
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(-0.5, -1.0, false);
EXPECT_DOUBLE_EQ(-0.5, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Backward right turn
speeds = wpi::DifferentialDrive::TankDriveIK(-0.5, 1.0, false);
EXPECT_DOUBLE_EQ(-0.5, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(-0.5, 1.0, false);
EXPECT_DOUBLE_EQ(-0.5, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
}
TEST(DifferentialDriveTest, TankDriveIKSquared) {
// Forward
auto speeds = wpi::DifferentialDrive::TankDriveIK(1.0, 1.0, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
auto velocities = wpi::DifferentialDrive::TankDriveIK(1.0, 1.0, true);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Forward left turn
speeds = wpi::DifferentialDrive::TankDriveIK(0.5, 1.0, true);
EXPECT_DOUBLE_EQ(0.25, speeds.left);
EXPECT_DOUBLE_EQ(1.0, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(0.5, 1.0, true);
EXPECT_DOUBLE_EQ(0.25, velocities.left);
EXPECT_DOUBLE_EQ(1.0, velocities.right);
// Forward right turn
speeds = wpi::DifferentialDrive::TankDriveIK(1.0, 0.5, true);
EXPECT_DOUBLE_EQ(1.0, speeds.left);
EXPECT_DOUBLE_EQ(0.25, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(1.0, 0.5, true);
EXPECT_DOUBLE_EQ(1.0, velocities.left);
EXPECT_DOUBLE_EQ(0.25, velocities.right);
// Backward
speeds = wpi::DifferentialDrive::TankDriveIK(-1.0, -1.0, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(-1.0, -1.0, true);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Backward left turn
speeds = wpi::DifferentialDrive::TankDriveIK(-0.5, -1.0, true);
EXPECT_DOUBLE_EQ(-0.25, speeds.left);
EXPECT_DOUBLE_EQ(-1.0, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(-0.5, -1.0, true);
EXPECT_DOUBLE_EQ(-0.25, velocities.left);
EXPECT_DOUBLE_EQ(-1.0, velocities.right);
// Backward right turn
speeds = wpi::DifferentialDrive::TankDriveIK(-1.0, -0.5, true);
EXPECT_DOUBLE_EQ(-1.0, speeds.left);
EXPECT_DOUBLE_EQ(-0.25, speeds.right);
velocities = wpi::DifferentialDrive::TankDriveIK(-1.0, -0.5, true);
EXPECT_DOUBLE_EQ(-1.0, velocities.left);
EXPECT_DOUBLE_EQ(-0.25, velocities.right);
}
TEST(DifferentialDriveTest, ArcadeDrive) {
wpi::MockPWMMotorController left;
wpi::MockPWMMotorController right;
wpi::DifferentialDrive drive{[&](double output) { left.Set(output); },
[&](double output) { right.Set(output); }};
wpi::DifferentialDrive drive{
[&](double output) { left.SetDutyCycle(output); },
[&](double output) { right.SetDutyCycle(output); }};
drive.SetDeadband(0.0);
// Forward
drive.ArcadeDrive(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
// Forward left turn
drive.ArcadeDrive(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(0.5, right.Get());
EXPECT_DOUBLE_EQ(0.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.5, right.GetDutyCycle());
// Forward right turn
drive.ArcadeDrive(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.5, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
EXPECT_DOUBLE_EQ(0.5, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.0, right.GetDutyCycle());
// Backward
drive.ArcadeDrive(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, right.GetDutyCycle());
// Backward left turn
drive.ArcadeDrive(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(-0.5, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
EXPECT_DOUBLE_EQ(-0.5, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.0, right.GetDutyCycle());
// Backward right turn
drive.ArcadeDrive(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(-0.5, right.Get());
EXPECT_DOUBLE_EQ(0.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-0.5, right.GetDutyCycle());
}
TEST(DifferentialDriveTest, ArcadeDriveSquared) {
wpi::MockPWMMotorController left;
wpi::MockPWMMotorController right;
wpi::DifferentialDrive drive{[&](double output) { left.Set(output); },
[&](double output) { right.Set(output); }};
wpi::DifferentialDrive drive{
[&](double output) { left.SetDutyCycle(output); },
[&](double output) { right.SetDutyCycle(output); }};
drive.SetDeadband(0.0);
// Forward
drive.ArcadeDrive(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
// Forward left turn
drive.ArcadeDrive(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(0.25, right.Get());
EXPECT_DOUBLE_EQ(0.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.25, right.GetDutyCycle());
// Forward right turn
drive.ArcadeDrive(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.25, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
EXPECT_DOUBLE_EQ(0.25, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.0, right.GetDutyCycle());
// Backward
drive.ArcadeDrive(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, right.GetDutyCycle());
// Backward left turn
drive.ArcadeDrive(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(-0.25, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
EXPECT_DOUBLE_EQ(-0.25, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.0, right.GetDutyCycle());
// Backward right turn
drive.ArcadeDrive(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(-0.25, right.Get());
EXPECT_DOUBLE_EQ(0.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-0.25, right.GetDutyCycle());
}
TEST(DifferentialDriveTest, CurvatureDrive) {
wpi::MockPWMMotorController left;
wpi::MockPWMMotorController right;
wpi::DifferentialDrive drive{[&](double output) { left.Set(output); },
[&](double output) { right.Set(output); }};
wpi::DifferentialDrive drive{
[&](double output) { left.SetDutyCycle(output); },
[&](double output) { right.SetDutyCycle(output); }};
drive.SetDeadband(0.0);
// Forward
drive.CurvatureDrive(1.0, 0.0, false);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
// Forward left turn
drive.CurvatureDrive(0.5, 0.5, false);
EXPECT_DOUBLE_EQ(0.25, left.Get());
EXPECT_DOUBLE_EQ(0.75, right.Get());
EXPECT_DOUBLE_EQ(0.25, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.75, right.GetDutyCycle());
// Forward right turn
drive.CurvatureDrive(0.5, -0.5, false);
EXPECT_DOUBLE_EQ(0.75, left.Get());
EXPECT_DOUBLE_EQ(0.25, right.Get());
EXPECT_DOUBLE_EQ(0.75, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.25, right.GetDutyCycle());
// Backward
drive.CurvatureDrive(-1.0, 0.0, false);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, right.GetDutyCycle());
// Backward left turn
drive.CurvatureDrive(-0.5, 0.5, false);
EXPECT_DOUBLE_EQ(-0.75, left.Get());
EXPECT_DOUBLE_EQ(-0.25, right.Get());
EXPECT_DOUBLE_EQ(-0.75, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-0.25, right.GetDutyCycle());
// Backward right turn
drive.CurvatureDrive(-0.5, -0.5, false);
EXPECT_DOUBLE_EQ(-0.25, left.Get());
EXPECT_DOUBLE_EQ(-0.75, right.Get());
EXPECT_DOUBLE_EQ(-0.25, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-0.75, right.GetDutyCycle());
}
TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) {
wpi::MockPWMMotorController left;
wpi::MockPWMMotorController right;
wpi::DifferentialDrive drive{[&](double output) { left.Set(output); },
[&](double output) { right.Set(output); }};
wpi::DifferentialDrive drive{
[&](double output) { left.SetDutyCycle(output); },
[&](double output) { right.SetDutyCycle(output); }};
drive.SetDeadband(0.0);
// Forward
drive.CurvatureDrive(1.0, 0.0, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
// Forward left turn
drive.CurvatureDrive(0.5, 0.5, true);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(0.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
// Forward right turn
drive.CurvatureDrive(0.5, -0.5, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.0, right.GetDutyCycle());
// Backward
drive.CurvatureDrive(-1.0, 0.0, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, right.GetDutyCycle());
// Backward left turn
drive.CurvatureDrive(-0.5, 0.5, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(0.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.0, right.GetDutyCycle());
// Backward right turn
drive.CurvatureDrive(-0.5, -0.5, true);
EXPECT_DOUBLE_EQ(0.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(0.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, right.GetDutyCycle());
}
TEST(DifferentialDriveTest, TankDrive) {
wpi::MockPWMMotorController left;
wpi::MockPWMMotorController right;
wpi::DifferentialDrive drive{[&](double output) { left.Set(output); },
[&](double output) { right.Set(output); }};
wpi::DifferentialDrive drive{
[&](double output) { left.SetDutyCycle(output); },
[&](double output) { right.SetDutyCycle(output); }};
drive.SetDeadband(0.0);
// Forward
drive.TankDrive(1.0, 1.0, false);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
// Forward left turn
drive.TankDrive(0.5, 1.0, false);
EXPECT_DOUBLE_EQ(0.5, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(0.5, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
// Forward right turn
drive.TankDrive(1.0, 0.5, false);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(0.5, right.Get());
EXPECT_DOUBLE_EQ(1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.5, right.GetDutyCycle());
// Backward
drive.TankDrive(-1.0, -1.0, false);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, right.GetDutyCycle());
// Backward left turn
drive.TankDrive(-0.5, -1.0, false);
EXPECT_DOUBLE_EQ(-0.5, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-0.5, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, right.GetDutyCycle());
// Backward right turn
drive.TankDrive(-0.5, 1.0, false);
EXPECT_DOUBLE_EQ(-0.5, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(-0.5, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
}
TEST(DifferentialDriveTest, TankDriveSquared) {
wpi::MockPWMMotorController left;
wpi::MockPWMMotorController right;
wpi::DifferentialDrive drive{[&](double output) { left.Set(output); },
[&](double output) { right.Set(output); }};
wpi::DifferentialDrive drive{
[&](double output) { left.SetDutyCycle(output); },
[&](double output) { right.SetDutyCycle(output); }};
drive.SetDeadband(0.0);
// Forward
drive.TankDrive(1.0, 1.0, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
// Forward left turn
drive.TankDrive(0.5, 1.0, true);
EXPECT_DOUBLE_EQ(0.25, left.Get());
EXPECT_DOUBLE_EQ(1.0, right.Get());
EXPECT_DOUBLE_EQ(0.25, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, right.GetDutyCycle());
// Forward right turn
drive.TankDrive(1.0, 0.5, true);
EXPECT_DOUBLE_EQ(1.0, left.Get());
EXPECT_DOUBLE_EQ(0.25, right.Get());
EXPECT_DOUBLE_EQ(1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(0.25, right.GetDutyCycle());
// Backward
drive.TankDrive(-1.0, -1.0, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, right.GetDutyCycle());
// Backward left turn
drive.TankDrive(-0.5, -1.0, true);
EXPECT_DOUBLE_EQ(-0.25, left.Get());
EXPECT_DOUBLE_EQ(-1.0, right.Get());
EXPECT_DOUBLE_EQ(-0.25, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, right.GetDutyCycle());
// Backward right turn
drive.TankDrive(-1.0, -0.5, true);
EXPECT_DOUBLE_EQ(-1.0, left.Get());
EXPECT_DOUBLE_EQ(-0.25, right.Get());
EXPECT_DOUBLE_EQ(-1.0, left.GetDutyCycle());
EXPECT_DOUBLE_EQ(-0.25, right.GetDutyCycle());
}

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

@@ -10,76 +10,76 @@
TEST(MecanumDriveTest, CartesianIK) {
// Forward
auto speeds = wpi::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);
auto velocities = wpi::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0);
EXPECT_DOUBLE_EQ(1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.rearRight);
// Left
speeds = wpi::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);
velocities = wpi::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0);
EXPECT_DOUBLE_EQ(-1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, velocities.rearRight);
// Right
speeds = wpi::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);
velocities = wpi::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0);
EXPECT_DOUBLE_EQ(1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(-1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(-1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.rearRight);
// Rotate CCW
speeds = wpi::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);
velocities = wpi::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0);
EXPECT_DOUBLE_EQ(-1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(-1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.rearRight);
// Rotate CW
speeds = wpi::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);
velocities = wpi::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0);
EXPECT_DOUBLE_EQ(1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(-1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, velocities.rearRight);
}
TEST(MecanumDriveTest, CartesianIKGyro90CW) {
// Forward in global frame; left in robot frame
auto speeds = wpi::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);
auto velocities = wpi::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0, 90_deg);
EXPECT_DOUBLE_EQ(-1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, velocities.rearRight);
// Left in global frame; backward in robot frame
speeds = wpi::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);
velocities = wpi::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0, 90_deg);
EXPECT_DOUBLE_EQ(-1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(-1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(-1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, velocities.rearRight);
// Right in global frame; forward in robot frame
speeds = wpi::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);
velocities = wpi::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0, 90_deg);
EXPECT_DOUBLE_EQ(1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.rearRight);
// Rotate CCW
speeds = wpi::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);
velocities = wpi::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0, 90_deg);
EXPECT_DOUBLE_EQ(-1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(-1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(1.0, velocities.rearRight);
// Rotate CW
speeds = wpi::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);
velocities = wpi::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0, 90_deg);
EXPECT_DOUBLE_EQ(1.0, velocities.frontLeft);
EXPECT_DOUBLE_EQ(-1.0, velocities.frontRight);
EXPECT_DOUBLE_EQ(1.0, velocities.rearLeft);
EXPECT_DOUBLE_EQ(-1.0, velocities.rearRight);
}
TEST(MecanumDriveTest, Cartesian) {
@@ -87,46 +87,46 @@ TEST(MecanumDriveTest, Cartesian) {
wpi::MockPWMMotorController rl;
wpi::MockPWMMotorController fr;
wpi::MockPWMMotorController rr;
wpi::MecanumDrive drive{[&](double output) { fl.Set(output); },
[&](double output) { rl.Set(output); },
[&](double output) { fr.Set(output); },
[&](double output) { rr.Set(output); }};
wpi::MecanumDrive drive{[&](double output) { fl.SetDutyCycle(output); },
[&](double output) { rl.SetDutyCycle(output); },
[&](double output) { fr.SetDutyCycle(output); },
[&](double output) { rr.SetDutyCycle(output); }};
drive.SetDeadband(0.0);
// Forward
drive.DriveCartesian(1.0, 0.0, 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());
EXPECT_DOUBLE_EQ(1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rr.GetDutyCycle());
// Left
drive.DriveCartesian(0.0, -1.0, 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());
EXPECT_DOUBLE_EQ(-1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rr.GetDutyCycle());
// Right
drive.DriveCartesian(0.0, 1.0, 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());
EXPECT_DOUBLE_EQ(1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rr.GetDutyCycle());
// 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, rr.Get());
EXPECT_DOUBLE_EQ(-1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rr.GetDutyCycle());
// 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, rr.Get());
EXPECT_DOUBLE_EQ(1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rr.GetDutyCycle());
}
TEST(MecanumDriveTest, CartesianGyro90CW) {
@@ -134,46 +134,46 @@ TEST(MecanumDriveTest, CartesianGyro90CW) {
wpi::MockPWMMotorController rl;
wpi::MockPWMMotorController fr;
wpi::MockPWMMotorController rr;
wpi::MecanumDrive drive{[&](double output) { fl.Set(output); },
[&](double output) { rl.Set(output); },
[&](double output) { fr.Set(output); },
[&](double output) { rr.Set(output); }};
wpi::MecanumDrive drive{[&](double output) { fl.SetDutyCycle(output); },
[&](double output) { rl.SetDutyCycle(output); },
[&](double output) { fr.SetDutyCycle(output); },
[&](double output) { rr.SetDutyCycle(output); }};
drive.SetDeadband(0.0);
// Forward in global frame; left in robot frame
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());
EXPECT_DOUBLE_EQ(-1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rr.GetDutyCycle());
// Left in global frame; backward in robot frame
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());
EXPECT_DOUBLE_EQ(-1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rr.GetDutyCycle());
// Right in global frame; forward in robot frame
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());
EXPECT_DOUBLE_EQ(1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rr.GetDutyCycle());
// Rotate CCW
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());
EXPECT_DOUBLE_EQ(-1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rr.GetDutyCycle());
// Rotate CW
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());
EXPECT_DOUBLE_EQ(1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rr.GetDutyCycle());
}
TEST(MecanumDriveTest, Polar) {
@@ -181,44 +181,44 @@ TEST(MecanumDriveTest, Polar) {
wpi::MockPWMMotorController rl;
wpi::MockPWMMotorController fr;
wpi::MockPWMMotorController rr;
wpi::MecanumDrive drive{[&](double output) { fl.Set(output); },
[&](double output) { rl.Set(output); },
[&](double output) { fr.Set(output); },
[&](double output) { rr.Set(output); }};
wpi::MecanumDrive drive{[&](double output) { fl.SetDutyCycle(output); },
[&](double output) { rl.SetDutyCycle(output); },
[&](double output) { fr.SetDutyCycle(output); },
[&](double output) { rr.SetDutyCycle(output); }};
drive.SetDeadband(0.0);
// Forward
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());
EXPECT_DOUBLE_EQ(1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rr.GetDutyCycle());
// Left
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());
EXPECT_DOUBLE_EQ(-1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rr.GetDutyCycle());
// Right
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());
EXPECT_DOUBLE_EQ(1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rr.GetDutyCycle());
// Rotate CCW
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());
EXPECT_DOUBLE_EQ(-1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rr.GetDutyCycle());
// Rotate CW
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());
EXPECT_DOUBLE_EQ(1.0, fl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, fr.GetDutyCycle());
EXPECT_DOUBLE_EQ(1.0, rl.GetDutyCycle());
EXPECT_DOUBLE_EQ(-1.0, rr.GetDutyCycle());
}

10
wpilibc/src/test/native/cpp/motorcontrol/MockMotorController.cpp
View File

@@ -6,12 +6,12 @@
using namespace wpi;
void MockMotorController::Set(double speed) {
m_speed = m_isInverted ? -speed : speed;
void MockMotorController::SetDutyCycle(double dutyCycle) {
m_dutyCycle = m_isInverted ? -dutyCycle : dutyCycle;
}
double MockMotorController::Get() const {
return m_speed;
double MockMotorController::GetDutyCycle() const {
return m_dutyCycle;
}
void MockMotorController::SetInverted(bool isInverted) {
@@ -23,5 +23,5 @@ bool MockMotorController::GetInverted() const {
}
void MockMotorController::Disable() {
m_speed = 0;
m_dutyCycle = 0;
}

10
wpilibc/src/test/native/cpp/motorcontrol/MockPWMMotorController.cpp
View File

@@ -6,12 +6,12 @@
using namespace wpi;
void MockPWMMotorController::Set(double speed) {
m_speed = m_isInverted ? -speed : speed;
void MockPWMMotorController::SetDutyCycle(double dutyCycle) {
m_dutyCycle = m_isInverted ? -dutyCycle : dutyCycle;
}
double MockPWMMotorController::Get() const {
return m_speed;
double MockPWMMotorController::GetDutyCycle() const {
return m_dutyCycle;
}
void MockPWMMotorController::SetInverted(bool isInverted) {
@@ -23,7 +23,7 @@ bool MockPWMMotorController::GetInverted() const {
}
void MockPWMMotorController::Disable() {
m_speed = 0;
m_dutyCycle = 0;
}
void MockPWMMotorController::StopMotor() {

8
wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp
View File

@@ -37,7 +37,7 @@ TEST(DCMotorSimTest, VoltageSteadyState) {
// Then, SimulationPeriodic runs
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
sim.SetInputVoltage(motor.Get() *
sim.SetInputVoltage(motor.GetDutyCycle() *
wpi::RobotController::GetBatteryVoltage());
sim.Update(20_ms);
encoderSim.SetRate(sim.GetAngularVelocity().value());
@@ -53,7 +53,7 @@ TEST(DCMotorSimTest, VoltageSteadyState) {
// Then, SimulationPeriodic runs
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
sim.SetInputVoltage(motor.Get() *
sim.SetInputVoltage(motor.GetDutyCycle() *
wpi::RobotController::GetBatteryVoltage());
sim.Update(20_ms);
encoderSim.SetRate(sim.GetAngularVelocity().value());
@@ -80,12 +80,12 @@ TEST(DCMotorSimTest, PositionFeedbackControl) {
for (int i = 0; i < 140; i++) {
// RobotPeriodic runs first
motor.Set(controller.Calculate(encoder.GetDistance(), 750));
motor.SetDutyCycle(controller.Calculate(encoder.GetDistance(), 750));
// Then, SimulationPeriodic runs
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
sim.SetInputVoltage(motor.Get() *
sim.SetInputVoltage(motor.GetDutyCycle() *
wpi::RobotController::GetBatteryVoltage());
sim.Update(20_ms);
encoderSim.SetDistance(sim.GetAngularPosition().value());

2
wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
View File

@@ -47,7 +47,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
auto state = trajectory.Sample(t);
auto feedbackOut = feedback.Calculate(sim.GetPose(), state);
auto [l, r] = kinematics.ToWheelSpeeds(feedbackOut);
auto [l, r] = kinematics.ToWheelVelocities(feedbackOut);
auto voltages =
feedforward.Calculate(wpi::math::Vectord<2>{l.value(), r.value()});

5
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
View File

@@ -13,7 +13,6 @@
#include "wpi/math/system/Models.hpp"
#include "wpi/simulation/EncoderSim.hpp"
#include "wpi/system/RobotController.hpp"
#include "wpi/units/math.hpp"
#include "wpi/units/time.hpp"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
@@ -31,9 +30,9 @@ TEST(ElevatorSimTest, StateSpaceSim) {
for (size_t i = 0; i < 100; ++i) {
controller.SetSetpoint(2.0);
auto nextVoltage = controller.Calculate(encoderSim.GetDistance());
motor.Set(nextVoltage / wpi::RobotController::GetInputVoltage());
motor.SetDutyCycle(nextVoltage / wpi::RobotController::GetInputVoltage());
wpi::math::Vectord<1> u{motor.Get() *
wpi::math::Vectord<1> u{motor.GetDutyCycle() *
wpi::RobotController::GetInputVoltage()};
sim.SetInput(u);
sim.Update(20_ms);

12
wpilibc/src/test/native/cpp/simulation/PWMMotorControlllerSimTest.cpp
View File

@@ -15,13 +15,13 @@ TEST(PWMMotorControllerSimTest, TestMotor) {
wpi::Spark spark{0};
wpi::sim::PWMMotorControllerSim sim{spark};
spark.Set(0);
EXPECT_EQ(0, sim.GetSpeed());
spark.SetDutyCycle(0);
EXPECT_EQ(0, sim.GetDutyCycle());
spark.Set(0.354);
EXPECT_EQ(0.354, sim.GetSpeed());
spark.SetDutyCycle(0.354);
EXPECT_EQ(0.354, sim.GetDutyCycle());
spark.Set(-0.785);
EXPECT_EQ(-0.785, sim.GetSpeed());
spark.SetDutyCycle(-0.785);
EXPECT_EQ(-0.785, sim.GetDutyCycle());
}
} // namespace wpi::sim

2
wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
View File

@@ -42,7 +42,7 @@ TEST(StateSpaceSimTest, FlywheelSim) {
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
sim.SetInput(wpi::math::Vectord<1>{
motor.Get() * wpi::RobotController::GetInputVoltage()});
motor.GetDutyCycle() * wpi::RobotController::GetInputVoltage()});
sim.Update(20_ms);
encoderSim.SetRate(sim.GetAngularVelocity().value());
}