Merge "Gyro: Add support for fixed calibration (artf4124)."

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java

@@ -40,11 +40,17 @@ public class GyroTest extends AbstractComsSetup {
 
   @After
   public void tearDown() throws Exception {
-    tpcam.reset();
     tpcam.teardown();
   }
 
   @Test
+  public void testAllGyroTests() {
+    testInitial();
+    testGyroAngle();
+    testDeviationOverTime();
+    testGyroAngleCalibratedParameters();
+  }
+
   public void testInitial() {
     double angle = tpcam.getGyro().getAngle();
     assertEquals(errorMessage(angle, 0), 0, angle, .5);
@@ -56,7 +62,6 @@ public class GyroTest extends AbstractComsSetup {
    * for so setAngle is significantly off. This has been calibrated for the
    * servo on the rig.
    */
-  @Test
   public void testGyroAngle() {
     // Set angle
     for (int i = 0; i < 5; i++) {
@@ -85,10 +90,9 @@ public class GyroTest extends AbstractComsSetup {
     assertEquals(errorMessage(diff, TEST_ANGLE), TEST_ANGLE, angle, 10);
   }
 
-  @Test
   public void testDeviationOverTime() {
     // Make sure that the test isn't influenced by any previous motions.
-    Timer.delay(0.25);
+    Timer.delay(0.5);
     tpcam.getGyro().reset();
     Timer.delay(0.25);
     double angle = tpcam.getGyro().getAngle();
@@ -98,6 +102,46 @@ public class GyroTest extends AbstractComsSetup {
     assertEquals("After 5 seconds " + errorMessage(angle, 0), 0, angle, 1);
   }
 
+  /**
+   *  Gets calibrated parameters from already calibrated gyro, allocates a
+   *  new gyro with the center and offset parameters, and re-runs the test.
+   */
+  public void testGyroAngleCalibratedParameters() {
+    // Get calibrated parameters to make new Gyro with parameters
+    double cOffset = tpcam.getGyro().getOffset();
+    int cCenter = tpcam.getGyro().getCenter();
+    tpcam.freeGyro();
+    tpcam.setupGyroParam(cCenter, cOffset);
+
+    // Repeat tests
+    // Set angle
+    for (int i = 0; i < 5; i++) {
+      tpcam.getPan().set(0);
+      Timer.delay(.1);
+    }
+
+    Timer.delay(0.5);
+
+    // Reset for setup
+    tpcam.getGyroParam().reset();
+    Timer.delay(0.5);
+
+    // Perform test
+    for (int i = 0; i < 53; i++) {
+      tpcam.getPan().set(i / 100.0);
+      Timer.delay(0.05);
+    }
+    Timer.delay(1.2);
+
+    double angle = tpcam.getGyroParam().getAngle();
+
+    double difference = TEST_ANGLE - angle;
+
+    double diff = Math.abs(difference);
+
+    assertEquals(errorMessage(diff, TEST_ANGLE), TEST_ANGLE, angle, 10);
+  }
+
   private String errorMessage(double difference, double target) {
     return "Gyro angle skewed " + difference + " deg away from target " + target;
   }

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java

@@ -8,7 +8,7 @@ package edu.wpi.first.wpilibj.fixtures;
 
 import java.util.logging.Logger;
 
-import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.Timer;
 
@@ -23,13 +23,16 @@ public abstract class TiltPanCameraFixture implements ITestFixture {
   public static final Logger logger = Logger.getLogger(TiltPanCameraFixture.class.getName());
 
   public static final double RESET_TIME = 2.0;
-  private Gyro gyro;
+  private AnalogGyro gyro;
+  private AnalogGyro gyroParam;
   private Servo tilt;
   private Servo pan;
   private boolean initialized = false;
 
 
-  protected abstract Gyro giveGyro();
+  protected abstract AnalogGyro giveGyro();
+
+  protected abstract AnalogGyro giveGyroParam(int center, double offset);
 
   protected abstract Servo giveTilt();
 
@@ -73,18 +76,32 @@ public abstract class TiltPanCameraFixture implements ITestFixture {
     return pan;
   }
 
-  public Gyro getGyro() {
+  public AnalogGyro getGyro() {
     return gyro;
   }
 
+  public AnalogGyro getGyroParam() {
+    return gyroParam;
+  }
+
+  // Do not call unless all other instances of Gyro have been deallocated
+  public void setupGyroParam(int center, double offset) {
+    gyroParam = giveGyroParam(center, offset);
+    gyroParam.reset();
+  }
+  public void freeGyro() {
+    gyro.free();
+    gyro = null;
+  }
+
   @Override
   public boolean teardown() {
     tilt.free();
     tilt = null;
     pan.free();
     pan = null;
-    gyro.free();
-    gyro = null;
+    gyroParam.free();
+    gyroParam = null;
     return true;
   }
 

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java

@@ -253,12 +253,19 @@ public final class TestBench {
   public TiltPanCameraFixture getTiltPanCam() {
     TiltPanCameraFixture tpcam = new TiltPanCameraFixture() {
       @Override
-      protected Gyro giveGyro() {
+      protected AnalogGyro giveGyro() {
         AnalogGyro gyro = new AnalogGyro(kGyroChannel);
         gyro.setSensitivity(kGyroSensitivity);
         return gyro;
       }
 
+      @Override
+      protected AnalogGyro giveGyroParam(int center, double offset) {
+        AnalogGyro gyro = new AnalogGyro(kGyroChannel, center, offset);
+        gyro.setSensitivity(kGyroSensitivity);
+        return gyro;
+      }
+
       @Override
       protected Servo giveTilt() {
         return new Servo(kTiltServoChannel);