wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java

   1 /*----------------------------------------------------------------------------*/
   2 /* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
   3 /* Open Source Software - may be modified and shared by FRC teams. The code   */
   4 /* must be accompanied by the FIRST BSD license file in the root directory of */
   5 /* the project.                                                               */
   6 /*----------------------------------------------------------------------------*/
   7 package edu.wpi.first.wpilibj;
   8
   9 import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
  10 import edu.wpi.first.wpilibj.communication.UsageReporting;
  11 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
  12 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
  13 import edu.wpi.first.wpilibj.tables.ITable;
  14 import edu.wpi.first.wpilibj.util.BoundaryException;
  15
  16 /**
  17  * Use a rate gyro to return the robots heading relative to a starting position.
  18  * The Gyro class tracks the robots heading based on the starting position. As
  19  * the robot rotates the new heading is computed by integrating the rate of
  20  * rotation returned by the sensor. When the class is instantiated, it does a
  21  * short calibration routine where it samples the gyro while at rest to
  22  * determine the default offset. This is subtracted from each sample to
  23  * determine the heading.
  24  */
  25 public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
  26
  27         static final int kOversampleBits = 10;
  28         static final int kAverageBits = 0;
  29         static final double kSamplesPerSecond = 50.0;
  30         static final double kCalibrationSampleTime = 5.0;
  31         static final double kDefaultVoltsPerDegreePerSecond = 0.007;
  32         protected AnalogInput m_analog;
  33         double m_voltsPerDegreePerSecond;
  34         double m_offset;
  35         int m_center;
  36         boolean m_channelAllocated = false;
  37         AccumulatorResult result;
  38         private PIDSourceParameter m_pidSource;
  39
  40         /**
  41          * Initialize the gyro. Calibrate the gyro by running for a number of
  42          * samples and computing the center value. Then use the center
  43          * value as the Accumulator center value for subsequent measurements. It's
  44          * important to make sure that the robot is not moving while the centering
  45          * calculations are in progress, this is typically done when the robot is
  46          * first turned on while it's sitting at rest before the competition starts.
  47          */
  48         public void initGyro() {
  49                 result = new AccumulatorResult();
  50                 if (m_analog == null) {
  51                         System.out.println("Null m_analog");
  52                 }
  53                 m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
  54                 m_analog.setAverageBits(kAverageBits);
  55                 m_analog.setOversampleBits(kOversampleBits);
  56                 double sampleRate = kSamplesPerSecond
  57                                 * (1 << (kAverageBits + kOversampleBits));
  58                 AnalogInput.setGlobalSampleRate(sampleRate);
  59                 Timer.delay(1.0);
  60
  61                 m_analog.initAccumulator();
  62                 m_analog.resetAccumulator();
  63
  64                 Timer.delay(kCalibrationSampleTime);
  65
  66                 m_analog.getAccumulatorOutput(result);
  67
  68                 m_center = (int) ((double) result.value / (double) result.count + .5);
  69
  70                 m_offset = ((double) result.value / (double) result.count)
  71                                 - m_center;
  72
  73                 m_analog.setAccumulatorCenter(m_center);
  74                 m_analog.resetAccumulator();
  75
  76                 setDeadband(0.0);
  77
  78                 setPIDSourceParameter(PIDSourceParameter.kAngle);
  79
  80                 UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
  81                 LiveWindow.addSensor("Gyro", m_analog.getChannel(), this);
  82         }
  83
  84         /**
  85          * Gyro constructor using the channel number
  86          *
  87          * @param channel
  88          *            The analog channel the gyro is connected to. Gyros can only
  89                   be used on on-board channels 0-1.
  90          */
  91         public Gyro(int channel) {
  92                 this(new AnalogInput(channel));
  93                 m_channelAllocated = true;
  94         }
  95
  96         /**
  97          * Gyro constructor with a precreated analog channel object. Use this
  98          * constructor when the analog channel needs to be shared.
  99          *
 100          * @param channel
 101          *            The AnalogInput object that the gyro is connected to. Gyros
 102                       can only be used on on-board channels 0-1.
 103          */
 104         public Gyro(AnalogInput channel) {
 105                 m_analog = channel;
 106                 if (m_analog == null) {
 107                         throw new NullPointerException("AnalogInput supplied to Gyro constructor is null");
 108                 }
 109                 initGyro();
 110         }
 111
 112         /**
 113          * Reset the gyro. Resets the gyro to a heading of zero. This can be used if
 114          * there is significant drift in the gyro and it needs to be recalibrated
 115          * after it has been running.
 116          */
 117         public void reset() {
 118                 if (m_analog != null) {
 119                         m_analog.resetAccumulator();
 120                 }
 121         }
 122
 123         /**
 124          * Delete (free) the accumulator and the analog components used for the
 125          * gyro.
 126          */
 127         @Override
 128         public void free() {
 129                 if (m_analog != null && m_channelAllocated) {
 130                         m_analog.free();
 131                 }
 132                 m_analog = null;
 133         }
 134
 135         /**
 136          * Return the actual angle in degrees that the robot is currently facing.
 137          *
 138          * The angle is based on the current accumulator value corrected by the
 139          * oversampling rate, the gyro type and the A/D calibration values. The
 140          * angle is continuous, that is it will continue from 360 to 361 degrees. This allows
 141          * algorithms that wouldn't want to see a discontinuity in the gyro output
 142          * as it sweeps past from 360 to 0 on the second time around.
 143          *
 144          * @return the current heading of the robot in degrees. This heading is
 145          *         based on integration of the returned rate from the gyro.
 146          */
 147         public double getAngle() {
 148                 if (m_analog == null) {
 149                         return 0.0;
 150                 } else {
 151                         m_analog.getAccumulatorOutput(result);
 152
 153                         long value = result.value - (long) (result.count * m_offset);
 154
 155                         double scaledValue = value
 156                                         * 1e-9
 157                                         * m_analog.getLSBWeight()
 158                                         * (1 << m_analog.getAverageBits())
 159                                         / (AnalogInput.getGlobalSampleRate() * m_voltsPerDegreePerSecond);
 160
 161                         return scaledValue;
 162                 }
 163         }
 164
 165         /**
 166          * Return the rate of rotation of the gyro
 167          *
 168          * The rate is based on the most recent reading of the gyro analog value
 169          *
 170          * @return the current rate in degrees per second
 171          */
 172         public double getRate() {
 173                 if (m_analog == null) {
 174                         return 0.0;
 175                 } else {
 176                         return (m_analog.getAverageValue() - (m_center + m_offset))
 177                                         * 1e-9
 178                                         * m_analog.getLSBWeight()
 179                                         / ((1 << m_analog.getOversampleBits()) * m_voltsPerDegreePerSecond);
 180                 }
 181         }
 182
 183         /**
 184          * Set the gyro sensitivity. This takes the number of
 185          * volts/degree/second sensitivity of the gyro and uses it in subsequent
 186          * calculations to allow the code to work with multiple gyros. This value
 187          * is typically found in the gyro datasheet.
 188          *
 189          * @param voltsPerDegreePerSecond
 190          *            The sensitivity in Volts/degree/second.
 191          */
 192         public void setSensitivity(double voltsPerDegreePerSecond) {
 193                 m_voltsPerDegreePerSecond = voltsPerDegreePerSecond;
 194         }
 195
 196         /**
 197          * Set the size of the neutral zone.  Any voltage from the gyro less than
 198          * this amount from the center is considered stationary.  Setting a
 199          * deadband will decrease the amount of drift when the gyro isn't rotating,
 200          * but will make it less accurate.
 201          *
 202          * @param volts The size of the deadband in volts
 203          */
 204         void setDeadband(double volts) {
 205                 int deadband = (int)(volts * 1e9 / m_analog.getLSBWeight() * (1 << m_analog.getOversampleBits()));
 206                 m_analog.setAccumulatorDeadband(deadband);
 207         }
 208
 209         /**
 210          * Set which parameter of the gyro you are using as a process control
 211          * variable. The Gyro class supports the rate and angle parameters
 212          *
 213          * @param pidSource
 214          *            An enum to select the parameter.
 215          */
 216         public void setPIDSourceParameter(PIDSourceParameter pidSource) {
 217                 BoundaryException.assertWithinBounds(pidSource.value, 1, 2);
 218                 m_pidSource = pidSource;
 219         }
 220
 221         /**
 222          * Get the output of the gyro for use with PIDControllers.
 223          * May be the angle or rate depending on the set PIDSourceParameter
 224          *
 225          * @return the output according to the gyro
 226          */
 227         @Override
 228         public double pidGet() {
 229                 switch (m_pidSource.value) {
 230                 case PIDSourceParameter.kRate_val:
 231                         return getRate();
 232                 case PIDSourceParameter.kAngle_val:
 233                         return getAngle();
 234                 default:
 235                         return 0.0;
 236                 }
 237         }
 238
 239         /*
 240          * Live Window code, only does anything if live window is activated.
 241          */
 242         @Override
 243         public String getSmartDashboardType() {
 244                 return "Gyro";
 245         }
 246
 247         private ITable m_table;
 248
 249         /**
 250          * {@inheritDoc}
 251          */
 252         @Override
 253         public void initTable(ITable subtable) {
 254                 m_table = subtable;
 255                 updateTable();
 256         }
 257
 258         /**
 259          * {@inheritDoc}
 260          */
 261         @Override
 262         public ITable getTable() {
 263                 return m_table;
 264         }
 265
 266         /**
 267          * {@inheritDoc}
 268          */
 269         @Override
 270         public void updateTable() {
 271                 if (m_table != null) {
 272                         m_table.putNumber("Value", getAngle());
 273                 }
 274         }
 275
 276         /**
 277          * {@inheritDoc}
 278          */
 279         @Override
 280         public void startLiveWindowMode() {
 281         }
 282
 283         /**
 284          * {@inheritDoc}
 285          */
 286         @Override
 287         public void stopLiveWindowMode() {
 288         }
 289 }