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