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