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.NTSendable;
014import edu.wpi.first.networktables.NTSendableBuilder;
015import edu.wpi.first.networktables.NetworkTableEntry;
016import edu.wpi.first.util.sendable.SendableRegistry;
017import edu.wpi.first.wpilibj.interfaces.Accelerometer;
018import java.nio.ByteBuffer;
019import java.nio.ByteOrder;
020
021/** ADXL345 I2C Accelerometer. */
022@SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
023public class ADXL345_I2C implements Accelerometer, NTSendable, AutoCloseable {
024  private static final byte kAddress = 0x1D;
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  private static final byte kDataFormat_Justify = 0x04;
039
040  public enum Axes {
041    kX((byte) 0x00),
042    kY((byte) 0x02),
043    kZ((byte) 0x04);
044
045    /** The integer value representing this enumeration. */
046    @SuppressWarnings("MemberName")
047    public final byte value;
048
049    Axes(byte value) {
050      this.value = value;
051    }
052  }
053
054  @SuppressWarnings("MemberName")
055  public static class AllAxes {
056    public double XAxis;
057    public double YAxis;
058    public double ZAxis;
059  }
060
061  protected I2C m_i2c;
062
063  protected SimDevice m_simDevice;
064  protected SimEnum m_simRange;
065  protected SimDouble m_simX;
066  protected SimDouble m_simY;
067  protected SimDouble m_simZ;
068
069  /**
070   * Constructs the ADXL345 Accelerometer with I2C address 0x1D.
071   *
072   * @param port The I2C port the accelerometer is attached to
073   * @param range The range (+ or -) that the accelerometer will measure.
074   */
075  public ADXL345_I2C(I2C.Port port, Range range) {
076    this(port, range, kAddress);
077  }
078
079  /**
080   * Constructs the ADXL345 Accelerometer over I2C.
081   *
082   * @param port The I2C port the accelerometer is attached to
083   * @param range The range (+ or -) that the accelerometer will measure.
084   * @param deviceAddress I2C address of the accelerometer (0x1D or 0x53)
085   */
086  public ADXL345_I2C(I2C.Port port, Range range, int deviceAddress) {
087    m_i2c = new I2C(port, deviceAddress);
088
089    // simulation
090    m_simDevice = SimDevice.create("Accel:ADXL345_I2C", port.value, deviceAddress);
091    if (m_simDevice != null) {
092      m_simRange =
093          m_simDevice.createEnumDouble(
094              "range",
095              SimDevice.Direction.kOutput,
096              new String[] {"2G", "4G", "8G", "16G"},
097              new double[] {2.0, 4.0, 8.0, 16.0},
098              0);
099      m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
100      m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
101      m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
102    }
103
104    // Turn on the measurements
105    m_i2c.write(kPowerCtlRegister, kPowerCtl_Measure);
106
107    setRange(range);
108
109    HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_I2C);
110    SendableRegistry.addLW(this, "ADXL345_I2C", port.value);
111  }
112
113  public int getPort() {
114    return m_i2c.getPort();
115  }
116
117  public int getDeviceAddress() {
118    return m_i2c.getDeviceAddress();
119  }
120
121  @Override
122  public void close() {
123    SendableRegistry.remove(this);
124    if (m_i2c != null) {
125      m_i2c.close();
126      m_i2c = null;
127    }
128    if (m_simDevice != null) {
129      m_simDevice.close();
130      m_simDevice = null;
131    }
132  }
133
134  @Override
135  public void setRange(Range range) {
136    final byte value;
137
138    switch (range) {
139      case k2G:
140        value = 0;
141        break;
142      case k4G:
143        value = 1;
144        break;
145      case k8G:
146        value = 2;
147        break;
148      case k16G:
149        value = 3;
150        break;
151      default:
152        throw new IllegalArgumentException(range + " unsupported range type");
153    }
154
155    // Specify the data format to read
156    m_i2c.write(kDataFormatRegister, kDataFormat_FullRes | value);
157
158    if (m_simRange != null) {
159      m_simRange.set(value);
160    }
161  }
162
163  @Override
164  public double getX() {
165    return getAcceleration(Axes.kX);
166  }
167
168  @Override
169  public double getY() {
170    return getAcceleration(Axes.kY);
171  }
172
173  @Override
174  public double getZ() {
175    return getAcceleration(Axes.kZ);
176  }
177
178  /**
179   * Get the acceleration of one axis in Gs.
180   *
181   * @param axis The axis to read from.
182   * @return Acceleration of the ADXL345 in Gs.
183   */
184  public double getAcceleration(Axes axis) {
185    if (axis == Axes.kX && m_simX != null) {
186      return m_simX.get();
187    }
188    if (axis == Axes.kY && m_simY != null) {
189      return m_simY.get();
190    }
191    if (axis == Axes.kZ && m_simZ != null) {
192      return m_simZ.get();
193    }
194    ByteBuffer rawAccel = ByteBuffer.allocate(2);
195    m_i2c.read(kDataRegister + axis.value, 2, rawAccel);
196
197    // Sensor is little endian... swap bytes
198    rawAccel.order(ByteOrder.LITTLE_ENDIAN);
199    return rawAccel.getShort(0) * kGsPerLSB;
200  }
201
202  /**
203   * Get the acceleration of all axes in Gs.
204   *
205   * @return An object containing the acceleration measured on each axis of the ADXL345 in Gs.
206   */
207  public AllAxes getAccelerations() {
208    AllAxes data = new AllAxes();
209    if (m_simX != null && m_simY != null && m_simZ != null) {
210      data.XAxis = m_simX.get();
211      data.YAxis = m_simY.get();
212      data.ZAxis = m_simZ.get();
213      return data;
214    }
215    ByteBuffer rawData = ByteBuffer.allocate(6);
216    m_i2c.read(kDataRegister, 6, rawData);
217
218    // Sensor is little endian... swap bytes
219    rawData.order(ByteOrder.LITTLE_ENDIAN);
220    data.XAxis = rawData.getShort(0) * kGsPerLSB;
221    data.YAxis = rawData.getShort(2) * kGsPerLSB;
222    data.ZAxis = rawData.getShort(4) * kGsPerLSB;
223    return data;
224  }
225
226  @Override
227  public void initSendable(NTSendableBuilder builder) {
228    builder.setSmartDashboardType("3AxisAccelerometer");
229    NetworkTableEntry entryX = builder.getEntry("X");
230    NetworkTableEntry entryY = builder.getEntry("Y");
231    NetworkTableEntry entryZ = builder.getEntry("Z");
232    builder.setUpdateTable(
233        () -> {
234          AllAxes data = getAccelerations();
235          entryX.setDouble(data.XAxis);
236          entryY.setDouble(data.YAxis);
237          entryZ.setDouble(data.ZAxis);
238        });
239  }
240}