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.SimDevice;
010import edu.wpi.first.hal.SimDouble;
011import edu.wpi.first.hal.SimEnum;
012import edu.wpi.first.networktables.DoublePublisher;
013import edu.wpi.first.networktables.DoubleTopic;
014import edu.wpi.first.networktables.NTSendable;
015import edu.wpi.first.networktables.NTSendableBuilder;
016import edu.wpi.first.util.sendable.SendableRegistry;
017import java.nio.ByteBuffer;
018import java.nio.ByteOrder;
019
020/**
021 * ADXL362 SPI Accelerometer.
022 *
023 * <p>This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
024 */
025public class ADXL362 implements NTSendable, AutoCloseable {
026  private static final byte kRegWrite = 0x0A;
027  private static final byte kRegRead = 0x0B;
028
029  private static final byte kPartIdRegister = 0x02;
030  private static final byte kDataRegister = 0x0E;
031  private static final byte kFilterCtlRegister = 0x2C;
032  private static final byte kPowerCtlRegister = 0x2D;
033
034  private static final byte kFilterCtl_Range2G = 0x00;
035  private static final byte kFilterCtl_Range4G = 0x40;
036  private static final byte kFilterCtl_Range8G = (byte) 0x80;
037  private static final byte kFilterCtl_ODR_100Hz = 0x03;
038
039  private static final byte kPowerCtl_UltraLowNoise = 0x20;
040
041  @SuppressWarnings("PMD.UnusedPrivateField")
042  private static final byte kPowerCtl_AutoSleep = 0x04;
043
044  private static final byte kPowerCtl_Measure = 0x02;
045
046  public enum Range {
047    k2G,
048    k4G,
049    k8G
050  }
051
052  public enum Axes {
053    kX((byte) 0x00),
054    kY((byte) 0x02),
055    kZ((byte) 0x04);
056
057    public final byte value;
058
059    Axes(byte value) {
060      this.value = value;
061    }
062  }
063
064  @SuppressWarnings("MemberName")
065  public static class AllAxes {
066    public double XAxis;
067    public double YAxis;
068    public double ZAxis;
069  }
070
071  private SPI m_spi;
072
073  private SimDevice m_simDevice;
074  private SimEnum m_simRange;
075  private SimDouble m_simX;
076  private SimDouble m_simY;
077  private SimDouble m_simZ;
078
079  private double m_gsPerLSB;
080
081  /**
082   * Constructor. Uses the onboard CS1.
083   *
084   * @param range The range (+ or -) that the accelerometer will measure.
085   */
086  public ADXL362(Range range) {
087    this(SPI.Port.kOnboardCS1, range);
088  }
089
090  /**
091   * Constructor.
092   *
093   * @param port The SPI port that the accelerometer is connected to
094   * @param range The range (+ or -) that the accelerometer will measure.
095   */
096  @SuppressWarnings("this-escape")
097  public ADXL362(SPI.Port port, Range range) {
098    m_spi = new SPI(port);
099
100    // simulation
101    m_simDevice = SimDevice.create("Accel:ADXL362", port.value);
102    if (m_simDevice != null) {
103      m_simRange =
104          m_simDevice.createEnumDouble(
105              "range",
106              SimDevice.Direction.kOutput,
107              new String[] {"2G", "4G", "8G", "16G"},
108              new double[] {2.0, 4.0, 8.0, 16.0},
109              0);
110      m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
111      m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
112      m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
113    }
114
115    m_spi.setClockRate(3000000);
116    m_spi.setMode(SPI.Mode.kMode3);
117    m_spi.setChipSelectActiveLow();
118
119    ByteBuffer transferBuffer = ByteBuffer.allocate(3);
120    if (m_simDevice == null) {
121      // Validate the part ID
122      transferBuffer.put(0, kRegRead);
123      transferBuffer.put(1, kPartIdRegister);
124      m_spi.transaction(transferBuffer, transferBuffer, 3);
125      if (transferBuffer.get(2) != (byte) 0xF2) {
126        m_spi.close();
127        m_spi = null;
128        DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false);
129        return;
130      }
131    }
132
133    setRange(range);
134
135    // Turn on the measurements
136    transferBuffer.put(0, kRegWrite);
137    transferBuffer.put(1, kPowerCtlRegister);
138    transferBuffer.put(2, (byte) (kPowerCtl_Measure | kPowerCtl_UltraLowNoise));
139    m_spi.write(transferBuffer, 3);
140
141    HAL.report(tResourceType.kResourceType_ADXL362, port.value + 1);
142    SendableRegistry.addLW(this, "ADXL362", port.value);
143  }
144
145  public int getPort() {
146    return m_spi.getPort();
147  }
148
149  @Override
150  public void close() {
151    SendableRegistry.remove(this);
152    if (m_spi != null) {
153      m_spi.close();
154      m_spi = null;
155    }
156    if (m_simDevice != null) {
157      m_simDevice.close();
158      m_simDevice = null;
159    }
160  }
161
162  /**
163   * Set the measuring range of the accelerometer.
164   *
165   * @param range The maximum acceleration, positive or negative, that the accelerometer will
166   *     measure.
167   */
168  public final void setRange(Range range) {
169    if (m_spi == null) {
170      return;
171    }
172
173    final byte value;
174    switch (range) {
175      case k2G:
176        value = kFilterCtl_Range2G;
177        m_gsPerLSB = 0.001;
178        break;
179      case k4G:
180        value = kFilterCtl_Range4G;
181        m_gsPerLSB = 0.002;
182        break;
183      case k8G:
184        value = kFilterCtl_Range8G;
185        m_gsPerLSB = 0.004;
186        break;
187      default:
188        throw new IllegalArgumentException("Missing case for range type " + range);
189    }
190
191    // Specify the data format to read
192    byte[] commands =
193        new byte[] {kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz | value)};
194    m_spi.write(commands, commands.length);
195
196    if (m_simRange != null) {
197      m_simRange.set(value);
198    }
199  }
200
201  /**
202   * Returns the acceleration along the X axis in g-forces.
203   *
204   * @return The acceleration along the X axis in g-forces.
205   */
206  public double getX() {
207    return getAcceleration(Axes.kX);
208  }
209
210  /**
211   * Returns the acceleration along the Y axis in g-forces.
212   *
213   * @return The acceleration along the Y axis in g-forces.
214   */
215  public double getY() {
216    return getAcceleration(Axes.kY);
217  }
218
219  /**
220   * Returns the acceleration along the Z axis in g-forces.
221   *
222   * @return The acceleration along the Z axis in g-forces.
223   */
224  public double getZ() {
225    return getAcceleration(Axes.kZ);
226  }
227
228  /**
229   * Get the acceleration of one axis in Gs.
230   *
231   * @param axis The axis to read from.
232   * @return Acceleration of the ADXL362 in Gs.
233   */
234  public double getAcceleration(ADXL362.Axes axis) {
235    if (axis == Axes.kX && m_simX != null) {
236      return m_simX.get();
237    }
238    if (axis == Axes.kY && m_simY != null) {
239      return m_simY.get();
240    }
241    if (axis == Axes.kZ && m_simZ != null) {
242      return m_simZ.get();
243    }
244    if (m_spi == null) {
245      return 0.0;
246    }
247    ByteBuffer transferBuffer = ByteBuffer.allocate(4);
248    transferBuffer.put(0, kRegRead);
249    transferBuffer.put(1, (byte) (kDataRegister + axis.value));
250    m_spi.transaction(transferBuffer, transferBuffer, 4);
251    // Sensor is little endian
252    transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
253
254    return transferBuffer.getShort(2) * m_gsPerLSB;
255  }
256
257  /**
258   * Get the acceleration of all axes in Gs.
259   *
260   * @return An object containing the acceleration measured on each axis of the ADXL362 in Gs.
261   */
262  public ADXL362.AllAxes getAccelerations() {
263    ADXL362.AllAxes data = new ADXL362.AllAxes();
264    if (m_simX != null && m_simY != null && m_simZ != null) {
265      data.XAxis = m_simX.get();
266      data.YAxis = m_simY.get();
267      data.ZAxis = m_simZ.get();
268      return data;
269    }
270    if (m_spi != null) {
271      ByteBuffer dataBuffer = ByteBuffer.allocate(8);
272      // Select the data address.
273      dataBuffer.put(0, kRegRead);
274      dataBuffer.put(1, kDataRegister);
275      m_spi.transaction(dataBuffer, dataBuffer, 8);
276      // Sensor is little endian... swap bytes
277      dataBuffer.order(ByteOrder.LITTLE_ENDIAN);
278
279      data.XAxis = dataBuffer.getShort(2) * m_gsPerLSB;
280      data.YAxis = dataBuffer.getShort(4) * m_gsPerLSB;
281      data.ZAxis = dataBuffer.getShort(6) * m_gsPerLSB;
282    }
283    return data;
284  }
285
286  @Override
287  public void initSendable(NTSendableBuilder builder) {
288    builder.setSmartDashboardType("3AxisAccelerometer");
289    DoublePublisher pubX = new DoubleTopic(builder.getTopic("X")).publish();
290    DoublePublisher pubY = new DoubleTopic(builder.getTopic("Y")).publish();
291    DoublePublisher pubZ = new DoubleTopic(builder.getTopic("Z")).publish();
292    builder.addCloseable(pubX);
293    builder.addCloseable(pubY);
294    builder.addCloseable(pubZ);
295    builder.setUpdateTable(
296        () -> {
297          AllAxes data = getAccelerations();
298          pubX.set(data.XAxis);
299          pubY.set(data.YAxis);
300          pubZ.set(data.ZAxis);
301        });
302  }
303}