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}