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