diff --git a/wpilibc/Athena/include/AnalogGyro.h b/wpilibc/Athena/include/AnalogGyro.h index 848740da00..817997f5c6 100644 --- a/wpilibc/Athena/include/AnalogGyro.h +++ b/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; diff --git a/wpilibc/Athena/src/AnalogGyro.cpp b/wpilibc/Athena/src/AnalogGyro.cpp index 916cc8a491..2a4c3f2c00 100644 --- a/wpilibc/Athena/src/AnalogGyro.cpp +++ b/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(channel); - InitGyro(); -} +AnalogGyro::AnalogGyro(int32_t channel) : + AnalogGyro(std::make_shared(channel)) {} /** * Gyro constructor with a precreated AnalogInput object. @@ -108,7 +56,29 @@ AnalogGyro::AnalogGyro(std::shared_ptr 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 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. * diff --git a/wpilibc/shared/include/interfaces/Gyro.h b/wpilibc/shared/include/interfaces/Gyro.h index 2938bb9c88..8317ca5703 100644 --- a/wpilibc/shared/include/interfaces/Gyro.h +++ b/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 diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java index 8ec341b81d..2a22e373b0 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java +++ b/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(); } /** diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/GyroBase.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/GyroBase.java index e5ad8279c1..1bf20c8753 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/GyroBase.java +++ b/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} diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/interfaces/Gyro.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/interfaces/Gyro.java index 97942f5b86..6a790afb5b 100644 --- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/interfaces/Gyro.java +++ b/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