001/*----------------------------------------------------------------------------*/
002/* Copyright (c) FIRST 2015-2017. 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;
016import edu.wpi.first.wpilibj.livewindow.LiveWindow;
017import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
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.
027 */
028@SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"})
029public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, LiveWindowSendable {
030  private static final double kSamplePeriod = 0.001;
031  private static final double kCalibrationSampleTime = 5.0;
032  private static final double kDegreePerSecondPerLSB = 0.0125;
033
034  private static final int kRateRegister = 0x00;
035  private static final int kTemRegister = 0x02;
036  private static final int kLoCSTRegister = 0x04;
037  private static final int kHiCSTRegister = 0x06;
038  private static final int kQuadRegister = 0x08;
039  private static final int kFaultRegister = 0x0A;
040  private static final int kPIDRegister = 0x0C;
041  private static final int kSNHighRegister = 0x0E;
042  private static final int kSNLowRegister = 0x10;
043
044  private SPI m_spi;
045
046  /**
047   * Constructor.  Uses the onboard CS0.
048   */
049  public ADXRS450_Gyro() {
050    this(SPI.Port.kOnboardCS0);
051  }
052
053  /**
054   * Constructor.
055   *
056   * @param port The SPI port that the gyro is connected to
057   */
058  public ADXRS450_Gyro(SPI.Port port) {
059    m_spi = new SPI(port);
060    m_spi.setClockRate(3000000);
061    m_spi.setMSBFirst();
062    m_spi.setSampleDataOnRising();
063    m_spi.setClockActiveHigh();
064    m_spi.setChipSelectActiveLow();
065
066    // Validate the part ID
067    if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
068      m_spi.free();
069      m_spi = null;
070      DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
071          false);
072      return;
073    }
074
075    m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
076        true, true);
077
078    calibrate();
079
080    HAL.report(tResourceType.kResourceType_ADXRS450, port.value);
081    LiveWindow.addSensor("ADXRS450_Gyro", port.value, this);
082  }
083
084  @Override
085  public void calibrate() {
086    if (m_spi == null) {
087      return;
088    }
089
090    Timer.delay(0.1);
091
092    m_spi.setAccumulatorCenter(0);
093    m_spi.resetAccumulator();
094
095    Timer.delay(kCalibrationSampleTime);
096
097    m_spi.setAccumulatorCenter((int) m_spi.getAccumulatorAverage());
098    m_spi.resetAccumulator();
099  }
100
101  private boolean calcParity(int value) {
102    boolean parity = false;
103    while (value != 0) {
104      parity = !parity;
105      value = value & (value - 1);
106    }
107    return parity;
108  }
109
110  private int readRegister(int reg) {
111    int cmdhi = 0x8000 | (reg << 1);
112    boolean parity = calcParity(cmdhi);
113
114    ByteBuffer buf = ByteBuffer.allocateDirect(4);
115    buf.order(ByteOrder.BIG_ENDIAN);
116    buf.put(0, (byte) (cmdhi >> 8));
117    buf.put(1, (byte) (cmdhi & 0xff));
118    buf.put(2, (byte) 0);
119    buf.put(3, (byte) (parity ? 0 : 1));
120
121    m_spi.write(buf, 4);
122    m_spi.read(false, buf, 4);
123
124    if ((buf.get(0) & 0xe0) == 0) {
125      return 0;  // error, return 0
126    }
127    return (buf.getInt(0) >> 5) & 0xffff;
128  }
129
130  @Override
131  public void reset() {
132    m_spi.resetAccumulator();
133  }
134
135  /**
136   * Delete (free) the spi port used for the gyro and stop accumulating.
137   */
138  @Override
139  public void 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}