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.util.sendable.Sendable;
013import edu.wpi.first.util.sendable.SendableBuilder;
014import edu.wpi.first.util.sendable.SendableRegistry;
015import edu.wpi.first.wpilibj.interfaces.Gyro;
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", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"})
030public class ADXRS450_Gyro implements Gyro, Sendable {
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  public ADXRS450_Gyro(SPI.Port port) {
063    m_spi = new SPI(port);
064
065    // simulation
066    m_simDevice = SimDevice.create("Gyro:ADXRS450", port.value);
067    if (m_simDevice != null) {
068      m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
069      m_simAngle = m_simDevice.createDouble("angle_x", SimDevice.Direction.kInput, 0.0);
070      m_simRate = m_simDevice.createDouble("rate_x", SimDevice.Direction.kInput, 0.0);
071    }
072
073    m_spi.setClockRate(3000000);
074    m_spi.setMSBFirst();
075    m_spi.setSampleDataOnLeadingEdge();
076    m_spi.setClockActiveHigh();
077    m_spi.setChipSelectActiveLow();
078
079    if (m_simDevice == null) {
080      // Validate the part ID
081      if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
082        m_spi.close();
083        m_spi = null;
084        DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value, false);
085        return;
086      }
087
088      m_spi.initAccumulator(
089          kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16, true, true);
090
091      calibrate();
092    }
093
094    HAL.report(tResourceType.kResourceType_ADXRS450, port.value + 1);
095    SendableRegistry.addLW(this, "ADXRS450_Gyro", port.value);
096  }
097
098  /**
099   * Determine if the gyro is connected.
100   *
101   * @return true if it is connected, false otherwise.
102   */
103  public boolean isConnected() {
104    if (m_simConnected != null) {
105      return m_simConnected.get();
106    }
107    return m_spi != null;
108  }
109
110  @Override
111  public void calibrate() {
112    if (m_spi == null) {
113      return;
114    }
115
116    Timer.delay(0.1);
117
118    m_spi.setAccumulatorIntegratedCenter(0);
119    m_spi.resetAccumulator();
120
121    Timer.delay(kCalibrationSampleTime);
122
123    m_spi.setAccumulatorIntegratedCenter(m_spi.getAccumulatorIntegratedAverage());
124    m_spi.resetAccumulator();
125  }
126
127  /**
128   * Get the SPI port number.
129   *
130   * @return The SPI port number.
131   */
132  public int getPort() {
133    return m_spi.getPort();
134  }
135
136  private boolean calcParity(int value) {
137    boolean parity = false;
138    while (value != 0) {
139      parity = !parity;
140      value = value & (value - 1);
141    }
142    return parity;
143  }
144
145  private int readRegister(int reg) {
146    int cmdhi = 0x8000 | (reg << 1);
147    boolean parity = calcParity(cmdhi);
148
149    ByteBuffer buf = ByteBuffer.allocate(4);
150    buf.order(ByteOrder.BIG_ENDIAN);
151    buf.put(0, (byte) (cmdhi >> 8));
152    buf.put(1, (byte) (cmdhi & 0xff));
153    buf.put(2, (byte) 0);
154    buf.put(3, (byte) (parity ? 0 : 1));
155
156    m_spi.write(buf, 4);
157    m_spi.read(false, buf, 4);
158
159    if ((buf.get(0) & 0xe0) == 0) {
160      return 0; // error, return 0
161    }
162    return (buf.getInt(0) >> 5) & 0xffff;
163  }
164
165  @Override
166  public void reset() {
167    if (m_simAngle != null) {
168      m_simAngle.reset();
169    }
170    if (m_spi != null) {
171      m_spi.resetAccumulator();
172    }
173  }
174
175  /** Delete (free) the spi port used for the gyro and stop accumulating. */
176  @Override
177  public void close() {
178    SendableRegistry.remove(this);
179    if (m_spi != null) {
180      m_spi.close();
181      m_spi = null;
182    }
183    if (m_simDevice != null) {
184      m_simDevice.close();
185      m_simDevice = null;
186    }
187  }
188
189  @Override
190  public double getAngle() {
191    if (m_simAngle != null) {
192      return m_simAngle.get();
193    }
194    if (m_spi == null) {
195      return 0.0;
196    }
197    return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
198  }
199
200  @Override
201  public double getRate() {
202    if (m_simRate != null) {
203      return m_simRate.get();
204    }
205    if (m_spi == null) {
206      return 0.0;
207    }
208    return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
209  }
210
211  @Override
212  public void initSendable(SendableBuilder builder) {
213    builder.setSmartDashboardType("Gyro");
214    builder.addDoubleProperty("Value", this::getAngle, null);
215  }
216}