From 60a3fd069839340eaa0d8a33ad1553fb0e499db2 Mon Sep 17 00:00:00 2001 From: Thomas Clark Date: Mon, 4 Aug 2014 17:44:04 -0400 Subject: [PATCH] Added gyro deadbands During calibration, the Gyro class sets the accumulator deadband to contain whatever the farthest sample from the center was. The integration test passes now. A SetDeadBand method was added to the Gyro class for teams to set their own deadbands. Change-Id: Idbe4c279e2991b4daed4d4cf3bfaf605d4ee25c0 --- wpilibc/wpilibC++/include/Gyro.h | 4 +- wpilibc/wpilibC++/lib/Gyro.cpp | 40 +++++++++++++++++-- .../main/java/edu/wpi/first/wpilibj/Gyro.java | 37 ++++++++++++++--- 3 files changed, 72 insertions(+), 9 deletions(-) diff --git a/wpilibc/wpilibC++/include/Gyro.h b/wpilibc/wpilibC++/include/Gyro.h index 36e529be4d..f1260ae266 100644 --- a/wpilibc/wpilibC++/include/Gyro.h +++ b/wpilibc/wpilibC++/include/Gyro.h @@ -27,7 +27,8 @@ public: static const uint32_t kOversampleBits = 10; static const uint32_t kAverageBits = 0; static constexpr float kSamplesPerSecond = 50.0; - static constexpr float kCalibrationSampleTime = 5.0; + static constexpr float kCalibrationSampleTime = 0.01; + static constexpr int kNumCalibrationSamples = 500; static constexpr float kDefaultVoltsPerDegreePerSecond = 0.007; explicit Gyro(uint32_t channel); @@ -37,6 +38,7 @@ public: virtual float GetAngle(); virtual double GetRate(); void SetSensitivity(float voltsPerDegreePerSecond); + void SetDeadband(float volts); void SetPIDSourceParameter(PIDSourceParameter pidSource); virtual void Reset(); diff --git a/wpilibc/wpilibC++/lib/Gyro.cpp b/wpilibc/wpilibC++/lib/Gyro.cpp index 523b8bbf8f..6855d11157 100644 --- a/wpilibc/wpilibC++/lib/Gyro.cpp +++ b/wpilibc/wpilibC++/lib/Gyro.cpp @@ -10,11 +10,12 @@ #include "Timer.h" #include "WPIErrors.h" #include "LiveWindow/LiveWindow.h" - +#include const uint32_t Gyro::kOversampleBits; const uint32_t Gyro::kAverageBits; constexpr float Gyro::kSamplesPerSecond; constexpr float Gyro::kCalibrationSampleTime; +constexpr int Gyro::kNumCalibrationSamples; constexpr float Gyro::kDefaultVoltsPerDegreePerSecond; /** @@ -49,7 +50,26 @@ void Gyro::InitGyro() Wait(1.0); m_analog->InitAccumulator(); - Wait(kCalibrationSampleTime); + + // Get the lowest and highest value that occur within a large number of + // calibration samples. These are used to determine an appropriate default + // deadband. + int32_t lowestSample = INT_MAX, highestSample = INT_MIN; + for(int i = 0; i < kNumCalibrationSamples; i++) + { + int32_t sample = m_analog->GetAverageValue(); + + if(sample < lowestSample) + { + lowestSample = sample; + } + else if(sample > highestSample) + { + highestSample = sample; + } + + Wait(kCalibrationSampleTime); + } int64_t value; uint32_t count; @@ -59,8 +79,10 @@ void Gyro::InitGyro() m_offset = ((float)value / (float)count) - (float)m_center; + int32_t deadband = std::max(highestSample - m_center, m_center - lowestSample); + m_analog->SetAccumulatorCenter(m_center); - m_analog->SetAccumulatorDeadband(0); ///< TODO: compute / parameterize this + m_analog->SetAccumulatorDeadband(deadband); m_analog->ResetAccumulator(); SetPIDSourceParameter(kAngle); @@ -179,6 +201,18 @@ void Gyro::SetSensitivity( float voltsPerDegreePerSecond ) m_voltsPerDegreePerSecond = voltsPerDegreePerSecond; } +/** + * Set the size of the neutral zone. Any voltage from the gyro less than this + * amount from the center is considered stationary. This is set by default + * after calibration. + * + * @param volts The size of the deadband in volts + */ +void Gyro::SetDeadband( float volts ) { + int32_t deadband = volts * 1e9 / m_analog->GetLSBWeight() * (1 << m_analog->GetOversampleBits()); + m_analog->SetAccumulatorDeadband(deadband); +} + void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource) { if(pidSource == 0 || pidSource > 2) diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java index 8f06d63738..7f296bc73a 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java @@ -27,7 +27,8 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable { static final int kOversampleBits = 10; static final int kAverageBits = 0; static final double kSamplesPerSecond = 50.0; - static final double kCalibrationSampleTime = 5.0; + static final double kCalibrationSampleTime = 0.01; + static final int kNumCalibrationSamples = 500; static final double kDefaultVoltsPerDegreePerSecond = 0.007; private AnalogInput m_analog; double m_voltsPerDegreePerSecond; @@ -61,7 +62,21 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable { m_analog.initAccumulator(); m_analog.resetAccumulator(); - Timer.delay(kCalibrationSampleTime); + // Get the lowest and highest value that occur within a large number of + // calibration samples. These are used to determine an appropriate + // default deadband. + int lowestSample = Integer.MAX_VALUE, highestSample = Integer.MIN_VALUE; + for(int i = 0; i < kNumCalibrationSamples; i++) { + int sample = m_analog.getAverageValue(); + + if(sample < lowestSample) { + lowestSample = sample; + } else if(sample > highestSample) { + highestSample = sample; + } + + Timer.delay(kCalibrationSampleTime); + } m_analog.getAccumulatorOutput(result); @@ -70,10 +85,10 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable { m_offset = ((double) result.value / (double) result.count) - m_center; - m_analog.setAccumulatorCenter(m_center); + int deadband = Math.max(highestSample - m_center, m_center - lowestSample); - m_analog.setAccumulatorDeadband(0); // /< TODO: compute / parameterize - // this + m_analog.setAccumulatorCenter(m_center); + m_analog.setAccumulatorDeadband(deadband); m_analog.resetAccumulator(); setPIDSourceParameter(PIDSourceParameter.kAngle); @@ -192,6 +207,18 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable { m_voltsPerDegreePerSecond = voltsPerDegreePerSecond; } + /** + * Set the size of the neutral zone. Any voltage from the gyro less than + * this amount from the center is considered stationary. This is set by + * default after calibration. + * + * @param volts The size of the deadband in volts + */ + void setDeadband(double volts) { + int deadband = (int)(volts * 1e9 / m_analog.getLSBWeight() * (1 << m_analog.getOversampleBits())); + m_analog.setAccumulatorDeadband(deadband); + } + /** * Set which parameter of the encoder you are using as a process control * variable. The Gyro class supports the rate and angle parameters