diff --git a/wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/PIDInputOutputTest.cpp rename to wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp diff --git a/wpilibc/src/test/native/cpp/PIDToleranceTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/PIDToleranceTest.cpp rename to wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp diff --git a/wpilibc/src/test/native/cpp/DriveTest.cpp b/wpilibc/src/test/native/cpp/drive/DriveTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/DriveTest.cpp rename to wpilibc/src/test/native/cpp/drive/DriveTest.cpp diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java similarity index 96% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java rename to wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java index 3a744643e4..4d0ce93b94 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDInputOutputTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java @@ -5,13 +5,11 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package edu.wpi.first.wpilibj; +package edu.wpi.first.wpilibj.controller; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; -import edu.wpi.first.wpilibj.controller.PIDController; - import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java similarity index 97% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java rename to wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java index d4d63f9692..764453a657 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/PIDToleranceTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java @@ -5,14 +5,12 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package edu.wpi.first.wpilibj; +package edu.wpi.first.wpilibj.controller; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; -import edu.wpi.first.wpilibj.controller.PIDController; - import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java new file mode 100644 index 0000000000..14e94013c8 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java @@ -0,0 +1,170 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.drive; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.MockSpeedController; +import edu.wpi.first.wpilibj.RobotDrive; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +/** + * Tests DifferentialDrive and MecanumDrive. + */ +public class DriveTest { + private final MockSpeedController m_rdFrontLeft = new MockSpeedController(); + private final MockSpeedController m_rdRearLeft = new MockSpeedController(); + private final MockSpeedController m_rdFrontRight = new MockSpeedController(); + private final MockSpeedController m_rdRearRight = new MockSpeedController(); + private final MockSpeedController m_frontLeft = new MockSpeedController(); + private final MockSpeedController m_rearLeft = new MockSpeedController(); + private final MockSpeedController m_frontRight = new MockSpeedController(); + private final MockSpeedController m_rearRight = new MockSpeedController(); + private final RobotDrive m_robotDrive = + new RobotDrive(m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight, m_rdRearRight); + private final DifferentialDrive m_differentialDrive = + new DifferentialDrive(m_frontLeft, m_frontRight); + private final MecanumDrive m_mecanumDrive = + new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight); + + private final double[] m_testJoystickValues = {1.0, 0.9, 0.5, 0.01, 0.0, -0.01, -0.5, -0.9, + -1.0}; + private final double[] m_testGyroValues = {0, 30, 45, 90, 135, 180, 225, 270, 305, 360, 540, + -45, -90, -135, -180, -225, -270, -305, -360, -540}; + + @BeforeEach + void setUp() { + m_differentialDrive.setDeadband(0.0); + m_differentialDrive.setSafetyEnabled(false); + m_mecanumDrive.setDeadband(0.0); + m_mecanumDrive.setSafetyEnabled(false); + m_robotDrive.setSafetyEnabled(false); + } + + @Test + public void testTankDriveSquared() { + for (double leftJoystick : m_testJoystickValues) { + for (double rightJoystick : m_testJoystickValues) { + m_robotDrive.tankDrive(leftJoystick, rightJoystick); + m_differentialDrive.tankDrive(leftJoystick, rightJoystick); + assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01, + "Left Motor squared didn't match. Left Joystick: " + leftJoystick + " Right Joystick: " + + rightJoystick); + assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01, + "Right Motor squared didn't match. Left Joystick: " + leftJoystick + " Right Joystick: " + + rightJoystick); + } + } + } + + @Test + void testTankDrive() { + for (double leftJoystick : m_testJoystickValues) { + for (double rightJoystick : m_testJoystickValues) { + m_robotDrive.tankDrive(leftJoystick, rightJoystick, false); + m_differentialDrive.tankDrive(leftJoystick, rightJoystick, false); + assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01, + "Left Motor didn't match. Left Joystick: " + leftJoystick + " Right Joystick: " + + rightJoystick); + assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01, + "Right Motor didn't match. Left Joystick: " + leftJoystick + " Right Joystick: " + + rightJoystick); + } + } + } + + @Test + void testArcadeDriveSquared() { + for (double moveJoystick : m_testJoystickValues) { + for (double rotateJoystick : m_testJoystickValues) { + m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick); + m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick); + assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01, + "Left Motor squared didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: " + + rotateJoystick); + assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01, + "Right Motor squared didn't match. Move Joystick: " + moveJoystick + + " Rotate Joystick: " + rotateJoystick); + } + } + } + + @Test + void testArcadeDrive() { + for (double moveJoystick : m_testJoystickValues) { + for (double rotateJoystick : m_testJoystickValues) { + m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick, false); + m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick, false); + assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01, + "Left Motor didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: " + + rotateJoystick); + assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01, + "Right Motor didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: " + + rotateJoystick); + } + } + } + + @Test + void testMecanumPolar() { + for (double magnitudeJoystick : m_testJoystickValues) { + for (double directionJoystick : m_testGyroValues) { + for (double rotationJoystick : m_testJoystickValues) { + m_robotDrive.mecanumDrive_Polar(magnitudeJoystick, directionJoystick, rotationJoystick); + m_mecanumDrive.drivePolar(magnitudeJoystick, directionJoystick, rotationJoystick); + assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01, + "Left Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick + + " Direction Joystick: " + directionJoystick + " RotationJoystick: " + + rotationJoystick); + assertEquals(m_rdFrontRight.get(), -m_frontRight.get(), 0.01, + "Right Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick + + " Direction Joystick: " + directionJoystick + " RotationJoystick: " + + rotationJoystick); + assertEquals(m_rdRearLeft.get(), m_rearLeft.get(), 0.01, + "Left Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick + + " Direction Joystick: " + directionJoystick + " RotationJoystick: " + + rotationJoystick); + assertEquals(m_rdRearRight.get(), -m_rearRight.get(), 0.01, + "Right Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick + + " Direction Joystick: " + directionJoystick + " RotationJoystick: " + + rotationJoystick); + } + } + } + } + + @Test + @SuppressWarnings("checkstyle:LocalVariableName") + void testMecanumCartesian() { + for (double x_Joystick : m_testJoystickValues) { + for (double y_Joystick : m_testJoystickValues) { + for (double rotationJoystick : m_testJoystickValues) { + for (double gyroValue : m_testGyroValues) { + m_robotDrive.mecanumDrive_Cartesian(x_Joystick, y_Joystick, rotationJoystick, + gyroValue); + m_mecanumDrive.driveCartesian(x_Joystick, -y_Joystick, rotationJoystick, -gyroValue); + assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01, + "Left Front Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: " + + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue); + assertEquals(m_rdFrontRight.get(), -m_frontRight.get(), 0.01, + "Right Front Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: " + + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue); + assertEquals(m_rdRearLeft.get(), m_rearLeft.get(), 0.01, + "Left Rear Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: " + + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue); + assertEquals(m_rdRearRight.get(), -m_rearRight.get(), 0.01, + "Right Rear Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: " + + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue); + } + } + } + } + } +} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriveTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriveTest.java deleted file mode 100644 index 10fa23d1c1..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriveTest.java +++ /dev/null @@ -1,183 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj; - -import java.util.logging.Logger; - -import org.junit.BeforeClass; -import org.junit.Test; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.drive.MecanumDrive; -import edu.wpi.first.wpilibj.test.AbstractComsSetup; - -import static org.junit.Assert.assertEquals; - -/** - * Tests DifferentialDrive and MecanumDrive. - */ -public class DriveTest extends AbstractComsSetup { - private static final Logger logger = Logger.getLogger(DriveTest.class.getName()); - - private static MockSpeedController m_rdFrontLeft = new MockSpeedController(); - private static MockSpeedController m_rdRearLeft = new MockSpeedController(); - private static MockSpeedController m_rdFrontRight = new MockSpeedController(); - private static MockSpeedController m_rdRearRight = new MockSpeedController(); - private static MockSpeedController m_frontLeft = new MockSpeedController(); - private static MockSpeedController m_rearLeft = new MockSpeedController(); - private static MockSpeedController m_frontRight = new MockSpeedController(); - private static MockSpeedController m_rearRight = new MockSpeedController(); - private static RobotDrive m_robotDrive = - new RobotDrive(m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight, m_rdRearRight); - private static DifferentialDrive m_differentialDrive = - new DifferentialDrive(m_frontLeft, m_frontRight); - private static MecanumDrive m_mecanumDrive = - new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight); - - private final double[] m_testJoystickValues = {1.0, 0.9, 0.5, 0.01, 0.0, -0.01, -0.5, -0.9, - -1.0}; - private final double[] m_testGyroValues = {0, 30, 45, 90, 135, 180, 225, 270, 305, 360, 540, - -45, -90, -135, -180, -225, -270, -305, -360, -540}; - - @BeforeClass - public static void before() { - m_differentialDrive.setDeadband(0.0); - m_differentialDrive.setSafetyEnabled(false); - m_mecanumDrive.setDeadband(0.0); - m_mecanumDrive.setSafetyEnabled(false); - m_robotDrive.setSafetyEnabled(false); - } - - @Test - public void testTankDriveSquared() { - for (double leftJoystick : m_testJoystickValues) { - for (double rightJoystick : m_testJoystickValues) { - m_robotDrive.tankDrive(leftJoystick, rightJoystick); - m_differentialDrive.tankDrive(leftJoystick, rightJoystick); - assertEquals("Left Motor squared didn't match. Left Joystick: " + leftJoystick - + " Right Joystick: " + rightJoystick + " ", m_rdFrontLeft.get(), m_frontLeft.get(), - 0.01); - assertEquals("Right Motor squared didn't match. Left Joystick: " + leftJoystick - + " Right Joystick: " + rightJoystick + " ", m_rdFrontRight.get(), m_frontRight.get(), - 0.01); - } - } - } - - @Test - public void testTankDrive() { - for (double leftJoystick : m_testJoystickValues) { - for (double rightJoystick : m_testJoystickValues) { - m_robotDrive.tankDrive(leftJoystick, rightJoystick, false); - m_differentialDrive.tankDrive(leftJoystick, rightJoystick, false); - assertEquals("Left Motor didn't match. Left Joystick: " + leftJoystick - + " Right Joystick: " + rightJoystick + " ", m_rdFrontLeft.get(), m_frontLeft.get(), - 0.01); - assertEquals("Right Motor didn't match. Left Joystick: " + leftJoystick - + " Right Joystick: " + rightJoystick + " ", m_rdFrontRight.get(), m_frontRight.get(), - 0.01); - } - } - } - - @Test - public void testArcadeDriveSquared() { - for (double moveJoystick : m_testJoystickValues) { - for (double rotateJoystick : m_testJoystickValues) { - m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick); - m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick); - assertEquals("Left Motor squared didn't match. Move Joystick: " + moveJoystick - + " Rotate Joystick: " + rotateJoystick + " ", m_rdFrontLeft.get(), m_frontLeft.get(), - 0.01); - assertEquals("Right Motor squared didn't match. Move Joystick: " + moveJoystick - + " Rotate Joystick: " + rotateJoystick + " ", m_rdFrontRight.get(), - m_frontRight.get(), 0.01); - } - } - } - - @Test - public void testArcadeDrive() { - for (double moveJoystick : m_testJoystickValues) { - for (double rotateJoystick : m_testJoystickValues) { - m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick, false); - m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick, false); - assertEquals("Left Motor didn't match. Move Joystick: " + moveJoystick - + " Rotate Joystick: " + rotateJoystick + " ", m_rdFrontLeft.get(), m_frontLeft.get(), - 0.01); - assertEquals("Right Motor didn't match. Move Joystick: " + moveJoystick - + " Rotate Joystick: " + rotateJoystick + " ", m_rdFrontRight.get(), - m_frontRight.get(), 0.01); - } - } - } - - @Test - public void testMecanumPolar() { - System.out.println("magnitudeJoystick, directionJoystick , rotationJoystick, " - + "m_rdFrontLeft, m_frontLeft, m_rdFrontRight, m_frontRight, m_rdRearLeft, " - + "m_rearLeft, m_rdRearRight, m_rearRight"); - for (double magnitudeJoystick : m_testJoystickValues) { - for (double directionJoystick : m_testGyroValues) { - for (double rotationJoystick : m_testJoystickValues) { - m_robotDrive.mecanumDrive_Polar(magnitudeJoystick, directionJoystick, rotationJoystick); - m_mecanumDrive.drivePolar(magnitudeJoystick, directionJoystick, rotationJoystick); - assertEquals("Left Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick - + " Direction Joystick: " + directionJoystick + " RotationJoystick: " - + rotationJoystick, m_rdFrontLeft.get(), m_frontLeft.get(), 0.01); - assertEquals("Right Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick - + " Direction Joystick: " + directionJoystick + " RotationJoystick: " - + rotationJoystick, m_rdFrontRight.get(), -m_frontRight.get(), 0.01); - assertEquals("Left Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick - + " Direction Joystick: " + directionJoystick + " RotationJoystick: " - + rotationJoystick, m_rdRearLeft.get(), m_rearLeft.get(), 0.01); - assertEquals("Right Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick - + " Direction Joystick: " + directionJoystick + " RotationJoystick: " - + rotationJoystick, m_rdRearRight.get(), -m_rearRight.get(), 0.01); - } - } - } - } - - @Test - @SuppressWarnings("checkstyle:LocalVariableName") - public void testMecanumCartesian() { - for (double x_Joystick : m_testJoystickValues) { - for (double y_Joystick : m_testJoystickValues) { - for (double rotationJoystick : m_testJoystickValues) { - for (double gyroValue : m_testGyroValues) { - m_robotDrive.mecanumDrive_Cartesian(x_Joystick, y_Joystick, rotationJoystick, - gyroValue); - m_mecanumDrive.driveCartesian(x_Joystick, -y_Joystick, rotationJoystick, -gyroValue); - assertEquals("Left Front Motor didn't match. X Joystick: " + x_Joystick - + " Y Joystick: " + y_Joystick + " RotationJoystick: " - + rotationJoystick + " Gyro: " + gyroValue, m_rdFrontLeft.get(), - m_frontLeft.get(), 0.01); - assertEquals("Right Front Motor didn't match. X Joystick: " + x_Joystick - + " Y Joystick: " + y_Joystick + " RotationJoystick: " - + rotationJoystick + " Gyro: " + gyroValue, m_rdFrontRight.get(), - -m_frontRight.get(), 0.01); - assertEquals("Left Rear Motor didn't match. X Joystick: " + x_Joystick - + " Y Joystick: " + y_Joystick + " RotationJoystick: " - + rotationJoystick + " Gyro: " + gyroValue, m_rdRearLeft.get(), - m_rearLeft.get(), 0.01); - assertEquals("Right Rear Motor didn't match. X Joystick: " + x_Joystick - + " Y Joystick: " + y_Joystick + " RotationJoystick: " - + rotationJoystick + " Gyro: " + gyroValue, m_rdRearRight.get(), - -m_rearRight.get(), 0.01); - } - } - } - } - } - - @Override - protected Logger getClassLogger() { - return logger; - } -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java index b323d78ae1..9ab68500c9 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java @@ -20,7 +20,7 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite; @RunWith(Suite.class) @SuiteClasses({AnalogCrossConnectTest.class, AnalogPotentiometerTest.class, BuiltInAccelerometerTest.class, ConstantsPortsTest.class, CounterTest.class, - DigitalGlitchFilterTest.class, DIOCrossConnectTest.class, DriveTest.class, + DigitalGlitchFilterTest.class, DIOCrossConnectTest.class, DriverStationTest.class, EncoderTest.class, GyroTest.class, MotorEncoderTest.class, MotorInvertingTest.class, PCMTest.class, PDPTest.class, PIDTest.class, RelayCrossConnectTest.class, SampleTest.class, TimerTest.class})