Source code

001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj;
006
007import edu.wpi.first.hal.FRCNetComm.tResourceType;
008import edu.wpi.first.hal.HAL;
009import edu.wpi.first.hal.SimBoolean;
010import edu.wpi.first.hal.SimDevice;
011import edu.wpi.first.hal.SimDouble;
012import edu.wpi.first.math.geometry.Rotation2d;
013import edu.wpi.first.util.sendable.Sendable;
014import edu.wpi.first.util.sendable.SendableBuilder;
015import edu.wpi.first.util.sendable.SendableRegistry;
016import java.nio.ByteBuffer;
017import java.nio.ByteOrder;
018
019/**
020 * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
021 * tracks the robots heading based on the starting position. As the robot rotates the new heading is
022 * computed by integrating the rate of rotation returned by the sensor. When the class is
023 * instantiated, it does a short calibration routine where it samples the gyro while at rest to
024 * determine the default offset. This is subtracted from each sample to determine the heading.
025 *
026 * <p>This class is for the digital ADXRS450 gyro sensor that connects via SPI. Only one instance of
027 * an ADXRS Gyro is supported.
028 */
029@SuppressWarnings("TypeName")
030public class ADXRS450_Gyro implements Sendable, AutoCloseable {
031  private static final double kSamplePeriod = 0.0005;
032  private static final double kCalibrationSampleTime = 5.0;
033  private static final double kDegreePerSecondPerLSB = 0.0125;
034
035  // private static final int kRateRegister = 0x00;
036  // private static final int kTemRegister = 0x02;
037  // private static final int kLoCSTRegister = 0x04;
038  // private static final int kHiCSTRegister = 0x06;
039  // private static final int kQuadRegister = 0x08;
040  // private static final int kFaultRegister = 0x0A;
041  private static final int kPIDRegister = 0x0C;
042  // private static final int kSNHighRegister = 0x0E;
043  // private static final int kSNLowRegister = 0x10;
044
045  private SPI m_spi;
046
047  private SimDevice m_simDevice;
048  private SimBoolean m_simConnected;
049  private SimDouble m_simAngle;
050  private SimDouble m_simRate;
051
052  /** Constructor. Uses the onboard CS0. */
053  public ADXRS450_Gyro() {
054    this(SPI.Port.kOnboardCS0);
055  }
056
057  /**
058   * Constructor.
059   *
060   * @param port The SPI port that the gyro is connected to
061   */
062  @SuppressWarnings("this-escape")
063  public ADXRS450_Gyro(SPI.Port port) {
064    m_spi = new SPI(port);
065
066    // simulation
067    m_simDevice = SimDevice.create("Gyro:ADXRS450", port.value);
068    if (m_simDevice != null) {
069      m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
070      m_simAngle = m_simDevice.createDouble("angle_x", SimDevice.Direction.kInput, 0.0);
071      m_simRate = m_simDevice.createDouble("rate_x", SimDevice.Direction.kInput, 0.0);
072    }
073
074    m_spi.setClockRate(3000000);
075    m_spi.setMode(SPI.Mode.kMode0);
076    m_spi.setChipSelectActiveLow();
077
078    if (m_simDevice == null) {
079      // Validate the part ID
080      if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
081        m_spi.close();
082        m_spi = null;
083        DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value, false);
084        return;
085      }
086
087      m_spi.initAccumulator(
088          kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16, true, true);
089
090      calibrate();
091    }
092
093    HAL.report(tResourceType.kResourceType_ADXRS450, port.value + 1);
094    SendableRegistry.addLW(this, "ADXRS450_Gyro", port.value);
095  }
096
097  /**
098   * Determine if the gyro is connected.
099   *
100   * @return true if it is connected, false otherwise.
101   */
102  public boolean isConnected() {
103    if (m_simConnected != null) {
104      return m_simConnected.get();
105    }
106    return m_spi != null;
107  }
108
109  /**
110   * Calibrate the gyro by running for a number of samples and computing the center value. Then use
111   * the center value as the Accumulator center value for subsequent measurements.
112   *
113   * <p>It's important to make sure that the robot is not moving while the centering calculations
114   * are in progress, this is typically done when the robot is first turned on while it's sitting at
115   * rest before the competition starts.
116   */
117  public final void calibrate() {
118    if (m_spi == null) {
119      return;
120    }
121
122    Timer.delay(0.1);
123
124    m_spi.setAccumulatorIntegratedCenter(0);
125    m_spi.resetAccumulator();
126
127    Timer.delay(kCalibrationSampleTime);
128
129    m_spi.setAccumulatorIntegratedCenter(m_spi.getAccumulatorIntegratedAverage());
130    m_spi.resetAccumulator();
131  }
132
133  /**
134   * Get the SPI port number.
135   *
136   * @return The SPI port number.
137   */
138  public int getPort() {
139    return m_spi.getPort();
140  }
141
142  private boolean calcParity(int value) {
143    boolean parity = false;
144    while (value != 0) {
145      parity = !parity;
146      value = value & (value - 1);
147    }
148    return parity;
149  }
150
151  private int readRegister(int reg) {
152    int cmdhi = 0x8000 | (reg << 1);
153    boolean parity = calcParity(cmdhi);
154
155    ByteBuffer buf = ByteBuffer.allocate(4);
156    buf.order(ByteOrder.BIG_ENDIAN);
157    buf.put(0, (byte) (cmdhi >> 8));
158    buf.put(1, (byte) (cmdhi & 0xff));
159    buf.put(2, (byte) 0);
160    buf.put(3, (byte) (parity ? 0 : 1));
161
162    m_spi.write(buf, 4);
163    m_spi.read(false, buf, 4);
164
165    if ((buf.get(0) & 0xe0) == 0) {
166      return 0; // error, return 0
167    }
168    return (buf.getInt(0) >> 5) & 0xffff;
169  }
170
171  /**
172   * Reset the gyro.
173   *
174   * <p>Resets the gyro to a heading of zero. This can be used if there is significant drift in the
175   * gyro, and it needs to be recalibrated after it has been running.
176   */
177  public void reset() {
178    if (m_simAngle != null) {
179      m_simAngle.reset();
180    }
181    if (m_spi != null) {
182      m_spi.resetAccumulator();
183    }
184  }
185
186  /** Delete (free) the spi port used for the gyro and stop accumulating. */
187  @Override
188  public void close() {
189    SendableRegistry.remove(this);
190    if (m_spi != null) {
191      m_spi.close();
192      m_spi = null;
193    }
194    if (m_simDevice != null) {
195      m_simDevice.close();
196      m_simDevice = null;
197    }
198  }
199
200  /**
201   * Return the heading of the robot in degrees.
202   *
203   * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
204   * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
205   * 360 to 0 on the second time around.
206   *
207   * <p>The angle is expected to increase as the gyro turns clockwise when looked at from the top.
208   * It needs to follow the NED axis convention.
209   *
210   * <p>This heading is based on integration of the returned rate from the gyro.
211   *
212   * @return the current heading of the robot in degrees.
213   */
214  public double getAngle() {
215    if (m_simAngle != null) {
216      return m_simAngle.get();
217    }
218    if (m_spi == null) {
219      return 0.0;
220    }
221    return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
222  }
223
224  /**
225   * Return the rate of rotation of the gyro.
226   *
227   * <p>The rate is based on the most recent reading of the gyro analog value
228   *
229   * <p>The rate is expected to be positive as the gyro turns clockwise when looked at from the top.
230   * It needs to follow the NED axis convention.
231   *
232   * @return the current rate in degrees per second
233   */
234  public double getRate() {
235    if (m_simRate != null) {
236      return m_simRate.get();
237    }
238    if (m_spi == null) {
239      return 0.0;
240    }
241    return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
242  }
243
244  /**
245   * Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
246   *
247   * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
248   * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
249   * 360 to 0 on the second time around.
250   *
251   * <p>The angle is expected to increase as the gyro turns counterclockwise when looked at from the
252   * top. It needs to follow the NWU axis convention.
253   *
254   * <p>This heading is based on integration of the returned rate from the gyro.
255   *
256   * @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
257   */
258  public Rotation2d getRotation2d() {
259    return Rotation2d.fromDegrees(-getAngle());
260  }
261
262  @Override
263  public void initSendable(SendableBuilder builder) {
264    builder.setSmartDashboardType("Gyro");
265    builder.addDoubleProperty("Value", this::getAngle, null);
266  }
267}