Add tests for getRate() function in RobotGyro

src/main/java/frc4388/robot/RobotMap.java

@@ -33,6 +33,7 @@ public class RobotMap {
     public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
     void configureLEDMotorControllers() {
+        
     }
 
     /* Drive Subsystem */

src/main/java/frc4388/utility/RobotGyro.java

@@ -159,7 +159,7 @@ public class RobotGyro extends GyroBase {
     @Override
     public double getRate() {
         if (m_isGyroAPigeon) {
-            return m_deltaPigeonAngle / (m_robotTime.m_deltaTime * 1000);
+            return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
         } else {
             return m_navX.getRate();
         }

src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java

@@ -23,14 +23,18 @@ public class LEDSubsystemTest {
 
     @Test
     public void testPatterns() {
+        // TEST 1
         assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
 
+        // TEST 2
         led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
         assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
 
+        // TEST 3
         led.setPattern(LEDPatterns.BLUE_BREATH);
         assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
 
+        // TEST 4
         led.setPattern(LEDPatterns.SOLID_BLACK);
         assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
     }

src/test/java/frc4388/utility/RobotGyroUtilityTest.java

@@ -27,14 +27,18 @@ public class RobotGyroUtilityTest {
     RobotGyro gyroPigeon = new RobotGyro(pigeon);
     AHRS navX = mock(AHRS.class);
     RobotGyro gyroNavX = new RobotGyro(navX);
+    RobotTime robotTime = RobotTime.getInstance();
 
-    // TODO UNTESTED: calibrate(), getRate(), and most functions for NavX
+    // TODO UNTESTED: most functions for NavX
 
     @Test
     public void testConfig() {
+        // TEST 1
         assertEquals(true, gyroPigeon.m_isGyroAPigeon);
         assertEquals(pigeon, gyroPigeon.getPigeon());
         assertEquals(null, gyroPigeon.getNavX());
+
+        // TEST 2
         assertEquals(false, gyroNavX.m_isGyroAPigeon);
         assertEquals(navX, gyroNavX.getNavX());
         assertEquals(null, gyroNavX.getPigeon());
@@ -42,7 +46,7 @@ public class RobotGyroUtilityTest {
 
     @Test
     public void testHeading() {
-        // Act & Assert
+        // TESTS
         assertEquals(-90, gyroPigeon.getHeading(270), 0.0001);
         assertEquals(-45, gyroPigeon.getHeading(315), 0.0001);
         assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001);
@@ -55,15 +59,19 @@ public class RobotGyroUtilityTest {
     }
 
     @Test
-    public void testYawPitchRoll() {
+    public void testYawPitchRoll() {        
+        // TEST 1
         assertEquals(0, gyroPigeon.getAngle(), 0.0001);
 
+        // TEST 2
         pigeon.setYaw(40);
         assertEquals(40, gyroPigeon.getAngle(), 0.0001);
 
+        // TEST 3
         gyroPigeon.reset();
         assertEquals(0, gyroPigeon.getAngle(), 0.0001);
 
+        // TEST 4
         pigeon.setYaw(-1457);
         pigeon.setCurrentPitch(100);
         pigeon.setCurrentRoll(100);
@@ -71,29 +79,77 @@ public class RobotGyroUtilityTest {
         assertEquals(90, gyroPigeon.getPitch(), 0.0001);
         assertEquals(90, gyroPigeon.getRoll(), 0.0001);
 
+        // TEST 5
         pigeon.setCurrentPitch(45);
         pigeon.setCurrentRoll(45);
         assertEquals(45, gyroPigeon.getPitch(), 0.0001);
         assertEquals(45, gyroPigeon.getRoll(), 0.0001);
 
+        // TEST 6
         pigeon.setCurrentPitch(0);
         pigeon.setCurrentRoll(0);
         assertEquals(0, gyroPigeon.getPitch(), 0.0001);
         assertEquals(0, gyroPigeon.getRoll(), 0.0001);
 
+        // TEST 7
         pigeon.setCurrentPitch(-60);
         pigeon.setCurrentRoll(-60);
         assertEquals(-60, gyroPigeon.getPitch(), 0.0001);
         assertEquals(-60, gyroPigeon.getRoll(), 0.0001);
 
+        // TEST 8
         pigeon.setCurrentPitch(-90);
         pigeon.setCurrentRoll(-90);
         assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
         assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
 
+        // TEST 9
         pigeon.setCurrentPitch(-100);
         pigeon.setCurrentRoll(-100);
         assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
         assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
     }
+
+    @Test
+    public void testRates() {
+        // SETUP
+        pigeon.setYaw(0);
+        gyroPigeon.updatePigeonDeltas();
+
+        // TEST 1
+        robotTime.m_deltaTime = 5;
+        pigeon.setYaw(0);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(0, gyroPigeon.getRate(), 1);
+
+        // TEST 2
+        robotTime.m_deltaTime = 5;
+        pigeon.setYaw(90);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(18000, gyroPigeon.getRate(), 0.001);
+
+        // TEST 3
+        robotTime.m_deltaTime = 5;
+        pigeon.setYaw(90);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(0, gyroPigeon.getRate(), 0.001);
+
+        // TEST 4
+        robotTime.m_deltaTime = 3;
+        pigeon.setYaw(-30);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(-40000, gyroPigeon.getRate(), 0.001);
+
+        // TEST 5
+        robotTime.m_deltaTime = 6;
+        pigeon.setYaw(690);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(120000, gyroPigeon.getRate(), 0.001);
+    }
+
+    private void wait(int millis) {
+        try {
+            Thread.sleep(millis);
+        } catch (Exception e) {}
+    }
 }

src/test/java/frc4388/utility/RobotTimeUtilityTest.java

@@ -21,29 +21,34 @@ public class RobotTimeUtilityTest {
 
     @Test
     public void testUpdateTimes() {
+        // SETUP
         long lastTime;
+        robotTime.m_deltaTime = 0;
+        robotTime.m_robotTime = 0;
+        robotTime.m_lastRobotTime = 0;
+        robotTime.m_frameNumber = 0;
+        robotTime.endMatchTime();
+        robotTime.m_lastMatchTime = 0;
 
-        // Initialisation
+        // TEST 1
         assertEquals(0, robotTime.m_deltaTime);
         assertEquals(0, robotTime.m_robotTime);
         assertEquals(0, robotTime.m_lastRobotTime);
         assertEquals(0, robotTime.m_frameNumber);
         lastTime = robotTime.m_robotTime;
 
+        // TEST 2
         wait(1);
         robotTime.updateTimes();
-
-        // First Frame
         assertEquals(true, robotTime.m_deltaTime > 0);
         assertEquals(true, robotTime.m_robotTime > 0);
         assertEquals(lastTime, robotTime.m_lastRobotTime);
         assertEquals(1, robotTime.m_frameNumber);
         lastTime = robotTime.m_robotTime;
 
+        // TEST 3
         wait(1);
         robotTime.updateTimes();
-
-        // Second Frame
         assertEquals(true, robotTime.m_deltaTime > 0);
         assertEquals(true, robotTime.m_robotTime > 0);
         assertEquals(lastTime, robotTime.m_lastRobotTime);
@@ -52,35 +57,33 @@ public class RobotTimeUtilityTest {
 
     @Test
     public void testMatchTime() {
+        // SETUP
         long lastTime;
 
-        // Second Frame
+        // TEST 1
         assertEquals(0, robotTime.m_matchTime);
         assertEquals(0, robotTime.m_lastMatchTime);
         lastTime = robotTime.m_matchTime;
 
+        // TEST 2
         robotTime.startMatchTime();
         wait(1);
         robotTime.updateTimes();
-
-        // Third Frame
         assertEquals(true, robotTime.m_matchTime > 0);
         assertEquals(lastTime, robotTime.m_lastMatchTime);
         lastTime = robotTime.m_matchTime;
         
+        // TEST 3
         wait(1);
         robotTime.updateTimes();
         robotTime.endMatchTime();
-
-        // Fourth Frame
         assertEquals(0, robotTime.m_matchTime);
         assertEquals(lastTime, robotTime.m_lastMatchTime);
         lastTime = robotTime.m_matchTime;
 
+        // TEST 4
         wait(1);
         robotTime.updateTimes();
-
-        // Fifth Frame
         assertEquals(0, robotTime.m_matchTime);
         assertEquals(lastTime, robotTime.m_lastMatchTime);
     }