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