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