From c59e08ee9f7f80c0c616c320686d02b0741cfa18 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 14 Nov 2015 23:37:51 -0800 Subject: [PATCH] Add support for ADXL362 SPI accelerometer. Tested with hardware on both C++ and Java. Change-Id: I52f95fc77fffe4efae0c8332007cd683835023ab --- wpilibc/Athena/include/ADXL362.h | 62 +++++ wpilibc/Athena/include/WPILib.h | 1 + wpilibc/Athena/src/ADXL362.cpp | 181 ++++++++++++++ .../java/edu/wpi/first/wpilibj/ADXL362.java | 235 ++++++++++++++++++ 4 files changed, 479 insertions(+) create mode 100644 wpilibc/Athena/include/ADXL362.h create mode 100644 wpilibc/Athena/src/ADXL362.cpp create mode 100644 wpilibj/src/athena/java/edu/wpi/first/wpilibj/ADXL362.java diff --git a/wpilibc/Athena/include/ADXL362.h b/wpilibc/Athena/include/ADXL362.h new file mode 100644 index 0000000000..ea47686f76 --- /dev/null +++ b/wpilibc/Athena/include/ADXL362.h @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008. All Rights Reserved. + */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ +/*----------------------------------------------------------------------------*/ +#pragma once + +#include "interfaces/Accelerometer.h" +#include "SensorBase.h" +#include "SPI.h" +#include "LiveWindow/LiveWindowSendable.h" + +#include + +class DigitalInput; +class DigitalOutput; + +/** + * ADXL362 SPI Accelerometer. + * + * This class allows access to an Analog Devices ADXL362 3-axis accelerometer. + */ +class ADXL362 : public Accelerometer, public LiveWindowSendable { + public: + enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 }; + struct AllAxes { + double XAxis; + double YAxis; + double ZAxis; + }; + + public: + ADXL362(Range range = kRange_2G); + ADXL362(SPI::Port port, Range range = kRange_2G); + virtual ~ADXL362() = default; + + ADXL362(const ADXL362&) = delete; + ADXL362& operator=(const ADXL362&) = delete; + + // Accelerometer interface + virtual void SetRange(Range range) override; + virtual double GetX() override; + virtual double GetY() override; + virtual double GetZ() override; + + virtual double GetAcceleration(Axes axis); + virtual AllAxes GetAccelerations(); + + virtual std::string GetSmartDashboardType() const override; + virtual void InitTable(std::shared_ptr subtable) override; + virtual void UpdateTable() override; + virtual std::shared_ptr GetTable() const override; + virtual void StartLiveWindowMode() override {} + virtual void StopLiveWindowMode() override {} + + private: + SPI m_spi; + double m_gsPerLSB = 0.001; + + std::shared_ptr m_table; +}; diff --git a/wpilibc/Athena/include/WPILib.h b/wpilibc/Athena/include/WPILib.h index 0b2ae21f22..55d9b5fc5a 100644 --- a/wpilibc/Athena/include/WPILib.h +++ b/wpilibc/Athena/include/WPILib.h @@ -13,6 +13,7 @@ #include "ADXL345_I2C.h" #include "ADXL345_SPI.h" +#include "ADXL362.h" #include "AnalogAccelerometer.h" #include "AnalogGyro.h" #include "AnalogInput.h" diff --git a/wpilibc/Athena/src/ADXL362.cpp b/wpilibc/Athena/src/ADXL362.cpp new file mode 100644 index 0000000000..0bd990efad --- /dev/null +++ b/wpilibc/Athena/src/ADXL362.cpp @@ -0,0 +1,181 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008. All Rights Reserved. + */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ +/*----------------------------------------------------------------------------*/ + +#include "ADXL362.h" +#include "DigitalInput.h" +#include "DigitalOutput.h" +#include "DriverStation.h" +#include "LiveWindow/LiveWindow.h" + +static uint8_t kRegWrite = 0x0A; +static uint8_t kRegRead = 0x0B; + +static uint8_t kPartIdRegister = 0x02; +static uint8_t kDataRegister = 0x0E; +static uint8_t kFilterCtlRegister = 0x2C; +static uint8_t kPowerCtlRegister = 0x2D; + +//static uint8_t kFilterCtl_Range2G = 0x00; +//static uint8_t kFilterCtl_Range4G = 0x40; +//static uint8_t kFilterCtl_Range8G = 0x80; +static uint8_t kFilterCtl_ODR_100Hz = 0x03; + +static uint8_t kPowerCtl_UltraLowNoise = 0x20; +//static uint8_t kPowerCtl_AutoSleep = 0x04; +static uint8_t kPowerCtl_Measure = 0x02; + +/** + * Constructor. Uses the onboard CS1. + * + * @param range The range (+ or -) that the accelerometer will measure. + */ +ADXL362::ADXL362(Range range) : ADXL362(SPI::Port::kOnboardCS1, range) {} + +/** + * Constructor. + * + * @param port The SPI port the accelerometer is attached to + * @param range The range (+ or -) that the accelerometer will measure. + */ +ADXL362::ADXL362(SPI::Port port, Range range) : m_spi(port) { + m_spi.SetClockRate(3000000); + m_spi.SetMSBFirst(); + m_spi.SetSampleDataOnFalling(); + m_spi.SetClockActiveLow(); + m_spi.SetChipSelectActiveLow(); + + // Validate the part ID + uint8_t commands[3]; + commands[0] = kRegRead; + commands[1] = kPartIdRegister; + commands[2] = 0; + m_spi.Transaction(commands, commands, 3); + if (commands[2] != 0xF2) { + DriverStation::ReportError("could not find ADXL362"); + m_gsPerLSB = 0.0; + return; + } + + SetRange(range); + + // Turn on the measurements + commands[0] = kRegWrite; + commands[1] = kPowerCtlRegister; + commands[2] = kPowerCtl_Measure | kPowerCtl_UltraLowNoise; + m_spi.Write(commands, 3); + + //HALReport(HALUsageReporting::kResourceType_ADXL362, 0); + + LiveWindow::GetInstance()->AddSensor("ADXL362", port, this); +} + +/** {@inheritdoc} */ +void ADXL362::SetRange(Range range) { + if (m_gsPerLSB == 0.0) return; + + uint8_t commands[3]; + + switch (range) { + case kRange_2G: + m_gsPerLSB = 0.001; + break; + case kRange_4G: + m_gsPerLSB = 0.002; + break; + case kRange_8G: + case kRange_16G: // 16G not supported; treat as 8G + m_gsPerLSB = 0.004; + break; + } + + // Specify the data format to read + commands[0] = kRegWrite; + commands[1] = kFilterCtlRegister; + commands[2] = kFilterCtl_ODR_100Hz | (uint8_t)((range & 0x03) << 6); + m_spi.Write(commands, 3); +} + +/** {@inheritdoc} */ +double ADXL362::GetX() { return GetAcceleration(kAxis_X); } + +/** {@inheritdoc} */ +double ADXL362::GetY() { return GetAcceleration(kAxis_Y); } + +/** {@inheritdoc} */ +double ADXL362::GetZ() { return GetAcceleration(kAxis_Z); } + +/** + * Get the acceleration of one axis in Gs. + * + * @param axis The axis to read from. + * @return Acceleration of the ADXL362 in Gs. + */ +double ADXL362::GetAcceleration(ADXL362::Axes axis) { + if (m_gsPerLSB == 0.0) return 0.0; + + uint8_t buffer[4]; + uint8_t command[4] = {0, 0, 0, 0}; + command[0] = kRegRead; + command[1] = kDataRegister + (uint8_t)axis; + m_spi.Transaction(command, buffer, 4); + + // Sensor is little endian... swap bytes + int16_t rawAccel = buffer[3] << 8 | buffer[2]; + return rawAccel * m_gsPerLSB; +} + +/** + * Get the acceleration of all axes in Gs. + * + * @return An object containing the acceleration measured on each axis of the + * ADXL362 in Gs. + */ +ADXL362::AllAxes ADXL362::GetAccelerations() { + AllAxes data = AllAxes(); + if (m_gsPerLSB == 0.0) { + data.XAxis = data.YAxis = data.ZAxis = 0.0; + return data; + } + + uint8_t dataBuffer[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + int16_t rawData[3]; + + // Select the data address. + dataBuffer[0] = kRegRead; + dataBuffer[1] = kDataRegister; + m_spi.Transaction(dataBuffer, dataBuffer, 8); + + for (int32_t i = 0; i < 3; i++) { + // Sensor is little endian... swap bytes + rawData[i] = dataBuffer[i * 2 + 3] << 8 | dataBuffer[i * 2 + 2]; + } + + data.XAxis = rawData[0] * m_gsPerLSB; + data.YAxis = rawData[1] * m_gsPerLSB; + data.ZAxis = rawData[2] * m_gsPerLSB; + + return data; +} + +std::string ADXL362::GetSmartDashboardType() const { + return "3AxisAccelerometer"; +} + +void ADXL362::InitTable(std::shared_ptr subtable) { + m_table = subtable; + UpdateTable(); +} + +void ADXL362::UpdateTable() { + if (m_table != nullptr) { + m_table->PutNumber("X", GetX()); + m_table->PutNumber("Y", GetY()); + m_table->PutNumber("Z", GetZ()); + } +} + +std::shared_ptr ADXL362::GetTable() const { return m_table; } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/ADXL362.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/ADXL362.java new file mode 100644 index 0000000000..d451e8eaf7 --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/ADXL362.java @@ -0,0 +1,235 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ +package edu.wpi.first.wpilibj; + +import java.nio.ByteOrder; +import java.nio.ByteBuffer; + +import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tInstances; +import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType; +import edu.wpi.first.wpilibj.communication.UsageReporting; +import edu.wpi.first.wpilibj.interfaces.Accelerometer; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.tables.ITable; + +/** + * ADXL362 SPI Accelerometer. + * + * This class allows access to an Analog Devices ADXL362 3-axis accelerometer. + */ +public class ADXL362 extends SensorBase implements Accelerometer, LiveWindowSendable { + private static final byte kRegWrite = 0x0A; + private static final byte kRegRead = 0x0B; + + private static final byte kPartIdRegister = 0x02; + private static final byte kDataRegister = 0x0E; + private static final byte kFilterCtlRegister = 0x2C; + private static final byte kPowerCtlRegister = 0x2D; + + private static final byte kFilterCtl_Range2G = 0x00; + private static final byte kFilterCtl_Range4G = 0x40; + private static final byte kFilterCtl_Range8G = (byte) 0x80; + private static final byte kFilterCtl_ODR_100Hz = 0x03; + + private static final byte kPowerCtl_UltraLowNoise = 0x20; + private static final byte kPowerCtl_AutoSleep = 0x04; + private static final byte kPowerCtl_Measure = 0x02; + + public static enum Axes { + kX((byte) 0x00), + kY((byte) 0x02), + kZ((byte) 0x04); + + /** + * The integer value representing this enumeration + */ + public final byte value; + + private Axes(byte value) { + this.value = value; + } + } + + public static class AllAxes { + + public double XAxis; + public double YAxis; + public double ZAxis; + } + + private SPI m_spi; + private double m_gsPerLSB; + + /** + * Constructor. Uses the onboard CS1. + * + * @param range The range (+ or -) that the accelerometer will measure. + */ + public ADXL362(Range range) { + this(SPI.Port.kOnboardCS1, range); + } + + /** + * Constructor. + * + * @param port The SPI port that the accelerometer is connected to + * @param range The range (+ or -) that the accelerometer will measure. + */ + public ADXL362(SPI.Port port, Range range) { + m_spi = new SPI(port); + m_spi.setClockRate(3000000); + m_spi.setMSBFirst(); + m_spi.setSampleDataOnFalling(); + m_spi.setClockActiveLow(); + m_spi.setChipSelectActiveLow(); + + // Validate the part ID + ByteBuffer transferBuffer = ByteBuffer.allocateDirect(3); + transferBuffer.put(0, kRegRead); + transferBuffer.put(1, kPartIdRegister); + m_spi.transaction(transferBuffer, transferBuffer, 3); + if (transferBuffer.get(2) != (byte)0xF2) { + m_spi.free(); + m_spi = null; + DriverStation.reportError("could not find ADXL362 on SPI port " + port.getValue(), false); + return; + } + + setRange(range); + + // Turn on the measurements + transferBuffer.put(0, kRegWrite); + transferBuffer.put(1, kPowerCtlRegister); + transferBuffer.put(2, (byte) (kPowerCtl_Measure | kPowerCtl_UltraLowNoise)); + m_spi.write(transferBuffer, 3); + + //UsageReporting.report(tResourceType.kResourceType_ADXL362, 0); + LiveWindow.addSensor("ADXL362", port.getValue(), this); + } + + public void free() { + m_spi.free(); + } + + /** {inheritdoc} */ + @Override + public void setRange(Range range) { + byte value = 0; + + switch (range) { + case k2G: + value = kFilterCtl_Range2G; + m_gsPerLSB = 0.001; + break; + case k4G: + value = kFilterCtl_Range4G; + m_gsPerLSB = 0.002; + break; + case k8G: + case k16G: // 16G not supported; treat as 8G + value = kFilterCtl_Range8G; + m_gsPerLSB = 0.004; + break; + } + + // Specify the data format to read + byte[] commands = new byte[] {kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz | value)}; + m_spi.write(commands, commands.length); + } + + /** {@inheritDoc} */ + @Override + public double getX() { + return getAcceleration(Axes.kX); + } + + /** {@inheritDoc} */ + @Override + public double getY() { + return getAcceleration(Axes.kY); + } + + /** {@inheritDoc} */ + @Override + public double getZ() { + return getAcceleration(Axes.kZ); + } + + /** + * Get the acceleration of one axis in Gs. + * + * @param axis The axis to read from. + * @return Acceleration of the ADXL362 in Gs. + */ + public double getAcceleration(ADXL362.Axes axis) { + if (m_spi == null) + return 0.0; + ByteBuffer transferBuffer = ByteBuffer.allocateDirect(4); + transferBuffer.put(0, kRegRead); + transferBuffer.put(1, (byte) (kDataRegister + axis.value)); + m_spi.transaction(transferBuffer, transferBuffer, 4); + // Sensor is little endian + transferBuffer.order(ByteOrder.LITTLE_ENDIAN); + + return transferBuffer.getShort(2) * m_gsPerLSB; + } + + /** + * Get the acceleration of all axes in Gs. + * + * @return An object containing the acceleration measured on each axis of the + * ADXL362 in Gs. + */ + public ADXL362.AllAxes getAccelerations() { + ADXL362.AllAxes data = new ADXL362.AllAxes(); + if (m_spi != null) { + ByteBuffer dataBuffer = ByteBuffer.allocateDirect(8); + // Select the data address. + dataBuffer.put(0, kRegRead); + dataBuffer.put(1, kDataRegister); + m_spi.transaction(dataBuffer, dataBuffer, 8); + // Sensor is little endian... swap bytes + dataBuffer.order(ByteOrder.LITTLE_ENDIAN); + + data.XAxis = dataBuffer.getShort(2) * m_gsPerLSB; + data.YAxis = dataBuffer.getShort(4) * m_gsPerLSB; + data.ZAxis = dataBuffer.getShort(6) * m_gsPerLSB; + } + return data; + } + + public String getSmartDashboardType() { + return "3AxisAccelerometer"; + } + + private ITable m_table; + + /** {@inheritDoc} */ + public void initTable(ITable subtable) { + m_table = subtable; + updateTable(); + } + + /** {@inheritDoc} */ + public void updateTable() { + if (m_table != null) { + m_table.putNumber("X", getX()); + m_table.putNumber("Y", getY()); + m_table.putNumber("Z", getZ()); + } + } + + /** {@inheritDoc} */ + public ITable getTable() { + return m_table; + } + + public void startLiveWindowMode() {} + + public void stopLiveWindowMode() {} +}