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