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.tResourceType; 008import edu.wpi.first.hal.HAL; 009import edu.wpi.first.hal.SimDevice; 010import edu.wpi.first.hal.SimDouble; 011import edu.wpi.first.hal.SimEnum; 012import edu.wpi.first.networktables.DoublePublisher; 013import edu.wpi.first.networktables.DoubleTopic; 014import edu.wpi.first.networktables.NTSendable; 015import edu.wpi.first.networktables.NTSendableBuilder; 016import edu.wpi.first.util.sendable.SendableRegistry; 017import java.nio.ByteBuffer; 018import java.nio.ByteOrder; 019 020/** 021 * ADXL362 SPI Accelerometer. 022 * 023 * <p>This class allows access to an Analog Devices ADXL362 3-axis accelerometer. 024 */ 025public class ADXL362 implements NTSendable, AutoCloseable { 026 private static final byte kRegWrite = 0x0A; 027 private static final byte kRegRead = 0x0B; 028 029 private static final byte kPartIdRegister = 0x02; 030 private static final byte kDataRegister = 0x0E; 031 private static final byte kFilterCtlRegister = 0x2C; 032 private static final byte kPowerCtlRegister = 0x2D; 033 034 private static final byte kFilterCtl_Range2G = 0x00; 035 private static final byte kFilterCtl_Range4G = 0x40; 036 private static final byte kFilterCtl_Range8G = (byte) 0x80; 037 private static final byte kFilterCtl_ODR_100Hz = 0x03; 038 039 private static final byte kPowerCtl_UltraLowNoise = 0x20; 040 041 @SuppressWarnings("PMD.UnusedPrivateField") 042 private static final byte kPowerCtl_AutoSleep = 0x04; 043 044 private static final byte kPowerCtl_Measure = 0x02; 045 046 public enum Range { 047 k2G, 048 k4G, 049 k8G 050 } 051 052 public enum Axes { 053 kX((byte) 0x00), 054 kY((byte) 0x02), 055 kZ((byte) 0x04); 056 057 public final byte value; 058 059 Axes(byte value) { 060 this.value = value; 061 } 062 } 063 064 @SuppressWarnings("MemberName") 065 public static class AllAxes { 066 public double XAxis; 067 public double YAxis; 068 public double ZAxis; 069 } 070 071 private SPI m_spi; 072 073 private SimDevice m_simDevice; 074 private SimEnum m_simRange; 075 private SimDouble m_simX; 076 private SimDouble m_simY; 077 private SimDouble m_simZ; 078 079 private double m_gsPerLSB; 080 081 /** 082 * Constructor. Uses the onboard CS1. 083 * 084 * @param range The range (+ or -) that the accelerometer will measure. 085 */ 086 public ADXL362(Range range) { 087 this(SPI.Port.kOnboardCS1, range); 088 } 089 090 /** 091 * Constructor. 092 * 093 * @param port The SPI port that the accelerometer is connected to 094 * @param range The range (+ or -) that the accelerometer will measure. 095 */ 096 @SuppressWarnings("this-escape") 097 public ADXL362(SPI.Port port, Range range) { 098 m_spi = new SPI(port); 099 100 // simulation 101 m_simDevice = SimDevice.create("Accel:ADXL362", port.value); 102 if (m_simDevice != null) { 103 m_simRange = 104 m_simDevice.createEnumDouble( 105 "range", 106 SimDevice.Direction.kOutput, 107 new String[] {"2G", "4G", "8G", "16G"}, 108 new double[] {2.0, 4.0, 8.0, 16.0}, 109 0); 110 m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0); 111 m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0); 112 m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0); 113 } 114 115 m_spi.setClockRate(3000000); 116 m_spi.setMode(SPI.Mode.kMode3); 117 m_spi.setChipSelectActiveLow(); 118 119 ByteBuffer transferBuffer = ByteBuffer.allocate(3); 120 if (m_simDevice == null) { 121 // Validate the part ID 122 transferBuffer.put(0, kRegRead); 123 transferBuffer.put(1, kPartIdRegister); 124 m_spi.transaction(transferBuffer, transferBuffer, 3); 125 if (transferBuffer.get(2) != (byte) 0xF2) { 126 m_spi.close(); 127 m_spi = null; 128 DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false); 129 return; 130 } 131 } 132 133 setRange(range); 134 135 // Turn on the measurements 136 transferBuffer.put(0, kRegWrite); 137 transferBuffer.put(1, kPowerCtlRegister); 138 transferBuffer.put(2, (byte) (kPowerCtl_Measure | kPowerCtl_UltraLowNoise)); 139 m_spi.write(transferBuffer, 3); 140 141 HAL.report(tResourceType.kResourceType_ADXL362, port.value + 1); 142 SendableRegistry.addLW(this, "ADXL362", port.value); 143 } 144 145 public int getPort() { 146 return m_spi.getPort(); 147 } 148 149 @Override 150 public void close() { 151 SendableRegistry.remove(this); 152 if (m_spi != null) { 153 m_spi.close(); 154 m_spi = null; 155 } 156 if (m_simDevice != null) { 157 m_simDevice.close(); 158 m_simDevice = null; 159 } 160 } 161 162 /** 163 * Set the measuring range of the accelerometer. 164 * 165 * @param range The maximum acceleration, positive or negative, that the accelerometer will 166 * measure. 167 */ 168 public final void setRange(Range range) { 169 if (m_spi == null) { 170 return; 171 } 172 173 final byte value; 174 switch (range) { 175 case k2G: 176 value = kFilterCtl_Range2G; 177 m_gsPerLSB = 0.001; 178 break; 179 case k4G: 180 value = kFilterCtl_Range4G; 181 m_gsPerLSB = 0.002; 182 break; 183 case k8G: 184 value = kFilterCtl_Range8G; 185 m_gsPerLSB = 0.004; 186 break; 187 default: 188 throw new IllegalArgumentException("Missing case for range type " + range); 189 } 190 191 // Specify the data format to read 192 byte[] commands = 193 new byte[] {kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz | value)}; 194 m_spi.write(commands, commands.length); 195 196 if (m_simRange != null) { 197 m_simRange.set(value); 198 } 199 } 200 201 /** 202 * Returns the acceleration along the X axis in g-forces. 203 * 204 * @return The acceleration along the X axis in g-forces. 205 */ 206 public double getX() { 207 return getAcceleration(Axes.kX); 208 } 209 210 /** 211 * Returns the acceleration along the Y axis in g-forces. 212 * 213 * @return The acceleration along the Y axis in g-forces. 214 */ 215 public double getY() { 216 return getAcceleration(Axes.kY); 217 } 218 219 /** 220 * Returns the acceleration along the Z axis in g-forces. 221 * 222 * @return The acceleration along the Z axis in g-forces. 223 */ 224 public double getZ() { 225 return getAcceleration(Axes.kZ); 226 } 227 228 /** 229 * Get the acceleration of one axis in Gs. 230 * 231 * @param axis The axis to read from. 232 * @return Acceleration of the ADXL362 in Gs. 233 */ 234 public double getAcceleration(ADXL362.Axes axis) { 235 if (axis == Axes.kX && m_simX != null) { 236 return m_simX.get(); 237 } 238 if (axis == Axes.kY && m_simY != null) { 239 return m_simY.get(); 240 } 241 if (axis == Axes.kZ && m_simZ != null) { 242 return m_simZ.get(); 243 } 244 if (m_spi == null) { 245 return 0.0; 246 } 247 ByteBuffer transferBuffer = ByteBuffer.allocate(4); 248 transferBuffer.put(0, kRegRead); 249 transferBuffer.put(1, (byte) (kDataRegister + axis.value)); 250 m_spi.transaction(transferBuffer, transferBuffer, 4); 251 // Sensor is little endian 252 transferBuffer.order(ByteOrder.LITTLE_ENDIAN); 253 254 return transferBuffer.getShort(2) * m_gsPerLSB; 255 } 256 257 /** 258 * Get the acceleration of all axes in Gs. 259 * 260 * @return An object containing the acceleration measured on each axis of the ADXL362 in Gs. 261 */ 262 public ADXL362.AllAxes getAccelerations() { 263 ADXL362.AllAxes data = new ADXL362.AllAxes(); 264 if (m_simX != null && m_simY != null && m_simZ != null) { 265 data.XAxis = m_simX.get(); 266 data.YAxis = m_simY.get(); 267 data.ZAxis = m_simZ.get(); 268 return data; 269 } 270 if (m_spi != null) { 271 ByteBuffer dataBuffer = ByteBuffer.allocate(8); 272 // Select the data address. 273 dataBuffer.put(0, kRegRead); 274 dataBuffer.put(1, kDataRegister); 275 m_spi.transaction(dataBuffer, dataBuffer, 8); 276 // Sensor is little endian... swap bytes 277 dataBuffer.order(ByteOrder.LITTLE_ENDIAN); 278 279 data.XAxis = dataBuffer.getShort(2) * m_gsPerLSB; 280 data.YAxis = dataBuffer.getShort(4) * m_gsPerLSB; 281 data.ZAxis = dataBuffer.getShort(6) * m_gsPerLSB; 282 } 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}