From d354ac078bbbd53102bd3e134e159d1e8aaeff87 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sun, 29 Mar 2020 00:10:06 -0600 Subject: [PATCH] Create Mock PigeonIMU and write more Unit tests - Test to make sure the RoboyGyro class knows what kind of gyro its using - Test to make sure all the getters for the gyro class work (at least for pigeon) --- .../java/frc4388/mocks/MockPigeonIMU.java | 54 ++++++++++++ src/main/java/frc4388/utility/RobotGyro.java | 4 +- .../robot/subsystems/LEDSubsystemTest.java | 6 -- .../frc4388/utility/RobotGyroUtilityTest.java | 83 ++++++++++++++++--- 4 files changed, 128 insertions(+), 19 deletions(-) create mode 100644 src/main/java/frc4388/mocks/MockPigeonIMU.java diff --git a/src/main/java/frc4388/mocks/MockPigeonIMU.java b/src/main/java/frc4388/mocks/MockPigeonIMU.java new file mode 100644 index 0000000..ecddde7 --- /dev/null +++ b/src/main/java/frc4388/mocks/MockPigeonIMU.java @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-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 frc4388.mocks; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.sensors.PigeonIMU; + +/** + * Add your docs here. + */ +public class MockPigeonIMU extends PigeonIMU { + public int m_deviceNumber; + public double currentYaw; + public double currentPitch; + public double currentRoll; + + public MockPigeonIMU(int deviceNumber) { + super(deviceNumber); + m_deviceNumber = deviceNumber; + } + + @Override + public ErrorCode setYaw(double angleDeg) { + currentYaw = angleDeg; + return ErrorCode.OK; + } + + /** + * @param currentPitch the Pitch to set + */ + public void setCurrentPitch(double currentPitch) { + this.currentPitch = currentPitch; + } + + /** + * @param currentRoll the Roll to set + */ + public void setCurrentRoll(double currentRoll) { + this.currentRoll = currentRoll; + } + + @Override + public ErrorCode getYawPitchRoll(double[] ypr_deg) { + ypr_deg[0] = currentYaw; + ypr_deg[1] = currentPitch; + ypr_deg[2] = currentRoll; + return ErrorCode.OK; + } +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 6d5ba5c..d7d1adf 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -20,8 +20,8 @@ import edu.wpi.first.wpiutil.math.MathUtil; public class RobotGyro extends GyroBase { private RobotTime m_robotTime = RobotTime.getInstance(); - private PigeonIMU m_pigeon; - private AHRS m_navX; + private PigeonIMU m_pigeon = null; + private AHRS m_navX = null; public boolean m_isGyroAPigeon; //true if pigeon, false if navX private double m_lastPigeonAngle; diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java index c4ce771..14c1ce8 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -31,12 +31,6 @@ public class LEDSubsystemTest { led.setPattern(LEDPatterns.BLUE_BREATH); assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); - led.setPattern(LEDPatterns.C1_SCANNER); - assertEquals(LEDPatterns.C1_SCANNER.getValue(), led.getPattern().getValue(), 0.0001); - - led.setPattern(LEDPatterns.C1_CHASE); - assertEquals(LEDPatterns.C1_CHASE.getValue(), led.getPattern().getValue(), 0.0001); - led.setPattern(LEDPatterns.SOLID_BLACK); assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); } diff --git a/src/test/java/frc4388/utility/RobotGyroUtilityTest.java b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java index 74c589c..7bcc8c3 100644 --- a/src/test/java/frc4388/utility/RobotGyroUtilityTest.java +++ b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java @@ -13,26 +13,87 @@ import org.junit.*; import edu.wpi.first.wpilibj.*; import com.ctre.phoenix.sensors.PigeonIMU; +import com.kauailabs.navx.frc.AHRS; + +import frc4388.mocks.MockPigeonIMU; +import frc4388.robot.Constants.DriveConstants; import frc4388.utility.RobotGyro; /** * Add your docs here. */ public class RobotGyroUtilityTest { - // Arrange - RobotGyro gyro = new RobotGyro(mock(PigeonIMU.class)); + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + RobotGyro gyroPigeon = new RobotGyro(pigeon); + AHRS navX = mock(AHRS.class); + RobotGyro gyroNavX = new RobotGyro(navX); + + // TODO UNTESTED: calibrate(), getRate(), and most functions for NavX + + @Test + public void testConfig() { + assertEquals(true, gyroPigeon.m_isGyroAPigeon); + assertEquals(pigeon, gyroPigeon.getPigeon()); + assertEquals(null, gyroPigeon.getNavX()); + assertEquals(false, gyroNavX.m_isGyroAPigeon); + assertEquals(navX, gyroNavX.getNavX()); + assertEquals(null, gyroNavX.getPigeon()); + } @Test public void testHeading() { // Act & Assert - assertEquals(-90, gyro.getHeading(270), 0.0001); - assertEquals(-45, gyro.getHeading(315), 0.0001); - assertEquals(-60, gyro.getHeading(-60), 0.0001); - assertEquals(30, gyro.getHeading(30), 0.0001); - assertEquals(0, gyro.getHeading(0), 0.0001); - assertEquals(180, gyro.getHeading(180), 0.0001); - assertEquals(-180, gyro.getHeading(-180), 0.0001); - assertEquals(97, gyro.getHeading(1537), 0.0001); - assertEquals(99, gyro.getHeading(-2781), 0.0001); + assertEquals(-90, gyroPigeon.getHeading(270), 0.0001); + assertEquals(-45, gyroPigeon.getHeading(315), 0.0001); + assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001); + assertEquals(30, gyroPigeon.getHeading(30), 0.0001); + assertEquals(0, gyroPigeon.getHeading(0), 0.0001); + assertEquals(180, gyroPigeon.getHeading(180), 0.0001); + assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001); + assertEquals(97, gyroPigeon.getHeading(1537), 0.0001); + assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001); + } + + @Test + public void testYawPitchRoll() { + assertEquals(0, gyroPigeon.getAngle(), 0.0001); + + pigeon.setYaw(40); + assertEquals(40, gyroPigeon.getAngle(), 0.0001); + + gyroPigeon.reset(); + assertEquals(0, gyroPigeon.getAngle(), 0.0001); + + pigeon.setYaw(-1457); + pigeon.setCurrentPitch(100); + pigeon.setCurrentRoll(100); + assertEquals(-1457, gyroPigeon.getAngle(), 0.0001); + assertEquals(90, gyroPigeon.getPitch(), 0.0001); + assertEquals(90, gyroPigeon.getRoll(), 0.0001); + + pigeon.setCurrentPitch(45); + pigeon.setCurrentRoll(45); + assertEquals(45, gyroPigeon.getPitch(), 0.0001); + assertEquals(45, gyroPigeon.getRoll(), 0.0001); + + pigeon.setCurrentPitch(0); + pigeon.setCurrentRoll(0); + assertEquals(0, gyroPigeon.getPitch(), 0.0001); + assertEquals(0, gyroPigeon.getRoll(), 0.0001); + + pigeon.setCurrentPitch(-60); + pigeon.setCurrentRoll(-60); + assertEquals(-60, gyroPigeon.getPitch(), 0.0001); + assertEquals(-60, gyroPigeon.getRoll(), 0.0001); + + pigeon.setCurrentPitch(-90); + pigeon.setCurrentRoll(-90); + assertEquals(-90, gyroPigeon.getPitch(), 0.0001); + assertEquals(-90, gyroPigeon.getRoll(), 0.0001); + + pigeon.setCurrentPitch(-100); + pigeon.setCurrentRoll(-100); + assertEquals(-90, gyroPigeon.getPitch(), 0.0001); + assertEquals(-90, gyroPigeon.getRoll(), 0.0001); } }