From bd2a28f5973aaf047213a7c49101d3a9143aae35 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 6 Nov 2015 14:49:28 -0800 Subject: [PATCH] Add ADXRS450_Gyro. This is a digital gyro interfaced via SPI. Uses the HAL SPI accumulator. Basic code function successfully tested on a ADXRS453. Change-Id: Ibc0c7db9964c041fb1e04af4db17e3310ea83c04 --- wpilibc/Athena/include/ADXRS450_Gyro.h | 44 +++++ wpilibc/Athena/include/WPILib.h | 1 + wpilibc/Athena/src/ADXRS450_Gyro.cpp | 157 ++++++++++++++++ .../edu/wpi/first/wpilibj/ADXRS450_Gyro.java | 169 ++++++++++++++++++ 4 files changed, 371 insertions(+) create mode 100644 wpilibc/Athena/include/ADXRS450_Gyro.h create mode 100644 wpilibc/Athena/src/ADXRS450_Gyro.cpp create mode 100644 wpilibj/src/athena/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java diff --git a/wpilibc/Athena/include/ADXRS450_Gyro.h b/wpilibc/Athena/include/ADXRS450_Gyro.h new file mode 100644 index 0000000000..42a7a59534 --- /dev/null +++ b/wpilibc/Athena/include/ADXRS450_Gyro.h @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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. */ +/*----------------------------------------------------------------------------*/ +#pragma once + +#include "GyroBase.h" +#include "Notifier.h" +#include "SPI.h" +#include "HAL/cpp/priority_mutex.h" + +#include + +/** + * Use a rate gyro to return the robots heading relative to a starting position. + * The Gyro class tracks the robots heading based on the starting position. As + * the robot rotates the new heading is computed by integrating the rate of + * rotation returned by the sensor. When the class is instantiated, it does a + * short calibration routine where it samples the gyro while at rest to + * determine the default offset. This is subtracted from each sample to + * determine the heading. + * + * This class is for the digital ADXRS450 gyro sensor that connects via SPI. + */ +class ADXRS450_Gyro : public GyroBase { + public: + ADXRS450_Gyro(); + explicit ADXRS450_Gyro(SPI::Port port); + virtual ~ADXRS450_Gyro() = default; + + float GetAngle() const override; + double GetRate() const override; + void Reset() override; + void Calibrate() override; + + std::string GetSmartDashboardType() const override; + + private: + SPI m_spi; + + uint16_t ReadRegister(uint8_t reg); +}; diff --git a/wpilibc/Athena/include/WPILib.h b/wpilibc/Athena/include/WPILib.h index 55d9b5fc5a..0386b03fcf 100644 --- a/wpilibc/Athena/include/WPILib.h +++ b/wpilibc/Athena/include/WPILib.h @@ -14,6 +14,7 @@ #include "ADXL345_I2C.h" #include "ADXL345_SPI.h" #include "ADXL362.h" +#include "ADXRS450_Gyro.h" #include "AnalogAccelerometer.h" #include "AnalogGyro.h" #include "AnalogInput.h" diff --git a/wpilibc/Athena/src/ADXRS450_Gyro.cpp b/wpilibc/Athena/src/ADXRS450_Gyro.cpp new file mode 100644 index 0000000000..2c9917690b --- /dev/null +++ b/wpilibc/Athena/src/ADXRS450_Gyro.cpp @@ -0,0 +1,157 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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. */ +/*----------------------------------------------------------------------------*/ + +#include "ADXRS450_Gyro.h" +#include "DriverStation.h" +#include "LiveWindow/LiveWindow.h" +#include "Timer.h" + +static constexpr double kSamplePeriod = 0.001; +static constexpr double kCalibrationSampleTime = 5.0; +static constexpr double kDegreePerSecondPerLSB = 0.0125; + +static constexpr uint8_t kRateRegister = 0x00; +static constexpr uint8_t kTemRegister = 0x02; +static constexpr uint8_t kLoCSTRegister = 0x04; +static constexpr uint8_t kHiCSTRegister = 0x06; +static constexpr uint8_t kQuadRegister = 0x08; +static constexpr uint8_t kFaultRegister = 0x0A; +static constexpr uint8_t kPIDRegister = 0x0C; +static constexpr uint8_t kSNHighRegister = 0x0E; +static constexpr uint8_t kSNLowRegister = 0x10; + +/** + * Initialize the gyro. + * Calibrate the gyro by running for a number of samples and computing the + * center value. + * Then use the center value as the Accumulator center value for subsequent + * measurements. + * It's important to make sure that the robot is not moving while the centering + * calculations are in progress, this is typically done when the robot is first + * turned on while it's sitting at rest before the competition starts. + */ +void ADXRS450_Gyro::Calibrate() { + Wait(0.1); + + m_spi.SetAccumulatorCenter(0); + m_spi.ResetAccumulator(); + + Wait(kCalibrationSampleTime); + + m_spi.SetAccumulatorCenter((int32_t)m_spi.GetAccumulatorAverage()); + m_spi.ResetAccumulator(); +} + +/** + * Gyro constructor on onboard CS0. + */ +ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {} + +/** + * Gyro constructor on the specified SPI port. + * + * @param port The SPI port the gyro is attached to. + */ +ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port) { + m_spi.SetClockRate(3000000); + m_spi.SetMSBFirst(); + m_spi.SetSampleDataOnRising(); + m_spi.SetClockActiveHigh(); + m_spi.SetChipSelectActiveLow(); + + // Validate the part ID + if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) { + DriverStation::ReportError("could not find ADXRS450 gyro"); + return; + } + + m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c000000u, 0x04000000u, + 10u, 16u, true, true); + + Calibrate(); + + //HALReport(HALUsageReporting::kResourceType_ADXRS450, port); + LiveWindow::GetInstance()->AddSensor("ADXRS450_Gyro", port, this); +} + +static bool CalcParity(uint32_t v) { + bool parity = false; + while (v != 0) { + parity = !parity; + v = v & (v - 1); + } + return parity; +} + +static inline uint32_t BytesToIntBE(uint8_t* buf) { + uint32_t result = ((uint32_t)buf[0]) << 24; + result |= ((uint32_t)buf[1]) << 16; + result |= ((uint32_t)buf[2]) << 8; + result |= (uint32_t)buf[3]; + return result; +} + +uint16_t ADXRS450_Gyro::ReadRegister(uint8_t reg) { + uint32_t cmd = 0x80000000 | (((uint32_t)reg) << 17); + if (!CalcParity(cmd)) cmd |= 1u; + + // big endian + uint8_t buf[4] = {(uint8_t)((cmd >> 24) & 0xff), + (uint8_t)((cmd >> 16) & 0xff), + (uint8_t)((cmd >> 8) & 0xff), + (uint8_t)(cmd & 0xff)}; + + m_spi.Write(buf, 4); + m_spi.Read(false, buf, 4); + if ((buf[0] & 0xe0) == 0) return 0; // error, return 0 + return (uint16_t)((BytesToIntBE(buf) >> 5) & 0xffff); +} + +/** + * Reset the gyro. + * Resets the gyro to a heading of zero. This can be used if there is + * significant + * drift in the gyro and it needs to be recalibrated after it has been running. + */ +void ADXRS450_Gyro::Reset() { + m_spi.ResetAccumulator(); +} + +/** + * Return the actual angle in degrees that the robot is currently facing. + * + * The angle is based on the current accumulator value corrected by the + * oversampling rate, the + * gyro type and the A/D calibration values. + * The angle is continuous, that is it will continue from 360->361 degrees. This + * allows algorithms that wouldn't + * want to see a discontinuity in the gyro output as it sweeps from 360 to 0 on + * the second time around. + * + * @return the current heading of the robot in degrees. This heading is based on + * integration + * of the returned rate from the gyro. + */ +float ADXRS450_Gyro::GetAngle() const { + return (float)(m_spi.GetAccumulatorValue() * kDegreePerSecondPerLSB * + kSamplePeriod); +} + +/** + * Return the rate of rotation of the gyro + * + * The rate is based on the most recent reading of the gyro analog value + * + * @return the current rate in degrees per second + */ +double ADXRS450_Gyro::GetRate() const { + return (double)m_spi.GetAccumulatorLastValue() * kDegreePerSecondPerLSB; +} + +std::string ADXRS450_Gyro::GetSmartDashboardType() const { + return "ADXRS450_Gyro"; +} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java new file mode 100644 index 0000000000..f05844c833 --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java @@ -0,0 +1,169 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2015. 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.Gyro; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.tables.ITable; + +/** + * Use a rate gyro to return the robots heading relative to a starting position. + * The Gyro class tracks the robots heading based on the starting position. As + * the robot rotates the new heading is computed by integrating the rate of + * rotation returned by the sensor. When the class is instantiated, it does a + * short calibration routine where it samples the gyro while at rest to + * determine the default offset. This is subtracted from each sample to + * determine the heading. + * + * This class is for the digital ADXRS450 gyro sensor that connects via SPI. + */ +public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, LiveWindowSendable { + private static final double kSamplePeriod = 0.001; + private static final double kCalibrationSampleTime = 5.0; + private static final double kDegreePerSecondPerLSB = 0.0125; + + private static final int kRateRegister = 0x00; + private static final int kTemRegister = 0x02; + private static final int kLoCSTRegister = 0x04; + private static final int kHiCSTRegister = 0x06; + private static final int kQuadRegister = 0x08; + private static final int kFaultRegister = 0x0A; + private static final int kPIDRegister = 0x0C; + private static final int kSNHighRegister = 0x0E; + private static final int kSNLowRegister = 0x10; + + private SPI m_spi; + + /** + * Constructor. Uses the onboard CS0. + */ + public ADXRS450_Gyro() { + this(SPI.Port.kOnboardCS0); + } + + /** + * Constructor. + * + * @param port The SPI port that the gyro is connected to + */ + public ADXRS450_Gyro(SPI.Port port) { + m_spi = new SPI(port); + m_spi.setClockRate(3000000); + m_spi.setMSBFirst(); + m_spi.setSampleDataOnRising(); + m_spi.setClockActiveHigh(); + m_spi.setChipSelectActiveLow(); + + // Validate the part ID + if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) { + m_spi.free(); + m_spi = null; + DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.getValue(), false); + return; + } + + m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c000000, 0x04000000, + 10, 16, true, true); + + calibrate(); + + //UsageReporting.report(tResourceType.kResourceType_ADXRS450, port.getValue()); + LiveWindow.addSensor("ADXRS450_Gyro", port.getValue(), this); + } + + /** + * {@inheritDoc} + */ + @Override + public void calibrate() { + if (m_spi == null) return; + + Timer.delay(0.1); + + m_spi.setAccumulatorCenter(0); + m_spi.resetAccumulator(); + + Timer.delay(kCalibrationSampleTime); + + m_spi.setAccumulatorCenter((int)m_spi.getAccumulatorAverage()); + m_spi.resetAccumulator(); + } + + private boolean calcParity(int v) { + boolean parity = false; + while (v != 0) { + parity = !parity; + v = v & (v - 1); + } + return parity; + } + + private int readRegister(int reg) { + int cmdhi = 0x8000 | (reg << 1); + boolean parity = calcParity(cmdhi); + + ByteBuffer buf = ByteBuffer.allocateDirect(4); + buf.order(ByteOrder.BIG_ENDIAN); + buf.put(0, (byte) (cmdhi >> 8)); + buf.put(1, (byte) (cmdhi & 0xff)); + buf.put(2, (byte) 0); + buf.put(3, (byte) (parity ? 0 : 1)); + + m_spi.write(buf, 4); + m_spi.read(false, buf, 4); + + if ((buf.get(0) & 0xe0) == 0) { + return 0; // error, return 0 + } + return (buf.getInt(0) >> 5) & 0xffff; + } + + /** + * {@inheritDoc} + */ + public void reset() { + m_spi.resetAccumulator(); + } + + /** + * Delete (free) the spi port used for the gyro and stop accumulating. + */ + @Override + public void free() { + if (m_spi != null) { + m_spi.free(); + m_spi = null; + } + } + + /** + * {@inheritDoc} + */ + public double getAngle() { + if (m_spi == null) return 0.0; + return m_spi.getAccumulatorValue() * kDegreePerSecondPerLSB * kSamplePeriod; + } + + /** + * {@inheritDoc} + */ + public double getRate() { + if (m_spi == null) return 0.0; + return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB; + } + + public String getSmartDashboardType() { + return "ADXRS450_Gyro"; + } +}