001/*----------------------------------------------------------------------------*/
002/* Copyright (c) 2015-2018 FIRST. 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
008package edu.wpi.first.wpilibj;
009
010import java.nio.ByteBuffer;
011import java.nio.ByteOrder;
012
013import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
014import edu.wpi.first.wpilibj.hal.HAL;
015import edu.wpi.first.wpilibj.interfaces.Gyro;
016
017/**
018 * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
019 * tracks the robots heading based on the starting position. As the robot rotates the new heading is
020 * computed by integrating the rate of rotation returned by the sensor. When the class is
021 * instantiated, it does a short calibration routine where it samples the gyro while at rest to
022 * determine the default offset. This is subtracted from each sample to determine the heading.
023 *
024 * <p>This class is for the digital ADXRS450 gyro sensor that connects via SPI.
025 */
026@SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"})
027public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable {
028  private static final double kSamplePeriod = 0.0005;
029  private static final double kCalibrationSampleTime = 5.0;
030  private static final double kDegreePerSecondPerLSB = 0.0125;
031
032  private static final int kRateRegister = 0x00;
033  private static final int kTemRegister = 0x02;
034  private static final int kLoCSTRegister = 0x04;
035  private static final int kHiCSTRegister = 0x06;
036  private static final int kQuadRegister = 0x08;
037  private static final int kFaultRegister = 0x0A;
038  private static final int kPIDRegister = 0x0C;
039  private static final int kSNHighRegister = 0x0E;
040  private static final int kSNLowRegister = 0x10;
041
042  private SPI m_spi;
043
044  /**
045   * Constructor.  Uses the onboard CS0.
046   */
047  public ADXRS450_Gyro() {
048    this(SPI.Port.kOnboardCS0);
049  }
050
051  /**
052   * Constructor.
053   *
054   * @param port The SPI port that the gyro is connected to
055   */
056  public ADXRS450_Gyro(SPI.Port port) {
057    m_spi = new SPI(port);
058
059    m_spi.setClockRate(3000000);
060    m_spi.setMSBFirst();
061    m_spi.setSampleDataOnRising();
062    m_spi.setClockActiveHigh();
063    m_spi.setChipSelectActiveLow();
064
065    // Validate the part ID
066    if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
067      m_spi.free();
068      m_spi = null;
069      DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
070          false);
071      return;
072    }
073
074    m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
075        true, true);
076
077    calibrate();
078
079    HAL.report(tResourceType.kResourceType_ADXRS450, port.value);
080    setName("ADXRS450_Gyro", port.value);
081  }
082
083  @Override
084  public void calibrate() {
085    if (m_spi == null) {
086      return;
087    }
088
089    Timer.delay(0.1);
090
091    m_spi.setAccumulatorCenter(0);
092    m_spi.resetAccumulator();
093
094    Timer.delay(kCalibrationSampleTime);
095
096    m_spi.setAccumulatorCenter((int) m_spi.getAccumulatorAverage());
097    m_spi.resetAccumulator();
098  }
099
100  private boolean calcParity(int value) {
101    boolean parity = false;
102    while (value != 0) {
103      parity = !parity;
104      value = value & (value - 1);
105    }
106    return parity;
107  }
108
109  private int readRegister(int reg) {
110    int cmdhi = 0x8000 | (reg << 1);
111    boolean parity = calcParity(cmdhi);
112
113    ByteBuffer buf = ByteBuffer.allocate(4);
114    buf.order(ByteOrder.BIG_ENDIAN);
115    buf.put(0, (byte) (cmdhi >> 8));
116    buf.put(1, (byte) (cmdhi & 0xff));
117    buf.put(2, (byte) 0);
118    buf.put(3, (byte) (parity ? 0 : 1));
119
120    m_spi.write(buf, 4);
121    m_spi.read(false, buf, 4);
122
123    if ((buf.get(0) & 0xe0) == 0) {
124      return 0;  // error, return 0
125    }
126    return (buf.getInt(0) >> 5) & 0xffff;
127  }
128
129  @Override
130  public void reset() {
131    m_spi.resetAccumulator();
132  }
133
134  /**
135   * Delete (free) the spi port used for the gyro and stop accumulating.
136   */
137  @Override
138  public void free() {
139    super.free();
140    if (m_spi != null) {
141      m_spi.free();
142      m_spi = null;
143    }
144  }
145
146  @Override
147  public double getAngle() {
148    if (m_spi == null) {
149      return 0.0;
150    }
151    return m_spi.getAccumulatorValue() * kDegreePerSecondPerLSB * kSamplePeriod;
152  }
153
154  @Override
155  public double getRate() {
156    if (m_spi == null) {
157      return 0.0;
158    }
159    return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
160  }
161}