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}