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