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

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

wpilibc/wpilibC++/lib/Gyro.cpp

@@ -10,11 +10,12 @@
 #include "Timer.h"
 #include "WPIErrors.h"
 #include "LiveWindow/LiveWindow.h"
-
+#include <climits>
 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)

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