001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007 package edu.wpi.first.wpilibj;
008
009 import edu.wpi.first.wpilibj.communication.UsageReporting;
010 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
011 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
012 import edu.wpi.first.wpilibj.parsing.ISensor;
013 import edu.wpi.first.wpilibj.tables.ITable;
014 import edu.wpi.first.wpilibj.util.BoundaryException;
015
016 /**
017 * Use a rate gyro to return the robots heading relative to a starting position.
018 * The Gyro class tracks the robots heading based on the starting position. As the robot
019 * rotates the new heading is computed by integrating the rate of rotation returned
020 * by the sensor. When the class is instantiated, it does a short calibration routine
021 * where it samples the gyro while at rest to determine the default offset. This is
022 * subtracted from each sample to determine the heading.
023 */
024 public class Gyro extends SensorBase implements PIDSource, ISensor, LiveWindowSendable {
025
026 static final int kOversampleBits = 10;
027 static final int kAverageBits = 0;
028 static final double kSamplesPerSecond = 50.0;
029 static final double kCalibrationSampleTime = 5.0;
030 static final double kDefaultVoltsPerDegreePerSecond = 0.007;
031 AnalogChannel m_analog;
032 double m_voltsPerDegreePerSecond;
033 double m_offset;
034 int m_center;
035 boolean m_channelAllocated;
036 AccumulatorResult result;
037 private PIDSourceParameter m_pidSource;
038
039 /**
040 * Initialize the gyro.
041 * Calibrate the gyro by running for a number of samples and computing the center value for this
042 * part. Then use the center value as the Accumulator center value for subsequent measurements.
043 * It's important to make sure that the robot is not moving while the centering calculations are
044 * in progress, this is typically done when the robot is first turned on while it's sitting at
045 * rest before the competition starts.
046 */
047 private void initGyro() {
048 result = new AccumulatorResult();
049 if (m_analog == null) {
050 System.out.println("Null m_analog");
051 }
052 m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
053 m_analog.setAverageBits(kAverageBits);
054 m_analog.setOversampleBits(kOversampleBits);
055 double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
056 m_analog.getModule().setSampleRate(sampleRate);
057
058 Timer.delay(1.0);
059 m_analog.initAccumulator();
060
061 Timer.delay(kCalibrationSampleTime);
062
063 m_analog.getAccumulatorOutput(result);
064
065 m_center = (int) ((double)result.value / (double)result.count + .5);
066
067 m_offset = ((double)result.value / (double)result.count) - (double)m_center;
068
069 m_analog.setAccumulatorCenter(m_center);
070
071 m_analog.setAccumulatorDeadband(0); ///< TODO: compute / parameterize this
072 m_analog.resetAccumulator();
073
074 setPIDSourceParameter(PIDSourceParameter.kAngle);
075
076 UsageReporting.report(UsageReporting.kResourceType_Gyro, m_analog.getChannel(), m_analog.getModuleNumber()-1);
077 LiveWindow.addSensor("Gyro", m_analog.getModuleNumber(), m_analog.getChannel(), this);
078 }
079
080 /**
081 * Gyro constructor given a slot and a channel.
082 .
083 * @param slot The cRIO slot for the analog module the gyro is connected to.
084 * @param channel The analog channel the gyro is connected to.
085 */
086 public Gyro(int slot, int channel) {
087 m_analog = new AnalogChannel(slot, channel);
088 m_channelAllocated = true;
089 initGyro();
090 }
091
092 /**
093 * Gyro constructor with only a channel.
094 *
095 * Use the default analog module slot.
096 *
097 * @param channel The analog channel the gyro is connected to.
098 */
099 public Gyro(int channel) {
100 m_analog = new AnalogChannel(channel);
101 m_channelAllocated = true;
102 initGyro();
103 }
104
105 /**
106 * Gyro constructor with a precreated analog channel object.
107 * Use this constructor when the analog channel needs to be shared. There
108 * is no reference counting when an AnalogChannel is passed to the gyro.
109 * @param channel The AnalogChannel object that the gyro is connected to.
110 */
111 public Gyro(AnalogChannel channel) {
112 m_analog = channel;
113 if (m_analog == null) {
114 System.err.println("Analog channel supplied to Gyro constructor is null");
115 } else {
116 m_channelAllocated = false;
117 initGyro();
118 }
119 }
120
121 /**
122 * Reset the gyro.
123 * Resets the gyro to a heading of zero. This can be used if there is significant
124 * drift in the gyro and it needs to be recalibrated after it has been running.
125 */
126 public void reset() {
127 if (m_analog != null) {
128 m_analog.resetAccumulator();
129 }
130 }
131
132 /**
133 * Delete (free) the accumulator and the analog components used for the gyro.
134 */
135 public void free() {
136 if (m_analog != null && m_channelAllocated) {
137 m_analog.free();
138 }
139 m_analog = null;
140 }
141
142 /**
143 * Return the actual angle in degrees that the robot is currently facing.
144 *
145 * The angle is based on the current accumulator value corrected by the oversampling rate, the
146 * gyro type and the A/D calibration values.
147 * The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't
148 * want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around.
149 *
150 * @return the current heading of the robot in degrees. This heading is based on integration
151 * of the returned rate from the gyro.
152 */
153 public double getAngle() {
154 if (m_analog == null) {
155 return 0.0;
156 } else {
157 m_analog.getAccumulatorOutput(result);
158
159 long value = result.value - (long) (result.count * m_offset);
160
161 double scaledValue = value * 1e-9 * m_analog.getLSBWeight() * (1 << m_analog.getAverageBits()) /
162 (m_analog.getModule().getSampleRate() * m_voltsPerDegreePerSecond);
163
164 return scaledValue;
165 }
166 }
167
168 /**
169 * Return the rate of rotation of the gyro
170 *
171 * The rate is based on the most recent reading of the gyro analog value
172 *
173 * @return the current rate in degrees per second
174 */
175 public double getRate() {
176 if(m_analog == null) {
177 return 0.0;
178 } else {
179 return (m_analog.getAverageValue() - ((double)m_center + m_offset)) * 1e-9 * m_analog.getLSBWeight()
180 / ((1 << m_analog.getOversampleBits()) * m_voltsPerDegreePerSecond);
181 }
182 }
183
184 /**
185 * Set the gyro type based on the sensitivity.
186 * This takes the number of volts/degree/second sensitivity of the gyro and uses it in subsequent
187 * calculations to allow the code to work with multiple gyros.
188 *
189 * @param voltsPerDegreePerSecond The type of gyro specified as the voltage that represents one degree/second.
190 */
191 public void setSensitivity(double voltsPerDegreePerSecond) {
192 m_voltsPerDegreePerSecond = voltsPerDegreePerSecond;
193 }
194
195 /**
196 * Set which parameter of the encoder you are using as a process control variable.
197 * The Gyro class supports the rate and angle parameters
198 * @param pidSource An enum to select the parameter.
199 */
200 public void setPIDSourceParameter(PIDSourceParameter pidSource) {
201 BoundaryException.assertWithinBounds(pidSource.value, 1, 2);
202 m_pidSource = pidSource;
203 }
204
205 /**
206 * Get the angle of the gyro for use with PIDControllers
207 * @return the current angle according to the gyro
208 */
209 public double pidGet() {
210 switch (m_pidSource.value) {
211 case PIDSourceParameter.kRate_val:
212 return getRate();
213 case PIDSourceParameter.kAngle_val:
214 return getAngle();
215 default:
216 return 0.0;
217 }
218 }
219
220 /*
221 * Live Window code, only does anything if live window is activated.
222 */
223 public String getSmartDashboardType(){
224 return "Gyro";
225 }
226 private ITable m_table;
227
228 /**
229 * {@inheritDoc}
230 */
231 public void initTable(ITable subtable) {
232 m_table = subtable;
233 updateTable();
234 }
235
236 /**
237 * {@inheritDoc}
238 */
239 public ITable getTable(){
240 return m_table;
241 }
242
243 /**
244 * {@inheritDoc}
245 */
246 public void updateTable() {
247 if (m_table != null) {
248 m_table.putNumber("Value", getAngle());
249 }
250 }
251
252 /**
253 * {@inheritDoc}
254 */
255 public void startLiveWindowMode() {}
256
257 /**
258 * {@inheritDoc}
259 */
260 public void stopLiveWindowMode() {}
261 }