mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
[wpilib] DifferentialDrive: Remove right side inversion (#3340)
Also refactor drive inverse kinematics into separate functions. This allows composing them with operations separate from the drive class.
This commit is contained in:
Tyler Veness
229
wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
Normal file
229
wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
Normal file
@@ -0,0 +1,229 @@
|
||||
// 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 "MockMotorController.h"
|
||||
#include "frc/drive/DifferentialDrive.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
TEST(DifferentialDriveTest, ArcadeDrive) {
|
||||
frc::MockMotorController left;
|
||||
frc::MockMotorController right;
|
||||
frc::DifferentialDrive drive{left, right};
|
||||
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());
|
||||
|
||||
// Forward left turn
|
||||
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);
|
||||
EXPECT_DOUBLE_EQ(0.5, left.Get());
|
||||
EXPECT_DOUBLE_EQ(0.0, right.Get());
|
||||
|
||||
// Backward
|
||||
drive.ArcadeDrive(-1.0, 0.0, false);
|
||||
EXPECT_DOUBLE_EQ(-1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-1.0, right.Get());
|
||||
|
||||
// Backward left turn
|
||||
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);
|
||||
EXPECT_DOUBLE_EQ(0.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-0.5, right.Get());
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveTest, ArcadeDriveSquared) {
|
||||
frc::MockMotorController left;
|
||||
frc::MockMotorController right;
|
||||
frc::DifferentialDrive drive{left, right};
|
||||
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());
|
||||
|
||||
// Forward left turn
|
||||
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);
|
||||
EXPECT_DOUBLE_EQ(0.25, left.Get());
|
||||
EXPECT_DOUBLE_EQ(0.0, right.Get());
|
||||
|
||||
// Backward
|
||||
drive.ArcadeDrive(-1.0, 0.0, true);
|
||||
EXPECT_DOUBLE_EQ(-1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-1.0, right.Get());
|
||||
|
||||
// Backward left turn
|
||||
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);
|
||||
EXPECT_DOUBLE_EQ(0.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-0.25, right.Get());
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveTest, CurvatureDrive) {
|
||||
frc::MockMotorController left;
|
||||
frc::MockMotorController right;
|
||||
frc::DifferentialDrive drive{left, right};
|
||||
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());
|
||||
|
||||
// Forward left turn
|
||||
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);
|
||||
EXPECT_DOUBLE_EQ(0.75, left.Get());
|
||||
EXPECT_DOUBLE_EQ(0.25, right.Get());
|
||||
|
||||
// Backward
|
||||
drive.CurvatureDrive(-1.0, 0.0, false);
|
||||
EXPECT_DOUBLE_EQ(-1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-1.0, right.Get());
|
||||
|
||||
// Backward left turn
|
||||
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);
|
||||
EXPECT_DOUBLE_EQ(-0.25, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-0.75, right.Get());
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) {
|
||||
frc::MockMotorController left;
|
||||
frc::MockMotorController right;
|
||||
frc::DifferentialDrive drive{left, right};
|
||||
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());
|
||||
|
||||
// Forward left turn
|
||||
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);
|
||||
EXPECT_DOUBLE_EQ(1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(0.0, right.Get());
|
||||
|
||||
// Backward
|
||||
drive.CurvatureDrive(-1.0, 0.0, true);
|
||||
EXPECT_DOUBLE_EQ(-1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-1.0, right.Get());
|
||||
|
||||
// Backward left turn
|
||||
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);
|
||||
EXPECT_DOUBLE_EQ(0.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-1.0, right.Get());
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveTest, TankDrive) {
|
||||
frc::MockMotorController left;
|
||||
frc::MockMotorController right;
|
||||
frc::DifferentialDrive drive{left, right};
|
||||
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());
|
||||
|
||||
// Forward left turn
|
||||
drive.TankDrive(0.5, 1.0, false);
|
||||
EXPECT_DOUBLE_EQ(0.5, left.Get());
|
||||
EXPECT_DOUBLE_EQ(1.0, right.Get());
|
||||
|
||||
// Forward right turn
|
||||
drive.TankDrive(1.0, 0.5, false);
|
||||
EXPECT_DOUBLE_EQ(1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(0.5, right.Get());
|
||||
|
||||
// Backward
|
||||
drive.TankDrive(-1.0, -1.0, false);
|
||||
EXPECT_DOUBLE_EQ(-1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-1.0, right.Get());
|
||||
|
||||
// Backward left turn
|
||||
drive.TankDrive(-0.5, -1.0, false);
|
||||
EXPECT_DOUBLE_EQ(-0.5, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-1.0, right.Get());
|
||||
|
||||
// Backward right turn
|
||||
drive.TankDrive(-0.5, 1.0, false);
|
||||
EXPECT_DOUBLE_EQ(-0.5, left.Get());
|
||||
EXPECT_DOUBLE_EQ(1.0, right.Get());
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveTest, TankDriveSquared) {
|
||||
frc::MockMotorController left;
|
||||
frc::MockMotorController right;
|
||||
frc::DifferentialDrive drive{left, right};
|
||||
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());
|
||||
|
||||
// Forward left turn
|
||||
drive.TankDrive(0.5, 1.0, true);
|
||||
EXPECT_DOUBLE_EQ(0.25, left.Get());
|
||||
EXPECT_DOUBLE_EQ(1.0, right.Get());
|
||||
|
||||
// Forward right turn
|
||||
drive.TankDrive(1.0, 0.5, true);
|
||||
EXPECT_DOUBLE_EQ(1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(0.25, right.Get());
|
||||
|
||||
// Backward
|
||||
drive.TankDrive(-1.0, -1.0, true);
|
||||
EXPECT_DOUBLE_EQ(-1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-1.0, right.Get());
|
||||
|
||||
// Backward left turn
|
||||
drive.TankDrive(-0.5, -1.0, true);
|
||||
EXPECT_DOUBLE_EQ(-0.25, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-1.0, right.Get());
|
||||
|
||||
// Backward right turn
|
||||
drive.TankDrive(-1.0, -0.5, true);
|
||||
EXPECT_DOUBLE_EQ(-1.0, left.Get());
|
||||
EXPECT_DOUBLE_EQ(-0.25, right.Get());
|
||||
}
|
||||
123
wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
Normal file
123
wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
Normal file
@@ -0,0 +1,123 @@
|
||||
// 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 "MockMotorController.h"
|
||||
#include "frc/drive/KilloughDrive.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
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());
|
||||
}
|
||||
139
wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
Normal file
139
wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
Normal file
@@ -0,0 +1,139 @@
|
||||
// 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 "MockMotorController.h"
|
||||
#include "frc/drive/MecanumDrive.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
TEST(MecanumDriveTest, Cartesian) {
|
||||
frc::MockMotorController fl;
|
||||
frc::MockMotorController fr;
|
||||
frc::MockMotorController rl;
|
||||
frc::MockMotorController rr;
|
||||
frc::MecanumDrive drive{fl, fr, rl, rr};
|
||||
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());
|
||||
|
||||
// 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());
|
||||
|
||||
// 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());
|
||||
|
||||
// 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());
|
||||
|
||||
// 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());
|
||||
}
|
||||
|
||||
TEST(MecanumDriveTest, CartesianGyro90CW) {
|
||||
frc::MockMotorController fl;
|
||||
frc::MockMotorController fr;
|
||||
frc::MockMotorController rl;
|
||||
frc::MockMotorController rr;
|
||||
frc::MecanumDrive drive{fl, fr, rl, rr};
|
||||
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(-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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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());
|
||||
}
|
||||
|
||||
TEST(MecanumDriveTest, Polar) {
|
||||
frc::MockMotorController fl;
|
||||
frc::MockMotorController fr;
|
||||
frc::MockMotorController rl;
|
||||
frc::MockMotorController rr;
|
||||
frc::MecanumDrive drive{fl, fr, rl, rr};
|
||||
drive.SetDeadband(0.0);
|
||||
|
||||
// Forward
|
||||
drive.DrivePolar(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());
|
||||
|
||||
// Left
|
||||
drive.DrivePolar(1.0, -90.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());
|
||||
|
||||
// Right
|
||||
drive.DrivePolar(1.0, 90.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());
|
||||
|
||||
// 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, 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, rr.Get());
|
||||
}
|
||||
Reference in New Issue
Block a user