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")
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
042  // private static final int kDataFormat_Justify = 0x04;
043
044  /** Accelerometer range. */
045  public enum Range {
046    /** 2 Gs max. */
047    k2G,
048    /** 4 Gs max. */
049    k4G,
050    /** 8 Gs max. */
051    k8G,
052    /** 16 Gs max. */
053    k16G
054  }
055
056  /** Accelerometer axes. */
057  public enum Axes {
058    /** X axis. */
059    kX((byte) 0x00),
060    /** Y axis. */
061    kY((byte) 0x02),
062    /** Z axis. */
063    kZ((byte) 0x04);
064
065    /** The integer value representing this enumeration. */
066    public final byte value;
067
068    Axes(byte value) {
069      this.value = value;
070    }
071  }
072
073  /** Container type for accelerations from all axes. */
074  @SuppressWarnings("MemberName")
075  public static class AllAxes {
076    /** Acceleration along the X axis in g-forces. */
077    public double XAxis;
078
079    /** Acceleration along the Y axis in g-forces. */
080    public double YAxis;
081
082    /** Acceleration along the Z axis in g-forces. */
083    public double ZAxis;
084
085    /** Default constructor. */
086    public AllAxes() {}
087  }
088
089  private SPI m_spi;
090
091  private SimDevice m_simDevice;
092  private SimEnum m_simRange;
093  private SimDouble m_simX;
094  private SimDouble m_simY;
095  private SimDouble m_simZ;
096
097  /**
098   * Constructor.
099   *
100   * @param port The SPI port that the accelerometer is connected to
101   * @param range The range (+ or -) that the accelerometer will measure.
102   */
103  @SuppressWarnings("this-escape")
104  public ADXL345_SPI(SPI.Port port, Range range) {
105    m_spi = new SPI(port);
106    // simulation
107    m_simDevice = SimDevice.create("Accel:ADXL345_SPI", port.value);
108    if (m_simDevice != null) {
109      m_simRange =
110          m_simDevice.createEnumDouble(
111              "range",
112              SimDevice.Direction.kOutput,
113              new String[] {"2G", "4G", "8G", "16G"},
114              new double[] {2.0, 4.0, 8.0, 16.0},
115              0);
116      m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
117      m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
118      m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
119    }
120    init(range);
121    SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
122  }
123
124  /**
125   * Returns the SPI port.
126   *
127   * @return The SPI port.
128   */
129  public int getPort() {
130    return m_spi.getPort();
131  }
132
133  @Override
134  public void close() {
135    SendableRegistry.remove(this);
136    if (m_spi != null) {
137      m_spi.close();
138      m_spi = null;
139    }
140    if (m_simDevice != null) {
141      m_simDevice.close();
142      m_simDevice = null;
143    }
144  }
145
146  /**
147   * Set SPI bus parameters, bring device out of sleep and set format.
148   *
149   * @param range The range (+ or -) that the accelerometer will measure.
150   */
151  private void init(Range range) {
152    m_spi.setClockRate(500000);
153    m_spi.setMode(SPI.Mode.kMode3);
154    m_spi.setChipSelectActiveHigh();
155
156    // Turn on the measurements
157    byte[] commands = new byte[2];
158    commands[0] = kPowerCtlRegister;
159    commands[1] = kPowerCtl_Measure;
160    m_spi.write(commands, 2);
161
162    setRange(range);
163
164    HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_SPI);
165  }
166
167  /**
168   * Set the measuring range of the accelerometer.
169   *
170   * @param range The maximum acceleration, positive or negative, that the accelerometer will
171   *     measure.
172   */
173  public void setRange(Range range) {
174    final byte value;
175    switch (range) {
176      case k2G:
177        value = 0;
178        break;
179      case k4G:
180        value = 1;
181        break;
182      case k8G:
183        value = 2;
184        break;
185      case k16G:
186        value = 3;
187        break;
188      default:
189        throw new IllegalArgumentException("Missing case for range type " + range);
190    }
191
192    // Specify the data format to read
193    byte[] commands = new byte[] {kDataFormatRegister, (byte) (kDataFormat_FullRes | 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 ADXL345 in Gs.
233   */
234  public double getAcceleration(ADXL345_SPI.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    ByteBuffer transferBuffer = ByteBuffer.allocate(3);
245    transferBuffer.put(
246        0, (byte) ((kAddress_Read | kAddress_MultiByte | kDataRegister) + axis.value));
247    m_spi.transaction(transferBuffer, transferBuffer, 3);
248    // Sensor is little endian
249    transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
250
251    return transferBuffer.getShort(1) * kGsPerLSB;
252  }
253
254  /**
255   * Get the acceleration of all axes in Gs.
256   *
257   * @return An object containing the acceleration measured on each axis of the ADXL345 in Gs.
258   */
259  public ADXL345_SPI.AllAxes getAccelerations() {
260    ADXL345_SPI.AllAxes data = new ADXL345_SPI.AllAxes();
261    if (m_simX != null && m_simY != null && m_simZ != null) {
262      data.XAxis = m_simX.get();
263      data.YAxis = m_simY.get();
264      data.ZAxis = m_simZ.get();
265      return data;
266    }
267    if (m_spi != null) {
268      ByteBuffer dataBuffer = ByteBuffer.allocate(7);
269      // Select the data address.
270      dataBuffer.put(0, (byte) (kAddress_Read | kAddress_MultiByte | kDataRegister));
271      m_spi.transaction(dataBuffer, dataBuffer, 7);
272      // Sensor is little endian... swap bytes
273      dataBuffer.order(ByteOrder.LITTLE_ENDIAN);
274
275      data.XAxis = dataBuffer.getShort(1) * kGsPerLSB;
276      data.YAxis = dataBuffer.getShort(3) * kGsPerLSB;
277      data.ZAxis = dataBuffer.getShort(5) * kGsPerLSB;
278    }
279    return data;
280  }
281
282  @Override
283  public void initSendable(NTSendableBuilder builder) {
284    builder.setSmartDashboardType("3AxisAccelerometer");
285    DoublePublisher pubX = new DoubleTopic(builder.getTopic("X")).publish();
286    DoublePublisher pubY = new DoubleTopic(builder.getTopic("Y")).publish();
287    DoublePublisher pubZ = new DoubleTopic(builder.getTopic("Z")).publish();
288    builder.addCloseable(pubX);
289    builder.addCloseable(pubY);
290    builder.addCloseable(pubZ);
291    builder.setUpdateTable(
292        () -> {
293          AllAxes data = getAccelerations();
294          pubX.set(data.XAxis);
295          pubY.set(data.YAxis);
296          pubZ.set(data.ZAxis);
297        });
298  }
299}