Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal, wpilib] Remove SPI support (#7678)

Browse Source
This commit is contained in:
Thad House
2025-01-17 00:22:29 -08:00
committed by GitHub
parent dc335ddedb
commit 92f0a3c961
84 changed files with 12 additions and 12763 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1078
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
View File

File diff suppressed because it is too large Load Diff

1084
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
View File

File diff suppressed because it is too large Load Diff

289
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
View File

@@ -1,289 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.hal.SimEnum;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.DoubleTopic;
import edu.wpi.first.networktables.NTSendable;
import edu.wpi.first.networktables.NTSendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
/** ADXL345 SPI Accelerometer. */
@SuppressWarnings("TypeName")
public class ADXL345_SPI implements NTSendable, AutoCloseable {
private static final int kPowerCtlRegister = 0x2D;
private static final int kDataFormatRegister = 0x31;
private static final int kDataRegister = 0x32;
private static final double kGsPerLSB = 0.00390625;
private static final int kAddress_Read = 0x80;
private static final int kAddress_MultiByte = 0x40;
// private static final int kPowerCtl_Link = 0x20;
// private static final int kPowerCtl_AutoSleep = 0x10;
private static final int kPowerCtl_Measure = 0x08;
// private static final int kPowerCtl_Sleep = 0x04;
// private static final int kDataFormat_SelfTest = 0x80;
// private static final int kDataFormat_SPI = 0x40;
// private static final int kDataFormat_IntInvert = 0x20;
private static final int kDataFormat_FullRes = 0x08;
// private static final int kDataFormat_Justify = 0x04;
/** Accelerometer range. */
public enum Range {
/** 2 Gs max. */
k2G,
/** 4 Gs max. */
k4G,
/** 8 Gs max. */
k8G,
/** 16 Gs max. */
k16G
}
/** Accelerometer axes. */
public enum Axes {
/** X axis. */
kX((byte) 0x00),
/** Y axis. */
kY((byte) 0x02),
/** Z axis. */
kZ((byte) 0x04);
/** The integer value representing this enumeration. */
public final byte value;
Axes(byte value) {
this.value = value;
}
}
/** Container type for accelerations from all axes. */
@SuppressWarnings("MemberName")
public static class AllAxes {
/** Acceleration along the X axis in g-forces. */
public double XAxis;
/** Acceleration along the Y axis in g-forces. */
public double YAxis;
/** Acceleration along the Z axis in g-forces. */
public double ZAxis;
/** Default constructor. */
public AllAxes() {}
}
private SPI m_spi;
private SimDevice m_simDevice;
private SimEnum m_simRange;
private SimDouble m_simX;
private SimDouble m_simY;
private SimDouble m_simZ;
/**
* Constructor.
*
* @param port The SPI port that the accelerometer is connected to
* @param range The range (+ or -) that the accelerometer will measure.
*/
@SuppressWarnings("this-escape")
public ADXL345_SPI(SPI.Port port, Range range) {
m_spi = new SPI(port);
// simulation
m_simDevice = SimDevice.create("Accel:ADXL345_SPI", port.value);
if (m_simDevice != null) {
m_simRange =
m_simDevice.createEnumDouble(
"range",
SimDevice.Direction.kOutput,
new String[] {"2G", "4G", "8G", "16G"},
new double[] {2.0, 4.0, 8.0, 16.0},
0);
m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
}
init(range);
SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
}
/**
* Returns the SPI port.
*
* @return The SPI port.
*/
public int getPort() {
return m_spi.getPort();
}
@Override
public void close() {
SendableRegistry.remove(this);
if (m_spi != null) {
m_spi.close();
m_spi = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
/**
* Set SPI bus parameters, bring device out of sleep and set format.
*
* @param range The range (+ or -) that the accelerometer will measure.
*/
private void init(Range range) {
m_spi.setClockRate(500000);
m_spi.setMode(SPI.Mode.kMode3);
m_spi.setChipSelectActiveHigh();
// Turn on the measurements
byte[] commands = new byte[2];
commands[0] = kPowerCtlRegister;
commands[1] = kPowerCtl_Measure;
m_spi.write(commands, 2);
setRange(range);
HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_SPI);
}
/**
* Set the measuring range of the accelerometer.
*
* @param range The maximum acceleration, positive or negative, that the accelerometer will
* measure.
*/
public void setRange(Range range) {
final byte value =
switch (range) {
case k2G -> 0;
case k4G -> 1;
case k8G -> 2;
case k16G -> 3;
};
// Specify the data format to read
byte[] commands = new byte[] {kDataFormatRegister, (byte) (kDataFormat_FullRes | value)};
m_spi.write(commands, commands.length);
if (m_simRange != null) {
m_simRange.set(value);
}
}
/**
* Returns the acceleration along the X axis in g-forces.
*
* @return The acceleration along the X axis in g-forces.
*/
public double getX() {
return getAcceleration(Axes.kX);
}
/**
* Returns the acceleration along the Y axis in g-forces.
*
* @return The acceleration along the Y axis in g-forces.
*/
public double getY() {
return getAcceleration(Axes.kY);
}
/**
* Returns the acceleration along the Z axis in g-forces.
*
* @return The acceleration along the Z axis in g-forces.
*/
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 ADXL345 in Gs.
*/
public double getAcceleration(ADXL345_SPI.Axes axis) {
if (axis == Axes.kX && m_simX != null) {
return m_simX.get();
}
if (axis == Axes.kY && m_simY != null) {
return m_simY.get();
}
if (axis == Axes.kZ && m_simZ != null) {
return m_simZ.get();
}
ByteBuffer transferBuffer = ByteBuffer.allocate(3);
transferBuffer.put(
0, (byte) ((kAddress_Read | kAddress_MultiByte | kDataRegister) + axis.value));
m_spi.transaction(transferBuffer, transferBuffer, 3);
// Sensor is little endian
transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
return transferBuffer.getShort(1) * kGsPerLSB;
}
/**
* Get the acceleration of all axes in Gs.
*
* @return An object containing the acceleration measured on each axis of the ADXL345 in Gs.
*/
public ADXL345_SPI.AllAxes getAccelerations() {
ADXL345_SPI.AllAxes data = new ADXL345_SPI.AllAxes();
if (m_simX != null && m_simY != null && m_simZ != null) {
data.XAxis = m_simX.get();
data.YAxis = m_simY.get();
data.ZAxis = m_simZ.get();
return data;
}
if (m_spi != null) {
ByteBuffer dataBuffer = ByteBuffer.allocate(7);
// Select the data address.
dataBuffer.put(0, (byte) (kAddress_Read | kAddress_MultiByte | kDataRegister));
m_spi.transaction(dataBuffer, dataBuffer, 7);
// Sensor is little endian... swap bytes
dataBuffer.order(ByteOrder.LITTLE_ENDIAN);
data.XAxis = dataBuffer.getShort(1) * kGsPerLSB;
data.YAxis = dataBuffer.getShort(3) * kGsPerLSB;
data.ZAxis = dataBuffer.getShort(5) * kGsPerLSB;
}
return data;
}
@Override
public void initSendable(NTSendableBuilder builder) {
builder.setSmartDashboardType("3AxisAccelerometer");
DoublePublisher pubX = new DoubleTopic(builder.getTopic("X")).publish();
DoublePublisher pubY = new DoubleTopic(builder.getTopic("Y")).publish();
DoublePublisher pubZ = new DoubleTopic(builder.getTopic("Z")).publish();
builder.addCloseable(pubX);
builder.addCloseable(pubY);
builder.addCloseable(pubZ);
builder.setUpdateTable(
() -> {
AllAxes data = getAccelerations();
pubX.set(data.XAxis);
pubY.set(data.YAxis);
pubZ.set(data.ZAxis);
});
}
}

324
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
View File

@@ -1,324 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.hal.SimEnum;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.DoubleTopic;
import edu.wpi.first.networktables.NTSendable;
import edu.wpi.first.networktables.NTSendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
/**
* ADXL362 SPI Accelerometer.
*
* <p>This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
*/
public class ADXL362 implements NTSendable, AutoCloseable {
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;
// @SuppressWarnings("PMD.UnusedPrivateField")
// private static final byte kPowerCtl_AutoSleep = 0x04;
private static final byte kPowerCtl_Measure = 0x02;
/** Accelerometer range. */
public enum Range {
/** 2 Gs max. */
k2G,
/** 4 Gs max. */
k4G,
/** 8 Gs max. */
k8G
}
/** Accelerometer axes. */
public enum Axes {
/** X axis. */
kX((byte) 0x00),
/** Y axis. */
kY((byte) 0x02),
/** Z axis. */
kZ((byte) 0x04);
/** Axis value. */
public final byte value;
Axes(byte value) {
this.value = value;
}
}
/** Container type for accelerations from all axes. */
@SuppressWarnings("MemberName")
public static class AllAxes {
/** Acceleration along the X axis in g-forces. */
public double XAxis;
/** Acceleration along the Y axis in g-forces. */
public double YAxis;
/** Acceleration along the Z axis in g-forces. */
public double ZAxis;
/** Default constructor. */
public AllAxes() {}
}
private SPI m_spi;
private SimDevice m_simDevice;
private SimEnum m_simRange;
private SimDouble m_simX;
private SimDouble m_simY;
private SimDouble m_simZ;
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.
*/
@SuppressWarnings("this-escape")
public ADXL362(SPI.Port port, Range range) {
m_spi = new SPI(port);
// simulation
m_simDevice = SimDevice.create("Accel:ADXL362", port.value);
if (m_simDevice != null) {
m_simRange =
m_simDevice.createEnumDouble(
"range",
SimDevice.Direction.kOutput,
new String[] {"2G", "4G", "8G", "16G"},
new double[] {2.0, 4.0, 8.0, 16.0},
0);
m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
}
m_spi.setClockRate(3000000);
m_spi.setMode(SPI.Mode.kMode3);
m_spi.setChipSelectActiveLow();
ByteBuffer transferBuffer = ByteBuffer.allocate(3);
if (m_simDevice == null) {
// Validate the part ID
transferBuffer.put(0, kRegRead);
transferBuffer.put(1, kPartIdRegister);
m_spi.transaction(transferBuffer, transferBuffer, 3);
if (transferBuffer.get(2) != (byte) 0xF2) {
m_spi.close();
m_spi = null;
DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, 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);
HAL.report(tResourceType.kResourceType_ADXL362, port.value + 1);
SendableRegistry.addLW(this, "ADXL362", port.value);
}
/**
* Returns the SPI port.
*
* @return The SPI port.
*/
public int getPort() {
return m_spi.getPort();
}
@Override
public void close() {
SendableRegistry.remove(this);
if (m_spi != null) {
m_spi.close();
m_spi = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
/**
* Set the measuring range of the accelerometer.
*
* @param range The maximum acceleration, positive or negative, that the accelerometer will
* measure.
*/
public final void setRange(Range range) {
if (m_spi == null) {
return;
}
final byte value =
switch (range) {
case k2G -> {
m_gsPerLSB = 0.001;
yield kFilterCtl_Range2G;
}
case k4G -> {
m_gsPerLSB = 0.002;
yield kFilterCtl_Range4G;
}
case k8G -> {
m_gsPerLSB = 0.004;
yield kFilterCtl_Range8G;
}
};
// Specify the data format to read
byte[] commands =
new byte[] {kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz | value)};
m_spi.write(commands, commands.length);
if (m_simRange != null) {
m_simRange.set(value);
}
}
/**
* Returns the acceleration along the X axis in g-forces.
*
* @return The acceleration along the X axis in g-forces.
*/
public double getX() {
return getAcceleration(Axes.kX);
}
/**
* Returns the acceleration along the Y axis in g-forces.
*
* @return The acceleration along the Y axis in g-forces.
*/
public double getY() {
return getAcceleration(Axes.kY);
}
/**
* Returns the acceleration along the Z axis in g-forces.
*
* @return The acceleration along the Z axis in g-forces.
*/
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 (axis == Axes.kX && m_simX != null) {
return m_simX.get();
}
if (axis == Axes.kY && m_simY != null) {
return m_simY.get();
}
if (axis == Axes.kZ && m_simZ != null) {
return m_simZ.get();
}
if (m_spi == null) {
return 0.0;
}
ByteBuffer transferBuffer = ByteBuffer.allocate(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_simX != null && m_simY != null && m_simZ != null) {
data.XAxis = m_simX.get();
data.YAxis = m_simY.get();
data.ZAxis = m_simZ.get();
return data;
}
if (m_spi != null) {
ByteBuffer dataBuffer = ByteBuffer.allocate(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;
}
@Override
public void initSendable(NTSendableBuilder builder) {
builder.setSmartDashboardType("3AxisAccelerometer");
DoublePublisher pubX = new DoubleTopic(builder.getTopic("X")).publish();
DoublePublisher pubY = new DoubleTopic(builder.getTopic("Y")).publish();
DoublePublisher pubZ = new DoubleTopic(builder.getTopic("Z")).publish();
builder.addCloseable(pubX);
builder.addCloseable(pubY);
builder.addCloseable(pubZ);
builder.setUpdateTable(
() -> {
AllAxes data = getAccelerations();
pubX.set(data.XAxis);
pubY.set(data.YAxis);
pubZ.set(data.ZAxis);
});
}
}

267
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
View File

@@ -1,267 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
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>This class is for the digital ADXRS450 gyro sensor that connects via SPI. Only one instance of
* an ADXRS Gyro is supported.
*/
@SuppressWarnings("TypeName")
public class ADXRS450_Gyro implements Sendable, AutoCloseable {
private static final double kSamplePeriod = 0.0005;
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;
private SimDevice m_simDevice;
private SimBoolean m_simConnected;
private SimDouble m_simAngle;
private SimDouble m_simRate;
/** Constructor. Uses the onboard CS0. */
public ADXRS450_Gyro() {
this(SPI.Port.kOnboardCS0);
}
/**
* Constructor.
*
* @param port The SPI port that the gyro is connected to
*/
@SuppressWarnings("this-escape")
public ADXRS450_Gyro(SPI.Port port) {
m_spi = new SPI(port);
// simulation
m_simDevice = SimDevice.create("Gyro:ADXRS450", port.value);
if (m_simDevice != null) {
m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
m_simAngle = m_simDevice.createDouble("angle_x", SimDevice.Direction.kInput, 0.0);
m_simRate = m_simDevice.createDouble("rate_x", SimDevice.Direction.kInput, 0.0);
}
m_spi.setClockRate(3000000);
m_spi.setMode(SPI.Mode.kMode0);
m_spi.setChipSelectActiveLow();
if (m_simDevice == null) {
// Validate the part ID
if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
m_spi.close();
m_spi = null;
DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value, false);
return;
}
m_spi.initAccumulator(
kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16, true, true);
calibrate();
}
HAL.report(tResourceType.kResourceType_ADXRS450, port.value + 1);
SendableRegistry.addLW(this, "ADXRS450_Gyro", port.value);
}
/**
* Determine if the gyro is connected.
*
* @return true if it is connected, false otherwise.
*/
public boolean isConnected() {
if (m_simConnected != null) {
return m_simConnected.get();
}
return m_spi != null;
}
/**
* 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.
*
* <p>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.
*/
public final void calibrate() {
if (m_spi == null) {
return;
}
Timer.delay(0.1);
m_spi.setAccumulatorIntegratedCenter(0);
m_spi.resetAccumulator();
Timer.delay(kCalibrationSampleTime);
m_spi.setAccumulatorIntegratedCenter(m_spi.getAccumulatorIntegratedAverage());
m_spi.resetAccumulator();
}
/**
* Get the SPI port number.
*
* @return The SPI port number.
*/
public int getPort() {
return m_spi.getPort();
}
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.allocate(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;
}
/**
* Reset the gyro.
*
* <p>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.
*/
public void reset() {
if (m_simAngle != null) {
m_simAngle.reset();
}
if (m_spi != null) {
m_spi.resetAccumulator();
}
}
/** Delete (free) the spi port used for the gyro and stop accumulating. */
@Override
public void close() {
SendableRegistry.remove(this);
if (m_spi != null) {
m_spi.close();
m_spi = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
/**
* Return the heading of the robot in degrees.
*
* <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
* algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
* 360 to 0 on the second time around.
*
* <p>The angle is expected to increase as the gyro turns clockwise when looked at from the top.
* It needs to follow the NED axis convention.
*
* <p>This heading is based on integration of the returned rate from the gyro.
*
* @return the current heading of the robot in degrees.
*/
public double getAngle() {
if (m_simAngle != null) {
return m_simAngle.get();
}
if (m_spi == null) {
return 0.0;
}
return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
}
/**
* Return the rate of rotation of the gyro.
*
* <p>The rate is based on the most recent reading of the gyro analog value
*
* <p>The rate is expected to be positive as the gyro turns clockwise when looked at from the top.
* It needs to follow the NED axis convention.
*
* @return the current rate in degrees per second
*/
public double getRate() {
if (m_simRate != null) {
return m_simRate.get();
}
if (m_spi == null) {
return 0.0;
}
return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
}
/**
* Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
*
* <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows
* algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from
* 360 to 0 on the second time around.
*
* <p>The angle is expected to increase as the gyro turns counterclockwise when looked at from the
* top. It needs to follow the NWU axis convention.
*
* <p>This heading is based on integration of the returned rate from the gyro.
*
* @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
*/
public Rotation2d getRotation2d() {
return Rotation2d.fromDegrees(-getAngle());
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}

791
wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
View File

@@ -1,791 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.AccumulatorResult;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SPIJNI;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.IntBuffer;
/** Represents an SPI bus port. */
public class SPI implements AutoCloseable {
/** SPI port. */
public enum Port {
/** Onboard SPI bus port CS0. */
kOnboardCS0(SPIJNI.ONBOARD_CS0_PORT),
/** Onboard SPI bus port CS1. */
kOnboardCS1(SPIJNI.ONBOARD_CS1_PORT),
/** Onboard SPI bus port CS2. */
kOnboardCS2(SPIJNI.ONBOARD_CS2_PORT),
/** Onboard SPI bus port CS3. */
kOnboardCS3(SPIJNI.ONBOARD_CS3_PORT),
/** MXP (roboRIO MXP) SPI bus port. */
kMXP(SPIJNI.MXP_PORT);
/** SPI port value. */
public final int value;
Port(int value) {
this.value = value;
}
}
/** SPI mode. */
public enum Mode {
/** Clock idle low, data sampled on rising edge. */
kMode0(SPIJNI.SPI_MODE0),
/** Clock idle low, data sampled on falling edge. */
kMode1(SPIJNI.SPI_MODE1),
/** Clock idle high, data sampled on falling edge. */
kMode2(SPIJNI.SPI_MODE2),
/** Clock idle high, data sampled on rising edge. */
kMode3(SPIJNI.SPI_MODE3);
/** SPI mode value. */
public final int value;
Mode(int value) {
this.value = value;
}
}
private final int m_port;
/**
* Constructor.
*
* @param port the physical SPI port
*/
public SPI(Port port) {
m_port = port.value;
SPIJNI.spiInitialize(m_port);
SPIJNI.spiSetMode(m_port, 0);
HAL.report(tResourceType.kResourceType_SPI, port.value + 1);
}
/**
* Returns the SPI port value.
*
* @return SPI port value.
*/
public int getPort() {
return m_port;
}
@Override
public void close() {
if (m_accum != null) {
m_accum.close();
m_accum = null;
}
SPIJNI.spiClose(m_port);
}
/**
* Configure the rate of the generated clock signal. The default value is 500,000 Hz. The maximum
* value is 4,000,000 Hz.
*
* @param hz The clock rate in Hertz.
*/
public final void setClockRate(int hz) {
SPIJNI.spiSetSpeed(m_port, hz);
}
/**
* Sets the mode for the SPI device.
*
* <p>Mode 0 is Clock idle low, data sampled on rising edge.
*
* <p>Mode 1 is Clock idle low, data sampled on falling edge.
*
* <p>Mode 2 is Clock idle high, data sampled on falling edge.
*
* <p>Mode 3 is Clock idle high, data sampled on rising edge.
*
* @param mode The mode to set.
*/
public final void setMode(Mode mode) {
SPIJNI.spiSetMode(m_port, mode.value & 0x3);
}
/** Configure the chip select line to be active high. */
public final void setChipSelectActiveHigh() {
SPIJNI.spiSetChipSelectActiveHigh(m_port);
}
/** Configure the chip select line to be active low. */
public final void setChipSelectActiveLow() {
SPIJNI.spiSetChipSelectActiveLow(m_port);
}
/**
* Write data to the peripheral device. Blocks until there is space in the output FIFO.
*
* <p>If not running in output only mode, also saves the data received on the CIPO input during
* the transfer into the receive FIFO.
*
* @param dataToSend The buffer containing the data to send.
* @param size The number of bytes to send.
* @return Number of bytes written or -1 on error.
*/
public int write(byte[] dataToSend, int size) {
if (dataToSend.length < size) {
throw new IllegalArgumentException("buffer is too small, must be at least " + size);
}
return SPIJNI.spiWriteB(m_port, dataToSend, (byte) size);
}
/**
* Write data to the peripheral device. Blocks until there is space in the output FIFO.
*
* <p>If not running in output only mode, also saves the data received on the CIPO input during
* the transfer into the receive FIFO.
*
* @param dataToSend The buffer containing the data to send.
* @param size The number of bytes to send.
* @return Number of bytes written or -1 on error.
*/
public int write(ByteBuffer dataToSend, int size) {
if (dataToSend.hasArray()) {
return write(dataToSend.array(), size);
}
if (!dataToSend.isDirect()) {
throw new IllegalArgumentException("must be a direct buffer");
}
if (dataToSend.capacity() < size) {
throw new IllegalArgumentException("buffer is too small, must be at least " + size);
}
return SPIJNI.spiWrite(m_port, dataToSend, (byte) size);
}
/**
* Read a word from the receive FIFO.
*
* <p>Waits for the current transfer to complete if the receive FIFO is empty.
*
* <p>If the receive FIFO is empty, there is no active transfer, and initiate is false, errors.
*
* @param initiate If true, this function pushes "0" into the transmit buffer and initiates a
* transfer. If false, this function assumes that data is already in the receive FIFO from a
* previous write.
* @param dataReceived Buffer in which to store bytes read.
* @param size Number of bytes to read.
* @return Number of bytes read or -1 on error.
*/
public int read(boolean initiate, byte[] dataReceived, int size) {
if (dataReceived.length < size) {
throw new IllegalArgumentException("buffer is too small, must be at least " + size);
}
return SPIJNI.spiReadB(m_port, initiate, dataReceived, (byte) size);
}
/**
* Read a word from the receive FIFO.
*
* <p>Waits for the current transfer to complete if the receive FIFO is empty.
*
* <p>If the receive FIFO is empty, there is no active transfer, and initiate is false, errors.
*
* @param initiate If true, this function pushes "0" into the transmit buffer and initiates a
* transfer. If false, this function assumes that data is already in the receive FIFO from a
* previous write.
* @param dataReceived The buffer to be filled with the received data.
* @param size The length of the transaction, in bytes
* @return Number of bytes read or -1 on error.
*/
public int read(boolean initiate, ByteBuffer dataReceived, int size) {
if (dataReceived.hasArray()) {
return read(initiate, dataReceived.array(), size);
}
if (!dataReceived.isDirect()) {
throw new IllegalArgumentException("must be a direct buffer");
}
if (dataReceived.capacity() < size) {
throw new IllegalArgumentException("buffer is too small, must be at least " + size);
}
return SPIJNI.spiRead(m_port, initiate, dataReceived, (byte) size);
}
/**
* Perform a simultaneous read/write transaction with the device.
*
* @param dataToSend The data to be written out to the device
* @param dataReceived Buffer to receive data from the device
* @param size The length of the transaction, in bytes
* @return TODO
*/
public int transaction(byte[] dataToSend, byte[] dataReceived, int size) {
if (dataToSend.length < size) {
throw new IllegalArgumentException("dataToSend is too small, must be at least " + size);
}
if (dataReceived.length < size) {
throw new IllegalArgumentException("dataReceived is too small, must be at least " + size);
}
return SPIJNI.spiTransactionB(m_port, dataToSend, dataReceived, (byte) size);
}
/**
* Perform a simultaneous read/write transaction with the device.
*
* @param dataToSend The data to be written out to the device.
* @param dataReceived Buffer to receive data from the device.
* @param size The length of the transaction, in bytes
* @return TODO
*/
public int transaction(ByteBuffer dataToSend, ByteBuffer dataReceived, int size) {
if (dataToSend.hasArray() && dataReceived.hasArray()) {
return transaction(dataToSend.array(), dataReceived.array(), size);
}
if (!dataToSend.isDirect()) {
throw new IllegalArgumentException("dataToSend must be a direct buffer");
}
if (dataToSend.capacity() < size) {
throw new IllegalArgumentException("dataToSend is too small, must be at least " + size);
}
if (!dataReceived.isDirect()) {
throw new IllegalArgumentException("dataReceived must be a direct buffer");
}
if (dataReceived.capacity() < size) {
throw new IllegalArgumentException("dataReceived is too small, must be at least " + size);
}
return SPIJNI.spiTransaction(m_port, dataToSend, dataReceived, (byte) size);
}
/**
* Initialize automatic SPI transfer engine.
*
* <p>Only a single engine is available, and use of it blocks use of all other chip select usage
* on the same physical SPI port while it is running.
*
* @param bufferSize buffer size in bytes
*/
public void initAuto(int bufferSize) {
SPIJNI.spiInitAuto(m_port, bufferSize);
}
/** Frees the automatic SPI transfer engine. */
public void freeAuto() {
SPIJNI.spiFreeAuto(m_port);
}
/**
* Set the data to be transmitted by the engine.
*
* <p>Up to 16 bytes are configurable, and may be followed by up to 127 zero bytes.
*
* @param dataToSend data to send (maximum 16 bytes)
* @param zeroSize number of zeros to send after the data
*/
public void setAutoTransmitData(byte[] dataToSend, int zeroSize) {
SPIJNI.spiSetAutoTransmitData(m_port, dataToSend, zeroSize);
}
/**
* Start running the automatic SPI transfer engine at a periodic rate.
*
* <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must be called before
* calling this function.
*
* @param period period between transfers, in seconds (us resolution)
*/
public void startAutoRate(double period) {
SPIJNI.spiStartAutoRate(m_port, period);
}
/**
* Start running the automatic SPI transfer engine when a trigger occurs.
*
* <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must be called before
* calling this function.
*
* @param source digital source for the trigger (may be an analog trigger)
* @param rising trigger on the rising edge
* @param falling trigger on the falling edge
*/
public void startAutoTrigger(DigitalSource source, boolean rising, boolean falling) {
SPIJNI.spiStartAutoTrigger(
m_port,
source.getPortHandleForRouting(),
source.getAnalogTriggerTypeForRouting(),
rising,
falling);
}
/** Stop running the automatic SPI transfer engine. */
public void stopAuto() {
SPIJNI.spiStopAuto(m_port);
}
/** Force the engine to make a single transfer. */
public void forceAutoRead() {
SPIJNI.spiForceAutoRead(m_port);
}
/**
* Read data that has been transferred by the automatic SPI transfer engine.
*
* <p>Transfers may be made a byte at a time, so it's necessary for the caller to handle cases
* where an entire transfer has not been completed.
*
* <p>Each received data sequence consists of a timestamp followed by the received data bytes, one
* byte per word (in the least significant byte). The length of each received data sequence is the
* same as the combined size of the data and zeroSize set in setAutoTransmitData().
*
* <p>Blocks until numToRead words have been read or timeout expires. May be called with
* numToRead=0 to retrieve how many words are available.
*
* @param buffer buffer where read words are stored
* @param numToRead number of words to read
* @param timeout timeout in seconds (ms resolution)
* @return Number of words remaining to be read
*/
public int readAutoReceivedData(ByteBuffer buffer, int numToRead, double timeout) {
if (!buffer.isDirect()) {
throw new IllegalArgumentException("must be a direct buffer");
}
if (buffer.capacity() < numToRead * 4) {
throw new IllegalArgumentException(
"buffer is too small, must be at least " + (numToRead * 4));
}
return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout);
}
/**
* Read data that has been transferred by the automatic SPI transfer engine.
*
* <p>Transfers may be made a byte at a time, so it's necessary for the caller to handle cases
* where an entire transfer has not been completed.
*
* <p>Each received data sequence consists of a timestamp followed by the received data bytes, one
* byte per word (in the least significant byte). The length of each received data sequence is the
* same as the combined size of the data and zeroSize set in setAutoTransmitData().
*
* <p>Blocks until numToRead words have been read or timeout expires. May be called with
* numToRead=0 to retrieve how many words are available.
*
* @param buffer array where read words are stored
* @param numToRead number of words to read
* @param timeout timeout in seconds (ms resolution)
* @return Number of words remaining to be read
*/
public int readAutoReceivedData(int[] buffer, int numToRead, double timeout) {
if (buffer.length < numToRead) {
throw new IllegalArgumentException("buffer is too small, must be at least " + numToRead);
}
return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout);
}
/**
* Get the number of bytes dropped by the automatic SPI transfer engine due to the receive buffer
* being full.
*
* @return Number of bytes dropped
*/
public int getAutoDroppedCount() {
return SPIJNI.spiGetAutoDroppedCount(m_port);
}
/**
* Configure the Auto SPI Stall time between reads.
*
* @param csToSclkTicks the number of ticks to wait before asserting the cs pin
* @param stallTicks the number of ticks to stall for
* @param pow2BytesPerRead the number of bytes to read before stalling
*/
public void configureAutoStall(int csToSclkTicks, int stallTicks, int pow2BytesPerRead) {
SPIJNI.spiConfigureAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead);
}
private static final int kAccumulateDepth = 2048;
private static class Accumulator implements AutoCloseable {
Accumulator(
int port,
int xferSize,
int validMask,
int validValue,
int dataShift,
int dataSize,
boolean isSigned,
boolean bigEndian) {
m_notifier = new Notifier(this::update);
m_buf =
ByteBuffer.allocateDirect((xferSize + 1) * kAccumulateDepth * 4)
.order(ByteOrder.nativeOrder());
m_intBuf = m_buf.asIntBuffer();
m_xferSize = xferSize + 1; // +1 for timestamp
m_validMask = validMask;
m_validValue = validValue;
m_dataShift = dataShift;
m_dataMax = 1 << dataSize;
m_dataMsbMask = 1 << (dataSize - 1);
m_isSigned = isSigned;
m_bigEndian = bigEndian;
m_port = port;
}
@Override
public void close() {
m_notifier.close();
}
final Notifier m_notifier;
final ByteBuffer m_buf;
final IntBuffer m_intBuf;
final Object m_mutex = new Object();
long m_value;
int m_count;
int m_lastValue;
long m_lastTimestamp;
double m_integratedValue;
int m_center;
int m_deadband;
double m_integratedCenter;
final int m_validMask;
final int m_validValue;
final int m_dataMax; // one more than max data value
final int m_dataMsbMask; // data field MSB mask (for signed)
final int m_dataShift; // data field shift right amount, in bits
final int m_xferSize; // SPI transfer size, in bytes
final boolean m_isSigned; // is data field signed?
final boolean m_bigEndian; // is response big endian?
final int m_port;
void update() {
synchronized (m_mutex) {
boolean done = false;
while (!done) {
done = true;
// get amount of data available
int numToRead = SPIJNI.spiReadAutoReceivedData(m_port, m_buf, 0, 0);
// only get whole responses
numToRead -= numToRead % m_xferSize;
if (numToRead > m_xferSize * kAccumulateDepth) {
numToRead = m_xferSize * kAccumulateDepth;
done = false;
}
if (numToRead == 0) {
return; // no samples
}
// read buffered data
SPIJNI.spiReadAutoReceivedData(m_port, m_buf, numToRead, 0);
// loop over all responses
for (int off = 0; off < numToRead; off += m_xferSize) {
// get timestamp from first word
long timestamp = m_intBuf.get(off) & 0xffffffffL;
// convert from bytes
int resp = 0;
if (m_bigEndian) {
for (int i = 1; i < m_xferSize; ++i) {
resp <<= 8;
resp |= m_intBuf.get(off + i) & 0xff;
}
} else {
for (int i = m_xferSize - 1; i >= 1; --i) {
resp <<= 8;
resp |= m_intBuf.get(off + i) & 0xff;
}
}
// process response
if ((resp & m_validMask) == m_validValue) {
// valid sensor data; extract data field
int data = resp >> m_dataShift;
data &= m_dataMax - 1;
// 2s complement conversion if signed MSB is set
if (m_isSigned && (data & m_dataMsbMask) != 0) {
data -= m_dataMax;
}
// center offset
int dataNoCenter = data;
data -= m_center;
// only accumulate if outside deadband
if (data < -m_deadband || data > m_deadband) {
m_value += data;
if (m_count != 0) {
// timestamps use the 1us FPGA clock; also handle rollover
if (timestamp >= m_lastTimestamp) {
m_integratedValue +=
dataNoCenter * (timestamp - m_lastTimestamp) * 1e-6 - m_integratedCenter;
} else {
m_integratedValue +=
dataNoCenter * ((1L << 32) - m_lastTimestamp + timestamp) * 1e-6
- m_integratedCenter;
}
}
}
++m_count;
m_lastValue = data;
} else {
// no data from the sensor; just clear the last value
m_lastValue = 0;
}
m_lastTimestamp = timestamp;
}
}
}
}
}
private Accumulator m_accum;
/**
* Initialize the accumulator.
*
* @param period Time between reads
* @param cmd SPI command to send to request data
* @param xferSize SPI transfer size, in bytes
* @param validMask Mask to apply to received data for validity checking
* @param validValue After validMask is applied, required matching value for validity checking
* @param dataShift Bit shift to apply to received data to get actual data value
* @param dataSize Size (in bits) of data field
* @param isSigned Is data field signed?
* @param bigEndian Is device big endian?
*/
public void initAccumulator(
double period,
int cmd,
int xferSize,
int validMask,
int validValue,
int dataShift,
int dataSize,
boolean isSigned,
boolean bigEndian) {
initAuto(xferSize * 2048);
byte[] cmdBytes = new byte[] {0, 0, 0, 0};
if (bigEndian) {
for (int i = xferSize - 1; i >= 0; --i) {
cmdBytes[i] = (byte) (cmd & 0xff);
cmd >>= 8;
}
} else {
cmdBytes[0] = (byte) (cmd & 0xff);
cmd >>= 8;
cmdBytes[1] = (byte) (cmd & 0xff);
cmd >>= 8;
cmdBytes[2] = (byte) (cmd & 0xff);
cmd >>= 8;
cmdBytes[3] = (byte) (cmd & 0xff);
}
setAutoTransmitData(cmdBytes, xferSize - 4);
startAutoRate(period);
m_accum =
new Accumulator(
m_port, xferSize, validMask, validValue, dataShift, dataSize, isSigned, bigEndian);
m_accum.m_notifier.startPeriodic(period * 1024);
}
/** Frees the accumulator. */
public void freeAccumulator() {
if (m_accum != null) {
m_accum.close();
m_accum = null;
}
freeAuto();
}
/** Resets the accumulator to zero. */
public void resetAccumulator() {
if (m_accum == null) {
return;
}
synchronized (m_accum.m_mutex) {
m_accum.m_value = 0;
m_accum.m_count = 0;
m_accum.m_lastValue = 0;
m_accum.m_lastTimestamp = 0;
m_accum.m_integratedValue = 0;
}
}
/**
* Set the center value of the accumulator.
*
* <p>The center value is subtracted from each value before it is added to the accumulator. This
* is used for the center value of devices like gyros and accelerometers to make integration work
* and to take the device offset into account when integrating.
*
* @param center The accumulator's center value.
*/
public void setAccumulatorCenter(int center) {
if (m_accum == null) {
return;
}
synchronized (m_accum.m_mutex) {
m_accum.m_center = center;
}
}
/**
* Set the accumulator's deadband.
*
* @param deadband The accumulator's deadband.
*/
public void setAccumulatorDeadband(int deadband) {
if (m_accum == null) {
return;
}
synchronized (m_accum.m_mutex) {
m_accum.m_deadband = deadband;
}
}
/**
* Read the last value read by the accumulator engine.
*
* @return The last value read by the accumulator engine.
*/
public int getAccumulatorLastValue() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
return m_accum.m_lastValue;
}
}
/**
* Read the accumulated value.
*
* @return The 64-bit value accumulated since the last Reset().
*/
public long getAccumulatorValue() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
return m_accum.m_value;
}
}
/**
* Read the number of accumulated values.
*
* <p>Read the count of the accumulated values since the accumulator was last Reset().
*
* @return The number of times samples from the channel were accumulated.
*/
public int getAccumulatorCount() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
return m_accum.m_count;
}
}
/**
* Read the average of the accumulated value.
*
* @return The accumulated average value (value / count).
*/
public double getAccumulatorAverage() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
if (m_accum.m_count == 0) {
return 0.0;
}
return ((double) m_accum.m_value) / m_accum.m_count;
}
}
/**
* Read the accumulated value and the number of accumulated values atomically.
*
* <p>This function reads the value and count atomically. This can be used for averaging.
*
* @param result AccumulatorResult object to store the results in.
*/
public void getAccumulatorOutput(AccumulatorResult result) {
if (result == null) {
throw new IllegalArgumentException("Null parameter `result'");
}
if (m_accum == null) {
result.value = 0;
result.count = 0;
return;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
result.value = m_accum.m_value;
result.count = m_accum.m_count;
}
}
/**
* Set the center value of the accumulator integrator.
*
* <p>The center value is subtracted from each value*dt before it is added to the integrated
* value. This is used for the center value of devices like gyros and accelerometers to take the
* device offset into account when integrating.
*
* @param center The accumulator integrator's center value.
*/
public void setAccumulatorIntegratedCenter(double center) {
if (m_accum == null) {
return;
}
synchronized (m_accum.m_mutex) {
m_accum.m_integratedCenter = center;
}
}
/**
* Read the integrated value. This is the sum of (each value * time between values).
*
* @return The integrated value accumulated since the last Reset().
*/
public double getAccumulatorIntegratedValue() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
return m_accum.m_integratedValue;
}
}
/**
* Read the average of the integrated value. This is the sum of (each value times the time between
* values), divided by the count.
*
* @return The average of the integrated value accumulated since the last Reset().
*/
public double getAccumulatorIntegratedAverage() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
if (m_accum.m_count <= 1) {
return 0.0;
}
// count-1 due to not integrating the first value received
return m_accum.m_integratedValue / (m_accum.m_count - 1);
}
}
}

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
View File

@@ -351,8 +351,6 @@ public enum BuiltInWidgets implements WidgetType {
*
* <ul>
* <li>{@link edu.wpi.first.wpilibj.ADXL345_I2C}
* <li>{@link edu.wpi.first.wpilibj.ADXL345_SPI}
* <li>{@link edu.wpi.first.wpilibj.ADXL362}
* </ul>
*
* <br>
@@ -374,7 +372,6 @@ public enum BuiltInWidgets implements WidgetType {
* Supported types:
*
* <ul>
* <li>{@link edu.wpi.first.wpilibj.ADXRS450_Gyro}
* <li>{@link edu.wpi.first.wpilibj.AnalogGyro}
* <li>Any custom subclass of {@code GyroBase} (such as a MXP gyro)
* </ul>

121
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java
View File

@@ -1,121 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADIS16448_IMU;
/** Class to control a simulated ADIS16448 gyroscope. */
@SuppressWarnings("TypeName")
public class ADIS16448_IMUSim {
private final SimDouble m_simGyroAngleX;
private final SimDouble m_simGyroAngleY;
private final SimDouble m_simGyroAngleZ;
private final SimDouble m_simGyroRateX;
private final SimDouble m_simGyroRateY;
private final SimDouble m_simGyroRateZ;
private final SimDouble m_simAccelX;
private final SimDouble m_simAccelY;
private final SimDouble m_simAccelZ;
/**
* Constructs from an ADIS16448_IMU object.
*
* @param gyro ADIS16448_IMU to simulate
*/
public ADIS16448_IMUSim(ADIS16448_IMU gyro) {
SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16448" + "[" + gyro.getPort() + "]");
m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
m_simAccelX = wrappedSimDevice.getDouble("accel_x");
m_simAccelY = wrappedSimDevice.getDouble("accel_y");
m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
}
/**
* Sets the X axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleX(double angleDegrees) {
m_simGyroAngleX.set(angleDegrees);
}
/**
* Sets the Y axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleY(double angleDegrees) {
m_simGyroAngleY.set(angleDegrees);
}
/**
* Sets the Z axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleZ(double angleDegrees) {
m_simGyroAngleZ.set(angleDegrees);
}
/**
* Sets the X axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateX(double angularRateDegreesPerSecond) {
m_simGyroRateX.set(angularRateDegreesPerSecond);
}
/**
* Sets the Y axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateY(double angularRateDegreesPerSecond) {
m_simGyroRateY.set(angularRateDegreesPerSecond);
}
/**
* Sets the Z axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateZ(double angularRateDegreesPerSecond) {
m_simGyroRateZ.set(angularRateDegreesPerSecond);
}
/**
* Sets the X axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelX(double accelMetersPerSecondSquared) {
m_simAccelX.set(accelMetersPerSecondSquared);
}
/**
* Sets the Y axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelY(double accelMetersPerSecondSquared) {
m_simAccelY.set(accelMetersPerSecondSquared);
}
/**
* Sets the Z axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelZ(double accelMetersPerSecondSquared) {
m_simAccelZ.set(accelMetersPerSecondSquared);
}
}

121
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java
View File

@@ -1,121 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
/** Class to control a simulated ADIS16470 gyroscope. */
@SuppressWarnings("TypeName")
public class ADIS16470_IMUSim {
private final SimDouble m_simGyroAngleX;
private final SimDouble m_simGyroAngleY;
private final SimDouble m_simGyroAngleZ;
private final SimDouble m_simGyroRateX;
private final SimDouble m_simGyroRateY;
private final SimDouble m_simGyroRateZ;
private final SimDouble m_simAccelX;
private final SimDouble m_simAccelY;
private final SimDouble m_simAccelZ;
/**
* Constructs from an ADIS16470_IMU object.
*
* @param gyro ADIS16470_IMU to simulate
*/
public ADIS16470_IMUSim(ADIS16470_IMU gyro) {
SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16470" + "[" + gyro.getPort() + "]");
m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
m_simAccelX = wrappedSimDevice.getDouble("accel_x");
m_simAccelY = wrappedSimDevice.getDouble("accel_y");
m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
}
/**
* Sets the X axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleX(double angleDegrees) {
m_simGyroAngleX.set(angleDegrees);
}
/**
* Sets the Y axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleY(double angleDegrees) {
m_simGyroAngleY.set(angleDegrees);
}
/**
* Sets the Z axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleZ(double angleDegrees) {
m_simGyroAngleZ.set(angleDegrees);
}
/**
* Sets the X axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateX(double angularRateDegreesPerSecond) {
m_simGyroRateX.set(angularRateDegreesPerSecond);
}
/**
* Sets the Y axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateY(double angularRateDegreesPerSecond) {
m_simGyroRateY.set(angularRateDegreesPerSecond);
}
/**
* Sets the Z axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateZ(double angularRateDegreesPerSecond) {
m_simGyroRateZ.set(angularRateDegreesPerSecond);
}
/**
* Sets the X axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelX(double accelMetersPerSecondSquared) {
m_simAccelX.set(accelMetersPerSecondSquared);
}
/**
* Sets the Y axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelY(double accelMetersPerSecondSquared) {
m_simAccelY.set(accelMetersPerSecondSquared);
}
/**
* Sets the Z axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelZ(double accelMetersPerSecondSquared) {
m_simAccelZ.set(accelMetersPerSecondSquared);
}
}

11
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java
View File

@@ -6,7 +6,6 @@ package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.ADXL345_SPI;
import java.util.Objects;
/** Class to control a simulated ADXL345. */
@@ -15,16 +14,6 @@ public class ADXL345Sim {
private SimDouble m_simY;
private SimDouble m_simZ;
/**
* Constructor.
*
* @param device The device to simulate
*/
public ADXL345Sim(ADXL345_SPI device) {
SimDeviceSim simDevice = new SimDeviceSim("Accel:ADXL345_SPI" + "[" + device.getPort() + "]");
initSim(simDevice);
}
/**
* Constructor.
*

64
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java
View File

@@ -1,64 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADXL362;
import java.util.Objects;
/** Class to control a simulated ADXL362. */
public class ADXL362Sim {
private SimDouble m_simX;
private SimDouble m_simY;
private SimDouble m_simZ;
/**
* Constructor.
*
* @param device The device to simulate
*/
public ADXL362Sim(ADXL362 device) {
SimDeviceSim wrappedSimDevice =
new SimDeviceSim("Accel:ADXL362" + "[" + device.getPort() + "]");
initSim(wrappedSimDevice);
}
private void initSim(SimDeviceSim wrappedSimDevice) {
m_simX = wrappedSimDevice.getDouble("x");
m_simY = wrappedSimDevice.getDouble("y");
m_simZ = wrappedSimDevice.getDouble("z");
Objects.requireNonNull(m_simX);
Objects.requireNonNull(m_simY);
Objects.requireNonNull(m_simZ);
}
/**
* Sets the X acceleration.
*
* @param accel The X acceleration.
*/
public void setX(double accel) {
m_simX.set(accel);
}
/**
* Sets the Y acceleration.
*
* @param accel The Y acceleration.
*/
public void setY(double accel) {
m_simY.set(accel);
}
/**
* Sets the Z acceleration.
*
* @param accel The Z acceleration.
*/
public void setZ(double accel) {
m_simZ.set(accel);
}
}

44
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java
View File

@@ -1,44 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
/** Class to control a simulated ADXRS450 gyroscope. */
@SuppressWarnings("TypeName")
public class ADXRS450_GyroSim {
private final SimDouble m_simAngle;
private final SimDouble m_simRate;
/**
* Constructs from an ADXRS450_Gyro object.
*
* @param gyro ADXRS450_Gyro to simulate
*/
public ADXRS450_GyroSim(ADXRS450_Gyro gyro) {
SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADXRS450" + "[" + gyro.getPort() + "]");
m_simAngle = wrappedSimDevice.getDouble("angle_x");
m_simRate = wrappedSimDevice.getDouble("rate_x");
}
/**
* Sets the angle in degrees (clockwise positive).
*
* @param angleDegrees The angle.
*/
public void setAngle(double angleDegrees) {
m_simAngle.set(angleDegrees);
}
/**
* Sets the angular rate in degrees per second (clockwise positive).
*
* @param rateDegreesPerSecond The angular rate.
*/
public void setRate(double rateDegreesPerSecond) {
m_simRate.set(rateDegreesPerSecond);
}
}

177
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java
View File

@@ -1,177 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.simulation.NotifyCallback;
import edu.wpi.first.hal.simulation.SPIAccelerometerDataJNI;
/** A class to control a simulated accelerometer over SPI. */
public class SPIAccelerometerSim {
private final int m_index;
/**
* Construct a new simulation object.
*
* @param index the HAL index of the accelerometer
*/
public SPIAccelerometerSim(int index) {
m_index = index;
}
/**
* Register a callback to be run when this accelerometer activates.
*
* @param callback the callback
* @param initialNotify whether to run the callback with the initial state
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerActiveCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerActiveCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelActiveCallback);
}
/**
* Check whether the accelerometer is active.
*
* @return true if active
*/
public boolean getActive() {
return SPIAccelerometerDataJNI.getActive(m_index);
}
/**
* Define whether this accelerometer is active.
*
* @param active the new state
*/
public void setActive(boolean active) {
SPIAccelerometerDataJNI.setActive(m_index, active);
}
/**
* Register a callback to be run whenever the range changes.
*
* @param callback the callback
* @param initialNotify whether to call the callback with the initial state
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerRangeCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerRangeCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelRangeCallback);
}
/**
* Check the range of this accelerometer.
*
* @return the accelerometer range
*/
public int getRange() {
return SPIAccelerometerDataJNI.getRange(m_index);
}
/**
* Change the range of this accelerometer.
*
* @param range the new accelerometer range
*/
public void setRange(int range) {
SPIAccelerometerDataJNI.setRange(m_index, range);
}
/**
* Register a callback to be run whenever the X axis value changes.
*
* @param callback the callback
* @param initialNotify whether to call the callback with the initial state
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerXCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerXCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelXCallback);
}
/**
* Measure the X axis value.
*
* @return the X axis measurement
*/
public double getX() {
return SPIAccelerometerDataJNI.getX(m_index);
}
/**
* Change the X axis value of the accelerometer.
*
* @param x the new reading of the X axis
*/
public void setX(double x) {
SPIAccelerometerDataJNI.setX(m_index, x);
}
/**
* Register a callback to be run whenever the Y axis value changes.
*
* @param callback the callback
* @param initialNotify whether to call the callback with the initial state
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerYCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerYCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelYCallback);
}
/**
* Measure the Y axis value.
*
* @return the Y axis measurement
*/
public double getY() {
return SPIAccelerometerDataJNI.getY(m_index);
}
/**
* Change the Y axis value of the accelerometer.
*
* @param y the new reading of the Y axis
*/
public void setY(double y) {
SPIAccelerometerDataJNI.setY(m_index, y);
}
/**
* Register a callback to be run whenever the Z axis value changes.
*
* @param callback the callback
* @param initialNotify whether to call the callback with the initial state
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerZCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerZCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelZCallback);
}
/**
* Measure the Z axis value.
*
* @return the Z axis measurement
*/
public double getZ() {
return SPIAccelerometerDataJNI.getZ(m_index);
}
/**
* Change the Z axis value of the accelerometer.
*
* @param z the new reading of the Z axis
*/
public void setZ(double z) {
SPIAccelerometerDataJNI.setZ(m_index, z);
}
/** Reset all simulation data of this object. */
public void resetData() {
SPIAccelerometerDataJNI.resetData(m_index);
}
}

90
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
View File

@@ -1,90 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.simulation.BufferCallback;
import edu.wpi.first.hal.simulation.ConstBufferCallback;
import edu.wpi.first.hal.simulation.NotifyCallback;
import edu.wpi.first.hal.simulation.SPIDataJNI;
import edu.wpi.first.hal.simulation.SpiReadAutoReceiveBufferCallback;
/** A class for controlling a simulated SPI device. */
public class SPISim {
private final int m_index;
/** Create a new simulated SPI device. */
public SPISim() {
m_index = 0;
}
/**
* Register a callback to be run when this device is initialized.
*
* @param callback the callback
* @param initialNotify whether to run the callback with the initial state
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIDataJNI::cancelInitializedCallback);
}
/**
* Check whether this device has been initialized.
*
* @return true if initialized
*/
public boolean getInitialized() {
return SPIDataJNI.getInitialized(m_index);
}
/**
* Define whether this device has been initialized.
*
* @param initialized whether this object is initialized
*/
public void setInitialized(boolean initialized) {
SPIDataJNI.setInitialized(m_index, initialized);
}
/**
* Register a callback to be run whenever a `read` operation is executed.
*
* @param callback the callback
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerReadCallback(BufferCallback callback) {
int uid = SPIDataJNI.registerReadCallback(m_index, callback);
return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadCallback);
}
/**
* Register a callback to be run whenever a `write` operation is executed.
*
* @param callback the callback
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerWriteCallback(ConstBufferCallback callback) {
int uid = SPIDataJNI.registerWriteCallback(m_index, callback);
return new CallbackStore(m_index, uid, SPIDataJNI::cancelWriteCallback);
}
/**
* Register a callback to be run whenever an auto receive buffer is received.
*
* @param callback the callback
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerReadAutoReceiveBufferCallback(
SpiReadAutoReceiveBufferCallback callback) {
int uid = SPIDataJNI.registerReadAutoReceiveBufferCallback(m_index, callback);
return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadAutoReceiveBufferCallback);
}
/** Reset all simulation data. */
public void resetData() {
SPIDataJNI.resetData(m_index);
}
}