Merge "Moved gyro calibration into a separate function so teams can recalibrate at any time after construction"

wpilibc/Athena/include/AnalogGyro.h

@@ -49,7 +49,7 @@ class AnalogGyro : public GyroBase {
   void SetSensitivity(float voltsPerDegreePerSecond);
   void SetDeadband(float volts);
   void Reset() override;
-  void InitGyro() override;
+  void Calibrate() override;
 
   std::string GetSmartDashboardType() const override;
 

wpilibc/Athena/src/AnalogGyro.cpp

@@ -18,66 +18,14 @@ constexpr float AnalogGyro::kSamplesPerSecond;
 constexpr float AnalogGyro::kCalibrationSampleTime;
 constexpr float AnalogGyro::kDefaultVoltsPerDegreePerSecond;
 
-/**
- * Initialize the gyro.
- * Calibrate the gyro by running for a number of samples and computing the
- * center value.
- * Then use the center value as the Accumulator center value for subsequent
- * measurements.
- * It's important to make sure that the robot is not moving while the centering
- * calculations are
- * in progress, this is typically done when the robot is first turned on while
- * it's sitting at
- * rest before the competition starts.
- */
-void AnalogGyro::InitGyro() {
-  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(1.0);
-
-  m_analog->InitAccumulator();
-
-  Wait(kCalibrationSampleTime);
-
-  int64_t value;
-  uint32_t count;
-  m_analog->GetAccumulatorOutput(value, count);
-
-  m_center = (uint32_t)((float)value / (float)count + .5);
-
-  m_offset = ((float)value / (float)count) - (float)m_center;
-  m_analog->SetAccumulatorCenter(m_center);
-  m_analog->ResetAccumulator();
-
-  SetDeadband(0.0f);
-
-  SetPIDSourceType(PIDSourceType::kDisplacement);
-
-  HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
-  LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetChannel(), this);
-}
-
 /**
  * Gyro constructor using the Analog Input channel number.
  *
  * @param channel The analog channel the gyro is connected to. Gyros
                       can only be used on on-board Analog Inputs 0-1.
  */
-AnalogGyro::AnalogGyro(int32_t channel) {
-  m_analog = std::make_shared<AnalogInput>(channel);
-  InitGyro();
-}
+AnalogGyro::AnalogGyro(int32_t channel) :
+    AnalogGyro(std::make_shared<AnalogInput>(channel)) {}
 
 /**
  * Gyro constructor with a precreated AnalogInput object.
@@ -108,7 +56,29 @@ AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
   if (channel == nullptr) {
     wpi_setWPIError(NullParameter);
   } else {
-    InitGyro();
+    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);
+
+    Calibrate();
   }
 }
 
@@ -120,6 +90,27 @@ AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
  */
 void AnalogGyro::Reset() { m_analog->ResetAccumulator(); }
 
+/**
+ * {@inheritDoc}
+ */
+void AnalogGyro::Calibrate() {
+  if (StatusIsFatal()) return;
+
+  m_analog->InitAccumulator();
+
+  Wait(kCalibrationSampleTime);
+
+  int64_t value;
+  uint32_t count;
+  m_analog->GetAccumulatorOutput(value, count);
+
+  m_center = (uint32_t)((float)value / (float)count + .5);
+
+  m_offset = ((float)value / (float)count) - (float)m_center;
+  m_analog->SetAccumulatorCenter(m_center);
+  m_analog->ResetAccumulator();
+}
+
 /**
  * Return the actual angle in degrees that the robot is currently facing.
  *

wpilibc/shared/include/interfaces/Gyro.h

@@ -13,14 +13,14 @@ class Gyro {
   virtual ~Gyro() = default;
 
   /**
-   * Initialize the gyro. Calibrate the gyro by running for a number of samples
-   * and computing the center value. Then use the center value as the
-   * Accumulator center value for subsequent measurements. It's important to
-   * make sure that the robot is not moving while the centering calculations are
-   * in progress, this is typically done when the robot is first turned on while
-   * it's sitting at rest before the competition starts.
+   * Calibrate the gyro by running for a number of samples and computing the
+   * center value. Then use the center value as the Accumulator center value for
+   * subsequent measurements. It's important to make sure that the robot is not
+   * moving while the centering calculations are in progress, this is typically
+   * done when the robot is first turned on while it's sitting at rest before
+   * the competition starts.
    */
-  virtual void InitGyro() = 0;
+  virtual void Calibrate() = 0;
 
   /**
    * Reset the gyro. Resets the gyro to a heading of zero. This can be used if

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

@@ -42,18 +42,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
   /**
    * {@inheritDoc}
    */
-  public void initGyro() {
-    result = new AccumulatorResult();
-    if (m_analog == null) {
-      System.out.println("Null m_analog");
-    }
-    m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
-    m_analog.setAverageBits(kAverageBits);
-    m_analog.setOversampleBits(kOversampleBits);
-    double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
-    AnalogInput.setGlobalSampleRate(sampleRate);
-    Timer.delay(1.0);
-
+  public void calibrate() {
     m_analog.initAccumulator();
     m_analog.resetAccumulator();
 
@@ -67,13 +56,6 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
 
     m_analog.setAccumulatorCenter(m_center);
     m_analog.resetAccumulator();
-
-    setDeadband(0.0);
-
-    setPIDSourceType(PIDSourceType.kDisplacement);
-
-    UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
-    LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this);
   }
 
   /**
@@ -99,7 +81,23 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
     if (m_analog == null) {
       throw new NullPointerException("AnalogInput supplied to Gyro constructor is null");
     }
-    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);
+
+    calibrate();
   }
 
   /**

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

@@ -21,7 +21,7 @@ public abstract class GyroBase extends SensorBase implements Gyro, PIDSource, Li
   /**
    * {@inheritDoc}
    */
-  public abstract void initGyro();
+  public abstract void calibrate();
 
   /**
    * {@inheritDoc}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/interfaces/Gyro.java

@@ -10,14 +10,14 @@ package edu.wpi.first.wpilibj.interfaces;
  */
 public interface Gyro {
   /**
-   * Initialize the gyro. Calibrate the gyro by running for a number of samples
-   * and computing the center value. Then use the center value as the
-   * Accumulator center value for subsequent measurements. It's important to
-   * make sure that the robot is not moving while the centering calculations are
-   * in progress, this is typically done when the robot is first turned on while
-   * it's sitting at rest before the competition starts.
+   * Calibrate the gyro by running for a number of samples and computing the
+   * center value. Then use the center value as the Accumulator center value for
+   * subsequent measurements. It's important to make sure that the robot is not
+   * moving while the centering calculations are in progress, this is typically
+   * done when the robot is first turned on while it's sitting at rest before
+   * the competition starts.
    */
-  public void initGyro();
+  public void calibrate();
 
   /**
    * Reset the gyro. Resets the gyro to a heading of zero. This can be used if