Hey guys, our team is trying to get our spartan board’s gyro to work with out 2019 code. Previously we’ve used the class below in 2018 to get gyro values from our spartan board, and it works perfectly with the 2018 build system and libraries.
However, once we update the RoboRIO to 2019, the gyro keeps returning 0.0 without ever changing.
We looked at the difference of the SPI library from 2018 to 2019 and the only thing that seems to have changed is the deprication of free() and setSampleDataOnRising(). We’ve even tried using setSampleDataOnLeadingEdge() instead of setSampleDataOnRising() and the issue still persists.
We even tried using the ADXRS450_Gyro class, and we still have the same problem.
Are any other teams experiencing problems with getting gyro values from the spartan board? It’s bizarre how this exact code works with the 2018 build system and not 2019.
Any help is greatly appreciated.
package frc.team3256.warriorlib.hardware;
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2015-2017. 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. */
/*----------------------------------------------------------------------------*/
import edu.wpi.first.hal.FRCNetComm;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
/**
* 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.
* <p>
* <p>This class is for the digital ADXRS453 gyro sensor that connects via SPI.
* <p>
* <p>Modified by Team 3256
*/
@SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"})
public class ADXRS453_Gyro extends GyroBase implements Gyro {
private static final double kSamplePeriod = 0.001;
public 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;
public boolean m_isCalibrating = false;
private double m_prevCenter = 0.0;
private SPI m_spi;
/**
* Constructor. Uses the onboard CS0.
*/
public ADXRS453_Gyro() {
this(SPI.Port.kOnboardCS0);
}
/**
* Constructor.
*
* @param port The SPI port that the gyro is connected to
*/
public ADXRS453_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 ADXRS453 gyro on SPI port " + port.value, false);
return;
}
m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16, true, true);
calibrate();
HAL.report(FRCNetComm.tResourceType.kResourceType_ADXRS450, port.value);
LiveWindow.addSensor("ADXRS453_Gyro", port.value, this);
}
@Override
public void calibrate() {
if (m_spi == null) {
return;
}
Timer.delay(0.1);
startCalibrate();
Timer.delay(kCalibrationSampleTime);
endCalibrate();
}
public void cancelCalibrate() {
if (m_spi == null) {
return;
}
if (!m_isCalibrating) {
return;
}
m_isCalibrating = false;
m_spi.setAccumulatorCenter((int) Math.round(m_prevCenter));
m_spi.resetAccumulator();
}
public void startCalibrate() {
if (m_spi == null) {
return;
}
if (m_isCalibrating) {
return;
}
m_isCalibrating = true;
m_spi.setAccumulatorCenter(0);
m_spi.resetAccumulator();
}
public void endCalibrate() {
if (m_spi == null) {
return;
}
if (!m_isCalibrating) {
return;
}
m_isCalibrating = false;
m_prevCenter = m_spi.getAccumulatorAverage();
m_spi.setAccumulatorCenter((int) Math.round(m_prevCenter));
m_spi.resetAccumulator();
}
private boolean calcParity(int value) {
boolean parity = false;
while (value != 0) {
parity = !parity;
value = value & (value - 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;
}
@Override
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;
}
}
@Override
public double getAngle() {
if (m_spi == null) {
return 0.0;
}
return m_spi.getAccumulatorValue() * kDegreePerSecondPerLSB * kSamplePeriod;
}
@Override
public double getRate() {
if (m_spi == null) {
return 0.0;
}
return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
}
}
