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