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

wpilibc/Athena/include/AnalogGyro.h

@@ -42,13 +42,18 @@ class AnalogGyro : public GyroBase {
       " with a channel number or passing a shared_ptr instead.")
   explicit AnalogGyro(AnalogInput *channel);
   explicit AnalogGyro(std::shared_ptr<AnalogInput> channel);
+  AnalogGyro(int32_t channel, uint32_t center, float offset);
+  AnalogGyro(std::shared_ptr<AnalogInput> channel, uint32_t center, float offset);
   virtual ~AnalogGyro() = default;
 
   float GetAngle() const override;
   double GetRate() const override;
+  virtual uint32_t GetCenter() const;
+  virtual float GetOffset() const;
   void SetSensitivity(float voltsPerDegreePerSecond);
   void SetDeadband(float volts);
   void Reset() override;
+  virtual void InitGyro();
   void Calibrate() override;
 
   std::string GetSmartDashboardType() const override;

wpilibc/Athena/src/AnalogGyro.cpp

@@ -56,32 +56,49 @@ AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
   if (channel == nullptr) {
     wpi_setWPIError(NullParameter);
   } else {
-    if (!m_analog->IsAccumulatorChannel()) {
-      wpi_setWPIErrorWithContext(ParameterOutOfRange,
-                                 " channel (must be accumulator channel)");
-      m_analog = nullptr;
-      return;
-    }
-
-    m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
-    m_analog->SetAverageBits(kAverageBits);
-    m_analog->SetOversampleBits(kOversampleBits);
-    float sampleRate =
-        kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
-    m_analog->SetSampleRate(sampleRate);
-    Wait(0.1);
-
-    SetDeadband(0.0f);
-
-    SetPIDSourceType(PIDSourceType::kDisplacement);
-
-    HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
-    LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetChannel(), this);
-
+    InitGyro();
     Calibrate();
   }
 }
 
+/**
+ * Gyro constructor using the Analog Input channel number with parameters for
+ * presetting the center and offset values. Bypasses calibration.
+ *
+ * @param channel The analog channel the gyro is connected to. Gyros
+ *        can only be used on on-board Analog Inputs 0-1.
+ * @param center Preset uncalibrated value to use as the accumulator center value.
+ * @param offset Preset uncalibrated value to use as the gyro offset.
+ */
+AnalogGyro::AnalogGyro(int32_t channel, uint32_t center, float offset) {
+  m_analog = std::make_shared<AnalogInput>(channel);
+  InitGyro();
+  m_center = center;
+  m_offset = offset;
+  m_analog->SetAccumulatorCenter(m_center);
+  m_analog->ResetAccumulator();
+}
+
+/**
+ * Gyro constructor with a precreated AnalogInput object and calibrated parameters.
+ * Use this constructor when the analog channel needs to be shared.
+ * This object will not clean up the AnalogInput object when using this
+ * constructor
+ * @param channel A pointer to the AnalogInput object that the gyro is
+ * connected to.
+ */
+AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel, uint32_t center, float offset) : m_analog(channel) {
+  if (channel == nullptr) {
+    wpi_setWPIError(NullParameter);
+  } else {
+    InitGyro();
+    m_center = center;
+    m_offset = offset;
+    m_analog->SetAccumulatorCenter(m_center);
+    m_analog->ResetAccumulator();
+  }
+}
+
 /**
  * Reset the gyro.
  * Resets the gyro to a heading of zero. This can be used if there is
@@ -93,6 +110,35 @@ void AnalogGyro::Reset() {
   m_analog->ResetAccumulator();
 }
 
+/**
+ * Initialize the gyro.  Calibration is handled by Calibrate().
+ */
+void AnalogGyro::InitGyro() {
+  if (StatusIsFatal()) return;
+
+  if (!m_analog->IsAccumulatorChannel()) {
+    wpi_setWPIErrorWithContext(ParameterOutOfRange,
+                               " channel (must be accumulator channel)");
+    m_analog = nullptr;
+    return;
+  }
+
+  m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
+  m_analog->SetAverageBits(kAverageBits);
+  m_analog->SetOversampleBits(kOversampleBits);
+  float sampleRate =
+      kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
+  m_analog->SetSampleRate(sampleRate);
+  Wait(0.1);
+
+  SetDeadband(0.0f);
+
+  SetPIDSourceType(PIDSourceType::kDisplacement);
+
+  HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
+  LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetChannel(), this);
+}
+
 /**
  * {@inheritDoc}
  */
@@ -160,6 +206,26 @@ double AnalogGyro::GetRate() const {
          ((1 << m_analog->GetOversampleBits()) * m_voltsPerDegreePerSecond);
 }
 
+/**
+ * Return the gyro offset value. If run after calibration,
+ * the offset value can be used as a preset later.
+ *
+ * @return the current offset value
+ */
+float AnalogGyro::GetOffset() const {
+  return m_offset;
+}
+
+/**
+ * Return the gyro center value. If run after calibration,
+ * the center value can be used as a preset later.
+ *
+ * @return the current center value
+ */
+uint32_t AnalogGyro::GetCenter() const {
+  return m_center;
+}
+
 /**
  * Set the gyro sensitivity.
  * This takes the number of volts/degree/second sensitivity of the gyro and uses

wpilibcIntegrationTests/src/TiltPanCameraTest.cpp

@@ -21,6 +21,7 @@ static constexpr double kTiltSetpoint45 = 0.45;
 static constexpr double kTiltSetpoint90 = 0.68;
 static constexpr double kTiltTime = 1.0;
 static constexpr double kAccelerometerTolerance = 0.2;
+static constexpr double kSensitivity = 0.013;
 
 /**
  * A fixture for the camera with two servos and a gyro
@@ -36,7 +37,7 @@ class TiltPanCameraTest : public testing::Test {
     // The gyro object blocks for 5 seconds in the constructor, so only
     // construct it once for the whole test case
     m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel);
-    m_gyro->SetSensitivity(0.013);
+    m_gyro->SetSensitivity(kSensitivity);
   }
 
   static void TearDownTestCase() { delete m_gyro; }
@@ -54,6 +55,10 @@ class TiltPanCameraTest : public testing::Test {
     m_gyro->Reset();
   }
 
+  void DefaultGyroAngle();
+  void GyroAngle();
+  void GyroCalibratedParameters();
+
   virtual void TearDown() override {
     delete m_tilt;
     delete m_pan;
@@ -66,7 +71,7 @@ AnalogGyro *TiltPanCameraTest::m_gyro = nullptr;
 /**
  * Test if the gyro angle defaults to 0 immediately after being reset.
  */
-TEST_F(TiltPanCameraTest, DefaultGyroAngle) {
+void TiltPanCameraTest::DefaultGyroAngle() {
   EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f);
 }
 
@@ -76,10 +81,10 @@ TEST_F(TiltPanCameraTest, DefaultGyroAngle) {
  * was designed for so setAngle is significantly off. This has been calibrated
  * for the servo on the rig.
  */
-TEST_F(TiltPanCameraTest, GyroAngle) {
+void TiltPanCameraTest::GyroAngle() {
   // Make sure that the gyro doesn't get jerked when the servo goes to zero.
   m_pan->SetAngle(0.0);
-  Wait(0.25);
+  Wait(0.5);
   m_gyro->Reset();
 
   for (int i = 0; i < 600; i++) {
@@ -94,6 +99,51 @@ TEST_F(TiltPanCameraTest, GyroAngle) {
       << kTestAngle << " degrees";
 }
 
+/**
+  * Gets calibrated parameters from previously calibrated gyro, allocates a new
+  * gyro with the given parameters for center and offset, and re-runs tests on
+  * the new gyro.
+  */
+void TiltPanCameraTest::GyroCalibratedParameters() {
+  uint32_t cCenter = m_gyro->GetCenter();
+  float cOffset = m_gyro->GetOffset();
+  delete m_gyro;
+  m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel, cCenter, cOffset);
+  m_gyro->SetSensitivity(kSensitivity);
+
+  // Default gyro angle test
+  // Accumulator needs a small amount of time to reset before being tested
+  m_gyro->Reset();
+  Wait(.001);
+  EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f);
+
+  // Gyro angle test
+  // Make sure that the gyro doesn't get jerked when the servo goes to zero.
+  m_pan->SetAngle(0.0);
+  Wait(0.5);
+  m_gyro->Reset();
+
+  for (int i = 0; i < 600; i++) {
+    m_pan->Set(i / 1000.0);
+    Wait(0.001);
+  }
+
+  double gyroAngle = m_gyro->GetAngle();
+
+  EXPECT_NEAR(gyroAngle, kTestAngle, 10.0)
+      << "Gyro measured " << gyroAngle << " degrees, servo should have turned "
+      << kTestAngle << " degrees";
+}
+
+/**
+ * Run all gyro tests in one function to make sure they are run in order.
+ */
+TEST_F(TiltPanCameraTest, TestAllGyroTests) {
+  DefaultGyroAngle();
+  GyroAngle();
+  GyroCalibratedParameters();
+}
+
 /**
  * Test if the accelerometer measures gravity along the correct axes when the
  * camera rotates

wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java

@@ -39,6 +39,27 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
   AccumulatorResult result;
   private PIDSourceType m_pidSource;
 
+  /**
+   * Initialize the gyro. Calibration is handled by calibrate().
+   */
+  public void initGyro() {
+    result = new AccumulatorResult();
+
+    m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
+    m_analog.setAverageBits(kAverageBits);
+    m_analog.setOversampleBits(kOversampleBits);
+    double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
+    AnalogInput.setGlobalSampleRate(sampleRate);
+    Timer.delay(0.1);
+
+    setDeadband(0.0);
+
+    setPIDSourceType(PIDSourceType.kDisplacement);
+
+    UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
+    LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this);
+  }
+
   /**
    * {@inheritDoc}
    */
@@ -81,25 +102,45 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
     if (m_analog == null) {
       throw new NullPointerException("AnalogInput supplied to Gyro constructor is null");
     }
-    result = new AccumulatorResult();
-
-    m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
-    m_analog.setAverageBits(kAverageBits);
-    m_analog.setOversampleBits(kOversampleBits);
-    double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
-    AnalogInput.setGlobalSampleRate(sampleRate);
-    Timer.delay(0.1);
-
-    setDeadband(0.0);
-
-    setPIDSourceType(PIDSourceType.kDisplacement);
-
-    UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
-    LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this);
-
+    initGyro();
     calibrate();
   }
 
+  /**
+   * Gyro constructor using the channel number along with parameters for
+   * presetting the center and offset values. Bypasses calibration.
+   * @param channel The analog channel the gyro is connected to. Gyros can only
+   *        be used on on-board channels 0-1.
+   * @param center Preset uncalibrated value to use as the accumulator center value.
+   * @param offset Preset uncalibrated value to use as the gyro offset.
+   */
+  public AnalogGyro(int channel, int center, double offset) {
+    this(new AnalogInput(channel), center, offset);
+    m_channelAllocated = true;
+  }
+
+  /**
+   * Gyro constructor with a precreated analog channel object along with
+   * parameters for presetting the center and offset values. Bypasses
+   * calibration.
+   * @param channel The analog channel the gyro is connected to. Gyros can only
+   *        be used on on-board channels 0-1.
+   * @param center Preset uncalibrated value to use as the accumulator center value.
+   * @param offset Preset uncalibrated value to use as the gyro offset.
+   */
+  public AnalogGyro(AnalogInput channel, int center, double offset) {
+    m_analog = channel;
+    if (m_analog == null) {
+      throw new NullPointerException("AnalogInput supplied to Gyro constructor is null");
+    }
+    initGyro();
+    m_offset = offset;
+    m_center = center;
+
+    m_analog.setAccumulatorCenter(m_center);
+    m_analog.resetAccumulator();
+  }
+
   /**
    * {@inheritDoc}
    */
@@ -151,6 +192,26 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
     }
   }
 
+  /**
+   * Return the gyro offset value set during calibration to
+   * use as a future preset
+   *
+   * @return the current offset value
+   */
+  public double getOffset() {
+    return m_offset;
+  }
+
+  /**
+   * Return the gyro center value set during calibration to
+   * use as a future preset
+   *
+   * @return the current center value
+   */
+  public int getCenter() {
+    return m_center;
+  }
+
   /**
    * Set the gyro sensitivity. This takes the number of volts/degree/second
    * sensitivity of the gyro and uses it in subsequent calculations to allow the

wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogInput.java

@@ -221,7 +221,7 @@ public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSend
   public void initAccumulator() {
     if (!isAccumulatorChannel()) {
       throw new AllocationException("Accumulators are only available on slot " + kAccumulatorSlot
-          + " on channels " + kAccumulatorChannels[0] + "," + kAccumulatorChannels[1]);
+          + " on channels " + kAccumulatorChannels[0] + ", " + kAccumulatorChannels[1]);
     }
     m_accumulatorOffset = 0;
     AnalogJNI.initAccumulator(m_port);

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);