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

Merge branch '2022'

Browse Source
This commit is contained in:
Peter Johnson
2021-05-09 14:15:40 -07:00
parent ba08baabb9 a3cd90dd71
commit 3cc2da3328
765 changed files with 5914 additions and 13714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,6 +9,8 @@ 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.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
@@ -24,7 +26,7 @@ import java.nio.ByteOrder;
* an ADXRS Gyro is supported.
*/
@SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"})
public class ADXRS450_Gyro extends GyroBase {
public class ADXRS450_Gyro implements Gyro, Sendable {
private static final double kSamplePeriod = 0.0005;
private static final double kCalibrationSampleTime = 5.0;
private static final double kDegreePerSecondPerLSB = 0.0125;
@@ -206,4 +208,10 @@ public class ADXRS450_Gyro extends GyroBase {
}
return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}

23
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
View File

@@ -16,12 +16,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* through the sensor. Many sensors have multiple axis and can be treated as multiple devices. Each
* is calibrated by finding the center value over a period of time.
*/
public class AnalogAccelerometer implements PIDSource, Sendable, AutoCloseable {
public class AnalogAccelerometer implements Sendable, AutoCloseable {
private AnalogInput m_analogChannel;
private double m_voltsPerG = 1.0;
private double m_zeroGVoltage = 2.5;
private final boolean m_allocatedChannel;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/** Common initialization. */
private void initAccelerometer() {
@@ -108,26 +107,6 @@ public class AnalogAccelerometer implements PIDSource, Sendable, AutoCloseable {
m_zeroGVoltage = zero;
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the Acceleration for the PID Source parent.
*
* @return The current acceleration in Gs.
*/
@Override
public double pidGet() {
return getAcceleration();
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Accelerometer");

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
View File

@@ -9,6 +9,8 @@ import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.hal.AnalogGyroJNI;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
@@ -20,7 +22,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
*
* <p>This class is for gyro sensors that connect to an analog input.
*/
public class AnalogGyro extends GyroBase {
public class AnalogGyro implements Gyro, Sendable {
private static final double kDefaultVoltsPerDegreePerSecond = 0.007;
protected AnalogInput m_analog;
private boolean m_channelAllocated;
@@ -188,4 +190,10 @@ public class AnalogGyro extends GyroBase {
public AnalogInput getAnalogInput() {
return m_analog;
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}

23
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
View File

@@ -25,13 +25,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* accumulated effectively increasing the resolution, while the averaged samples are divided by the
* number of samples to retain the resolution, but get more stable values.
*/
public class AnalogInput implements PIDSource, Sendable, AutoCloseable {
public class AnalogInput implements Sendable, AutoCloseable {
private static final int kAccumulatorSlot = 1;
int m_port; // explicit no modifier, private and package accessible.
private int m_channel;
private static final int[] kAccumulatorChannels = {0, 1};
private long m_accumulatorOffset;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Construct an analog channel.
@@ -322,26 +321,6 @@ public class AnalogInput implements PIDSource, Sendable, AutoCloseable {
return AnalogJNI.getAnalogSampleRate();
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the average voltage for use with PIDController.
*
* @return the average voltage
*/
@Override
public double pidGet() {
return getAverageVoltage();
}
/**
* Indicates this input is used by a simulated device.
*

28
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpilibj.interfaces.Potentiometer;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
@@ -13,12 +12,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* corresponds to a position. The position is in whichever units you choose, by way of the scaling
* and offset constants passed to the constructor.
*/
public class AnalogPotentiometer implements Potentiometer, Sendable, AutoCloseable {
public class AnalogPotentiometer implements Sendable, AutoCloseable {
private AnalogInput m_analogInput;
private boolean m_initAnalogInput;
private double m_fullRange;
private double m_offset;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* AnalogPotentiometer constructor.
@@ -120,7 +118,6 @@ public class AnalogPotentiometer implements Potentiometer, Sendable, AutoCloseab
*
* @return The current position of the potentiometer.
*/
@Override
public double get() {
if (m_analogInput == null) {
return m_offset;
@@ -129,29 +126,6 @@ public class AnalogPotentiometer implements Potentiometer, Sendable, AutoCloseab
+ m_offset;
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
if (!pidSource.equals(PIDSourceType.kDisplacement)) {
throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
}
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*
* @return The current reading.
*/
@Override
public double pidGet() {
return get();
}
@Override
public void initSendable(SendableBuilder builder) {
if (m_analogInput != null) {

814
wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
View File

@@ -1,814 +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.cscore.AxisCamera;
import edu.wpi.cscore.CameraServerJNI;
import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.MjpegServer;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoEvent;
import edu.wpi.cscore.VideoException;
import edu.wpi.cscore.VideoListener;
import edu.wpi.cscore.VideoMode;
import edu.wpi.cscore.VideoMode.PixelFormat;
import edu.wpi.cscore.VideoProperty;
import edu.wpi.cscore.VideoSink;
import edu.wpi.cscore.VideoSource;
import edu.wpi.first.cameraserver.CameraServerSharedStore;
import edu.wpi.first.networktables.EntryListenerFlags;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Hashtable;
import java.util.Map;
import java.util.concurrent.atomic.AtomicInteger;
/**
* Singleton class for creating and keeping camera servers. Also publishes camera information to
* NetworkTables.
*
* @deprecated Replaced with edu.wpi.first.cameraserver.CameraServer
*/
@Deprecated
public final class CameraServer {
public static final int kBasePort = 1181;
@Deprecated public static final int kSize640x480 = 0;
@Deprecated public static final int kSize320x240 = 1;
@Deprecated public static final int kSize160x120 = 2;
private static final String kPublishName = "/CameraPublisher";
private static CameraServer server;
/** Get the CameraServer instance. */
public static synchronized CameraServer getInstance() {
if (server == null) {
server = new CameraServer();
}
return server;
}
private final AtomicInteger m_defaultUsbDevice;
private String m_primarySourceName;
private final Map<String, VideoSource> m_sources;
private final Map<String, VideoSink> m_sinks;
private final Map<Integer, NetworkTable> m_tables; // indexed by source handle
private final NetworkTable m_publishTable;
private final VideoListener m_videoListener; // NOPMD
private final int m_tableListener; // NOPMD
private int m_nextPort;
private String[] m_addresses;
@SuppressWarnings("MissingJavadocMethod")
private static String makeSourceValue(int source) {
switch (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))) {
case kUsb:
return "usb:" + CameraServerJNI.getUsbCameraPath(source);
case kHttp:
{
String[] urls = CameraServerJNI.getHttpCameraUrls(source);
if (urls.length > 0) {
return "ip:" + urls[0];
} else {
return "ip:";
}
}
case kCv:
// FIXME: Should be "cv:", but LabVIEW dashboard requires "usb:".
// https://github.com/wpilibsuite/allwpilib/issues/407
return "usb:";
default:
return "unknown:";
}
}
@SuppressWarnings("MissingJavadocMethod")
private static String makeStreamValue(String address, int port) {
return "mjpg:http://" + address + ":" + port + "/?action=stream";
}
@SuppressWarnings({"MissingJavadocMethod", "PMD.AvoidUsingHardCodedIP"})
private synchronized String[] getSinkStreamValues(int sink) {
// Ignore all but MjpegServer
if (VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink)) != VideoSink.Kind.kMjpeg) {
return new String[0];
}
// Get port
int port = CameraServerJNI.getMjpegServerPort(sink);
// Generate values
ArrayList<String> values = new ArrayList<>(m_addresses.length + 1);
String listenAddress = CameraServerJNI.getMjpegServerListenAddress(sink);
if (!listenAddress.isEmpty()) {
// If a listen address is specified, only use that
values.add(makeStreamValue(listenAddress, port));
} else {
// Otherwise generate for hostname and all interface addresses
values.add(makeStreamValue(CameraServerJNI.getHostname() + ".local", port));
for (String addr : m_addresses) {
if ("127.0.0.1".equals(addr)) {
continue; // ignore localhost
}
values.add(makeStreamValue(addr, port));
}
}
return values.toArray(new String[0]);
}
@SuppressWarnings({"MissingJavadocMethod", "PMD.AvoidUsingHardCodedIP"})
private synchronized String[] getSourceStreamValues(int source) {
// Ignore all but HttpCamera
if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
!= VideoSource.Kind.kHttp) {
return new String[0];
}
// Generate values
String[] values = CameraServerJNI.getHttpCameraUrls(source);
for (int j = 0; j < values.length; j++) {
values[j] = "mjpg:" + values[j];
}
if (CameraServerSharedStore.getCameraServerShared().isRoboRIO()) {
// Look to see if we have a passthrough server for this source
// Only do this on the roboRIO
for (VideoSink i : m_sinks.values()) {
int sink = i.getHandle();
int sinkSource = CameraServerJNI.getSinkSource(sink);
if (source == sinkSource
&& VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink))
== VideoSink.Kind.kMjpeg) {
// Add USB-only passthrough
String[] finalValues = Arrays.copyOf(values, values.length + 1);
int port = CameraServerJNI.getMjpegServerPort(sink);
finalValues[values.length] = makeStreamValue("172.22.11.2", port);
return finalValues;
}
}
}
return values;
}
@SuppressWarnings({"MissingJavadocMethod", "PMD.AvoidUsingHardCodedIP"})
private synchronized void updateStreamValues() {
// Over all the sinks...
for (VideoSink i : m_sinks.values()) {
int sink = i.getHandle();
// Get the source's subtable (if none exists, we're done)
int source = CameraServerJNI.getSinkSource(sink);
if (source == 0) {
continue;
}
NetworkTable table = m_tables.get(source);
if (table != null) {
// Don't set stream values if this is a HttpCamera passthrough
if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
== VideoSource.Kind.kHttp) {
continue;
}
// Set table value
String[] values = getSinkStreamValues(sink);
if (values.length > 0) {
table.getEntry("streams").setStringArray(values);
}
}
}
// Over all the sources...
for (VideoSource i : m_sources.values()) {
int source = i.getHandle();
// Get the source's subtable (if none exists, we're done)
NetworkTable table = m_tables.get(source);
if (table != null) {
// Set table value
String[] values = getSourceStreamValues(source);
if (values.length > 0) {
table.getEntry("streams").setStringArray(values);
}
}
}
}
@SuppressWarnings("MissingJavadocMethod")
private static String pixelFormatToString(PixelFormat pixelFormat) {
switch (pixelFormat) {
case kMJPEG:
return "MJPEG";
case kYUYV:
return "YUYV";
case kRGB565:
return "RGB565";
case kBGR:
return "BGR";
case kGray:
return "Gray";
default:
return "Unknown";
}
}
/// Provide string description of video mode.
/// The returned string is "{width}x{height} {format} {fps} fps".
@SuppressWarnings("MissingJavadocMethod")
private static String videoModeToString(VideoMode mode) {
return mode.width
+ "x"
+ mode.height
+ " "
+ pixelFormatToString(mode.pixelFormat)
+ " "
+ mode.fps
+ " fps";
}
@SuppressWarnings("MissingJavadocMethod")
private static String[] getSourceModeValues(int sourceHandle) {
VideoMode[] modes = CameraServerJNI.enumerateSourceVideoModes(sourceHandle);
String[] modeStrings = new String[modes.length];
for (int i = 0; i < modes.length; i++) {
modeStrings[i] = videoModeToString(modes[i]);
}
return modeStrings;
}
@SuppressWarnings({"MissingJavadocMethod", "PMD.CyclomaticComplexity"})
private static void putSourcePropertyValue(NetworkTable table, VideoEvent event, boolean isNew) {
String name;
String infoName;
if (event.name.startsWith("raw_")) {
name = "RawProperty/" + event.name;
infoName = "RawPropertyInfo/" + event.name;
} else {
name = "Property/" + event.name;
infoName = "PropertyInfo/" + event.name;
}
NetworkTableEntry entry = table.getEntry(name);
try {
switch (event.propertyKind) {
case kBoolean:
if (isNew) {
entry.setDefaultBoolean(event.value != 0);
} else {
entry.setBoolean(event.value != 0);
}
break;
case kInteger:
case kEnum:
if (isNew) {
entry.setDefaultDouble(event.value);
table
.getEntry(infoName + "/min")
.setDouble(CameraServerJNI.getPropertyMin(event.propertyHandle));
table
.getEntry(infoName + "/max")
.setDouble(CameraServerJNI.getPropertyMax(event.propertyHandle));
table
.getEntry(infoName + "/step")
.setDouble(CameraServerJNI.getPropertyStep(event.propertyHandle));
table
.getEntry(infoName + "/default")
.setDouble(CameraServerJNI.getPropertyDefault(event.propertyHandle));
} else {
entry.setDouble(event.value);
}
break;
case kString:
if (isNew) {
entry.setDefaultString(event.valueStr);
} else {
entry.setString(event.valueStr);
}
break;
default:
break;
}
} catch (VideoException ignored) {
// ignore
}
}
@SuppressWarnings({
"MissingJavadocMethod",
"PMD.UnusedLocalVariable",
"PMD.ExcessiveMethodLength",
"PMD.NPathComplexity"
})
private CameraServer() {
m_defaultUsbDevice = new AtomicInteger();
m_sources = new Hashtable<>();
m_sinks = new Hashtable<>();
m_tables = new Hashtable<>();
m_publishTable = NetworkTableInstance.getDefault().getTable(kPublishName);
m_nextPort = kBasePort;
m_addresses = new String[0];
// We publish sources to NetworkTables using the following structure:
// "/CameraPublisher/{Source.Name}/" - root
// - "source" (string): Descriptive, prefixed with type (e.g. "usb:0")
// - "streams" (string array): URLs that can be used to stream data
// - "description" (string): Description of the source
// - "connected" (boolean): Whether source is connected
// - "mode" (string): Current video mode
// - "modes" (string array): Available video modes
// - "Property/{Property}" - Property values
// - "PropertyInfo/{Property}" - Property supporting information
// Listener for video events
m_videoListener =
new VideoListener(
event -> {
switch (event.kind) {
case kSourceCreated:
{
// Create subtable for the camera
NetworkTable table = m_publishTable.getSubTable(event.name);
m_tables.put(event.sourceHandle, table);
table.getEntry("source").setString(makeSourceValue(event.sourceHandle));
table
.getEntry("description")
.setString(CameraServerJNI.getSourceDescription(event.sourceHandle));
table
.getEntry("connected")
.setBoolean(CameraServerJNI.isSourceConnected(event.sourceHandle));
table
.getEntry("streams")
.setStringArray(getSourceStreamValues(event.sourceHandle));
try {
VideoMode mode = CameraServerJNI.getSourceVideoMode(event.sourceHandle);
table.getEntry("mode").setDefaultString(videoModeToString(mode));
table
.getEntry("modes")
.setStringArray(getSourceModeValues(event.sourceHandle));
} catch (VideoException ignored) {
// Do nothing. Let the other event handlers update this if there is an error.
}
break;
}
case kSourceDestroyed:
{
NetworkTable table = m_tables.get(event.sourceHandle);
if (table != null) {
table.getEntry("source").setString("");
table.getEntry("streams").setStringArray(new String[0]);
table.getEntry("modes").setStringArray(new String[0]);
}
break;
}
case kSourceConnected:
{
NetworkTable table = m_tables.get(event.sourceHandle);
if (table != null) {
// update the description too (as it may have changed)
table
.getEntry("description")
.setString(CameraServerJNI.getSourceDescription(event.sourceHandle));
table.getEntry("connected").setBoolean(true);
}
break;
}
case kSourceDisconnected:
{
NetworkTable table = m_tables.get(event.sourceHandle);
if (table != null) {
table.getEntry("connected").setBoolean(false);
}
break;
}
case kSourceVideoModesUpdated:
{
NetworkTable table = m_tables.get(event.sourceHandle);
if (table != null) {
table
.getEntry("modes")
.setStringArray(getSourceModeValues(event.sourceHandle));
}
break;
}
case kSourceVideoModeChanged:
{
NetworkTable table = m_tables.get(event.sourceHandle);
if (table != null) {
table.getEntry("mode").setString(videoModeToString(event.mode));
}
break;
}
case kSourcePropertyCreated:
{
NetworkTable table = m_tables.get(event.sourceHandle);
if (table != null) {
putSourcePropertyValue(table, event, true);
}
break;
}
case kSourcePropertyValueUpdated:
{
NetworkTable table = m_tables.get(event.sourceHandle);
if (table != null) {
putSourcePropertyValue(table, event, false);
}
break;
}
case kSourcePropertyChoicesUpdated:
{
NetworkTable table = m_tables.get(event.sourceHandle);
if (table != null) {
try {
String[] choices =
CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
table
.getEntry("PropertyInfo/" + event.name + "/choices")
.setStringArray(choices);
} catch (VideoException ignored) {
// ignore
}
}
break;
}
case kSinkSourceChanged:
case kSinkCreated:
case kSinkDestroyed:
case kNetworkInterfacesChanged:
{
m_addresses = CameraServerJNI.getNetworkInterfaces();
updateStreamValues();
break;
}
default:
break;
}
},
0x4fff,
true);
// Listener for NetworkTable events
// We don't currently support changing settings via NT due to
// synchronization issues, so just update to current setting if someone
// else tries to change it.
m_tableListener =
NetworkTableInstance.getDefault()
.addEntryListener(
kPublishName + "/",
event -> {
String relativeKey = event.name.substring(kPublishName.length() + 1);
// get source (sourceName/...)
int subKeyIndex = relativeKey.indexOf('/');
if (subKeyIndex == -1) {
return;
}
String sourceName = relativeKey.substring(0, subKeyIndex);
VideoSource source = m_sources.get(sourceName);
if (source == null) {
return;
}
// get subkey
relativeKey = relativeKey.substring(subKeyIndex + 1);
// handle standard names
String propName;
if ("mode".equals(relativeKey)) {
// reset to current mode
event.getEntry().setString(videoModeToString(source.getVideoMode()));
return;
} else if (relativeKey.startsWith("Property/")) {
propName = relativeKey.substring(9);
} else if (relativeKey.startsWith("RawProperty/")) {
propName = relativeKey.substring(12);
} else {
return; // ignore
}
// everything else is a property
VideoProperty property = source.getProperty(propName);
switch (property.getKind()) {
case kNone:
return;
case kBoolean:
// reset to current setting
event.getEntry().setBoolean(property.get() != 0);
return;
case kInteger:
case kEnum:
// reset to current setting
event.getEntry().setDouble(property.get());
return;
case kString:
// reset to current setting
event.getEntry().setString(property.getString());
return;
default:
return;
}
},
EntryListenerFlags.kImmediate | EntryListenerFlags.kUpdate);
}
/**
* Start automatically capturing images to send to the dashboard.
*
* <p>You should call this method to see a camera feed on the dashboard. If you also want to
* perform vision processing on the roboRIO, use getVideo() to get access to the camera images.
*
* <p>The first time this overload is called, it calls {@link #startAutomaticCapture(int)} with
* device 0, creating a camera named "USB Camera 0". Subsequent calls increment the device number
* (e.g. 1, 2, etc).
*/
public UsbCamera startAutomaticCapture() {
UsbCamera camera = startAutomaticCapture(m_defaultUsbDevice.getAndIncrement());
CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
return camera;
}
/**
* Start automatically capturing images to send to the dashboard.
*
* <p>This overload calls {@link #startAutomaticCapture(String, int)} with a name of "USB Camera
* {dev}".
*
* @param dev The device number of the camera interface
*/
public UsbCamera startAutomaticCapture(int dev) {
UsbCamera camera = new UsbCamera("USB Camera " + dev, dev);
startAutomaticCapture(camera);
CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
return camera;
}
/**
* Start automatically capturing images to send to the dashboard.
*
* @param name The name to give the camera
* @param dev The device number of the camera interface
*/
public UsbCamera startAutomaticCapture(String name, int dev) {
UsbCamera camera = new UsbCamera(name, dev);
startAutomaticCapture(camera);
CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
return camera;
}
/**
* Start automatically capturing images to send to the dashboard.
*
* @param name The name to give the camera
* @param path The device path (e.g. "/dev/video0") of the camera
*/
public UsbCamera startAutomaticCapture(String name, String path) {
UsbCamera camera = new UsbCamera(name, path);
startAutomaticCapture(camera);
CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
return camera;
}
/**
* Start automatically capturing images to send to the dashboard from an existing camera.
*
* @param camera Camera
*/
public void startAutomaticCapture(VideoSource camera) {
addCamera(camera);
VideoSink server = addServer("serve_" + camera.getName());
server.setSource(camera);
}
/**
* Adds an Axis IP camera.
*
* <p>This overload calls {@link #addAxisCamera(String, String)} with name "Axis Camera".
*
* @param host Camera host IP or DNS name (e.g. "10.x.y.11")
*/
public AxisCamera addAxisCamera(String host) {
return addAxisCamera("Axis Camera", host);
}
/**
* Adds an Axis IP camera.
*
* <p>This overload calls {@link #addAxisCamera(String, String[])} with name "Axis Camera".
*
* @param hosts Array of Camera host IPs/DNS names
*/
public AxisCamera addAxisCamera(String[] hosts) {
return addAxisCamera("Axis Camera", hosts);
}
/**
* Adds an Axis IP camera.
*
* @param name The name to give the camera
* @param host Camera host IP or DNS name (e.g. "10.x.y.11")
*/
public AxisCamera addAxisCamera(String name, String host) {
AxisCamera camera = new AxisCamera(name, host);
// Create a passthrough MJPEG server for USB access
startAutomaticCapture(camera);
CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle());
return camera;
}
/**
* Adds an Axis IP camera.
*
* @param name The name to give the camera
* @param hosts Array of Camera host IPs/DNS names
*/
public AxisCamera addAxisCamera(String name, String[] hosts) {
AxisCamera camera = new AxisCamera(name, hosts);
// Create a passthrough MJPEG server for USB access
startAutomaticCapture(camera);
CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle());
return camera;
}
/**
* Get OpenCV access to the primary camera feed. This allows you to get images from the camera for
* image processing on the roboRIO.
*
* <p>This is only valid to call after a camera feed has been added with startAutomaticCapture()
* or addServer().
*/
public CvSink getVideo() {
VideoSource source;
synchronized (this) {
if (m_primarySourceName == null) {
throw new VideoException("no camera available");
}
source = m_sources.get(m_primarySourceName);
}
if (source == null) {
throw new VideoException("no camera available");
}
return getVideo(source);
}
/**
* Get OpenCV access to the specified camera. This allows you to get images from the camera for
* image processing on the roboRIO.
*
* @param camera Camera (e.g. as returned by startAutomaticCapture).
*/
public CvSink getVideo(VideoSource camera) {
String name = "opencv_" + camera.getName();
synchronized (this) {
VideoSink sink = m_sinks.get(name);
if (sink != null) {
VideoSink.Kind kind = sink.getKind();
if (kind != VideoSink.Kind.kCv) {
throw new VideoException("expected OpenCV sink, but got " + kind);
}
return (CvSink) sink;
}
}
CvSink newsink = new CvSink(name);
newsink.setSource(camera);
addServer(newsink);
return newsink;
}
/**
* Get OpenCV access to the specified camera. This allows you to get images from the camera for
* image processing on the roboRIO.
*
* @param name Camera name
*/
public CvSink getVideo(String name) {
VideoSource source;
synchronized (this) {
source = m_sources.get(name);
if (source == null) {
throw new VideoException("could not find camera " + name);
}
}
return getVideo(source);
}
/**
* Create a MJPEG stream with OpenCV input. This can be called to pass custom annotated images to
* the dashboard.
*
* @param name Name to give the stream
* @param width Width of the image being sent
* @param height Height of the image being sent
*/
public CvSource putVideo(String name, int width, int height) {
CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, width, height, 30);
startAutomaticCapture(source);
return source;
}
/**
* Adds a MJPEG server at the next available port.
*
* @param name Server name
*/
public MjpegServer addServer(String name) {
int port;
synchronized (this) {
port = m_nextPort;
m_nextPort++;
}
return addServer(name, port);
}
/**
* Adds a MJPEG server.
*
* @param name Server name
*/
public MjpegServer addServer(String name, int port) {
MjpegServer server = new MjpegServer(name, port);
addServer(server);
return server;
}
/**
* Adds an already created server.
*
* @param server Server
*/
public void addServer(VideoSink server) {
synchronized (this) {
m_sinks.put(server.getName(), server);
}
}
/**
* Removes a server by name.
*
* @param name Server name
*/
public void removeServer(String name) {
synchronized (this) {
m_sinks.remove(name);
}
}
/**
* Get server for the primary camera feed.
*
* <p>This is only valid to call after a camera feed has been added with startAutomaticCapture()
* or addServer().
*/
public VideoSink getServer() {
synchronized (this) {
if (m_primarySourceName == null) {
throw new VideoException("no camera available");
}
return getServer("serve_" + m_primarySourceName);
}
}
/**
* Gets a server by name.
*
* @param name Server name
*/
public VideoSink getServer(String name) {
synchronized (this) {
return m_sinks.get(name);
}
}
/**
* Adds an already created camera.
*
* @param camera Camera
*/
public void addCamera(VideoSource camera) {
String name = camera.getName();
synchronized (this) {
if (m_primarySourceName == null) {
m_primarySourceName = name;
}
m_sources.put(name, camera);
}
}
/**
* Removes a camera by name.
*
* @param name Camera name
*/
public void removeCamera(String name) {
synchronized (this) {
m_sources.remove(name);
}
}
}

36
wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
View File

@@ -26,7 +26,7 @@ import java.nio.ByteOrder;
* <p>All counters will immediately start counting - reset() them if you need them to be zeroed
* before use.
*/
public class Counter implements CounterBase, PIDSource, Sendable, AutoCloseable {
public class Counter implements CounterBase, Sendable, AutoCloseable {
/** Mode determines how and what the counter counts. */
public enum Mode {
/** mode: two pulse. */
@@ -51,7 +51,6 @@ public class Counter implements CounterBase, PIDSource, Sendable, AutoCloseable
private boolean m_allocatedDownSource;
private int m_counter; // /< The FPGA counter object.
private int m_index; // /< The index of this counter.
private PIDSourceType m_pidSource;
private double m_distancePerPulse; // distance of travel for each tick
/** Create an instance of a counter with the given mode. */
@@ -507,39 +506,6 @@ public class Counter implements CounterBase, PIDSource, Sendable, AutoCloseable
m_distancePerPulse = distancePerPulse;
}
/**
* Set which parameter of the encoder you are using as a process control variable. The counter
* class supports the rate and distance parameters.
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
requireNonNullParam(pidSource, "pidSource", "setPIDSourceType");
if (pidSource != PIDSourceType.kDisplacement && pidSource != PIDSourceType.kRate) {
throw new IllegalArgumentException("PID Source parameter was not valid type: " + pidSource);
}
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
@Override
public double pidGet() {
switch (m_pidSource) {
case kDisplacement:
return getDistance();
case kRate:
return getRate();
default:
return 0.0;
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Counter");

38
wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
View File

@@ -27,7 +27,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <p>All encoders will immediately start counting - reset() them if you need them to be zeroed
* before use.
*/
public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable {
public class Encoder implements CounterBase, Sendable, AutoCloseable {
public enum IndexingType {
kResetWhileHigh(0),
kResetWhileLow(1),
@@ -51,7 +51,6 @@ public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable
private boolean m_allocatedA;
private boolean m_allocatedB;
private boolean m_allocatedI;
private PIDSourceType m_pidSource;
private int m_encoder; // the HAL encoder object
@@ -73,8 +72,6 @@ public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable
reverseDirection,
type.value);
m_pidSource = PIDSourceType.kDisplacement;
int fpgaIndex = getFPGAIndex();
HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex + 1, type.value + 1);
SendableRegistry.addLW(this, "Encoder", fpgaIndex);
@@ -481,39 +478,6 @@ public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable
return EncoderJNI.getEncoderSamplesToAverage(m_encoder);
}
/**
* Set which parameter of the encoder you are using as a process control variable. The encoder
* class supports the rate and distance parameters.
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Implement the PIDSource interface.
*
* @return The current value of the selected source parameter.
*/
@Override
public double pidGet() {
switch (m_pidSource) {
case kDisplacement:
return getDistance();
case kRate:
return getRate();
default:
return 0.0;
}
}
/**
* Set the index source for the encoder. When this source is activated, the encoder count
* automatically resets.

97
wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
View File

@@ -1,97 +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.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/**
* Alias for counter class. Implement the gear tooth sensor supplied by FIRST. Currently there is no
* reverse sensing on the gear tooth sensor, but in future versions we might implement the necessary
* timing in the FPGA to sense direction.
*
* @deprecated The only sensor this works with is no longer available and no teams use it according
* to FMS usage reporting.
*/
@Deprecated(since = "2020", forRemoval = true)
public class GearTooth extends Counter {
private static final double kGearToothThreshold = 55e-6;
/** Common code called by the constructors. */
public void enableDirectionSensing(boolean directionSensitive) {
if (directionSensitive) {
setPulseLengthMode(kGearToothThreshold);
}
}
/**
* Construct a GearTooth sensor given a channel.
*
* <p>No direction sensing is assumed.
*
* @param channel The GPIO channel that the sensor is connected to.
*/
public GearTooth(final int channel) {
this(channel, false);
}
/**
* Construct a GearTooth sensor given a channel.
*
* @param channel The DIO channel that the sensor is connected to. 0-9 are on-board, 10-25 are on
* the MXP port
* @param directionSensitive True to enable the pulse length decoding in hardware to specify count
* direction.
*/
public GearTooth(final int channel, boolean directionSensitive) {
super(channel);
enableDirectionSensing(directionSensitive);
if (directionSensitive) {
HAL.report(tResourceType.kResourceType_GearTooth, channel + 1, 0, "D");
} else {
HAL.report(tResourceType.kResourceType_GearTooth, channel + 1, 0);
}
SendableRegistry.setName(this, "GearTooth", channel);
}
/**
* Construct a GearTooth sensor given a digital input. This should be used when sharing digital
* inputs.
*
* @param source An existing DigitalSource object (such as a DigitalInput)
* @param directionSensitive True to enable the pulse length decoding in hardware to specify count
* direction.
*/
public GearTooth(DigitalSource source, boolean directionSensitive) {
super(source);
enableDirectionSensing(directionSensitive);
if (directionSensitive) {
HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel() + 1, 0, "D");
} else {
HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel() + 1, 0);
}
SendableRegistry.setName(this, "GearTooth", source.getChannel());
}
/**
* Construct a GearTooth sensor given a digital input. This should be used when sharing digital
* inputs.
*
* <p>No direction sensing is assumed.
*
* @param source An object that fully describes the input that the sensor is connected to.
*/
public GearTooth(DigitalSource source) {
this(source, false);
}
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
builder.setSmartDashboardType("Gear Tooth");
}
}

53
wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java
View File

@@ -1,53 +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.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
/** GyroBase is the common base class for Gyro implementations such as AnalogGyro. */
public abstract class GyroBase implements Gyro, PIDSource, Sendable {
private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Set which parameter of the gyro you are using as a process control variable. The Gyro class
* supports the rate and displacement parameters
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the output of the gyro for use with PIDControllers. May be the angle or rate depending on
* the set PIDSourceType
*
* @return the output according to the gyro
*/
@Override
public double pidGet() {
switch (m_pidSource) {
case kRate:
return getRate();
case kDisplacement:
return getAngle();
default:
return 0.0;
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}

64
wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.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;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
/**
* IterativeRobot implements the IterativeRobotBase robot program framework.
*
* <p>The IterativeRobot class is intended to be subclassed by a user creating a robot program.
*
* <p>periodic() functions from the base class are called each time a new packet is received from
* the driver station.
*
* @deprecated Use TimedRobot instead. It's a drop-in replacement that provides more regular
* execution periods.
*/
@Deprecated
public class IterativeRobot extends IterativeRobotBase {
private static final double kPacketPeriod = 0.02;
private volatile boolean m_exit;
/** Create a new IterativeRobot. */
public IterativeRobot() {
super(kPacketPeriod);
HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative);
}
/** Provide an alternate "main loop" via startCompetition(). */
@Override
public void startCompetition() {
robotInit();
if (isSimulation()) {
simulationInit();
}
// Tell the DS that the robot is ready to be enabled
HAL.observeUserProgramStarting();
// Loop forever, calling the appropriate mode-dependent function
while (!Thread.currentThread().isInterrupted()) {
// Wait for new data to arrive
m_ds.waitForData();
if (m_exit) {
break;
}
loopFunc();
}
}
/** Ends the main loop in startCompetition(). */
@Override
public void endCompetition() {
m_exit = true;
m_ds.wakeupWaitForData();
}
}

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
View File

@@ -32,8 +32,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
* disabledPeriodic() - autonomousPeriodic() - teleopPeriodic() - testPeriodic()
*/
public abstract class IterativeRobotBase extends RobotBase {
protected double m_period;
private enum Mode {
kNone,
kDisabled,
@@ -43,6 +41,7 @@ public abstract class IterativeRobotBase extends RobotBase {
}
private Mode m_lastMode = Mode.kNone;
private final double m_period;
private final Watchdog m_watchdog;
private boolean m_ntFlushEnabled;
@@ -204,6 +203,11 @@ public abstract class IterativeRobotBase extends RobotBase {
m_ntFlushEnabled = enabled;
}
/** Gets time period between calls to Periodic() functions. */
public double getPeriod() {
return m_period;
}
@SuppressWarnings("PMD.CyclomaticComplexity")
protected void loopFunc() {
m_watchdog.reset();

819
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
View File

@@ -1,819 +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 static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.util.BoundaryException;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import java.util.concurrent.locks.ReentrantLock;
/**
* Class implements a PID Control Loop.
*
* <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
* calculations, as well as writing the given PIDOutput.
*
* <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
* and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
* given set of PID constants.
*
* @deprecated All APIs which use this have been deprecated.
*/
@Deprecated(since = "2020", forRemoval = true)
@SuppressWarnings("PMD.TooManyFields")
public class PIDBase implements PIDInterface, PIDOutput, Sendable, AutoCloseable {
public static final double kDefaultPeriod = 0.05;
private static int instances;
// Factor for "proportional" control
@SuppressWarnings("MemberName")
private double m_P;
// Factor for "integral" control
@SuppressWarnings("MemberName")
private double m_I;
// Factor for "derivative" control
@SuppressWarnings("MemberName")
private double m_D;
// Factor for "feed forward" control
@SuppressWarnings("MemberName")
private double m_F;
// |maximum output|
private double m_maximumOutput = 1.0;
// |minimum output|
private double m_minimumOutput = -1.0;
// Maximum input - limit setpoint to this
private double m_maximumInput;
// Minimum input - limit setpoint to this
private double m_minimumInput;
// Input range - difference between maximum and minimum
private double m_inputRange;
// Do the endpoints wrap around? (e.g., absolute encoder)
private boolean m_continuous;
// Is the PID controller enabled
protected boolean m_enabled;
// The prior error (used to compute velocity)
private double m_prevError;
// The sum of the errors for use in the integral calc
private double m_totalError;
// The tolerance object used to check if on target
private Tolerance m_tolerance;
private double m_setpoint;
private double m_prevSetpoint;
@SuppressWarnings("PMD.UnusedPrivateField")
private double m_error;
private double m_result;
private LinearFilter m_filter;
protected ReentrantLock m_thisMutex = new ReentrantLock();
// Ensures when disable() is called, pidWrite() won't run if calculate()
// is already running at that time.
protected ReentrantLock m_pidWriteMutex = new ReentrantLock();
protected PIDSource m_pidInput;
protected PIDOutput m_pidOutput;
protected Timer m_setpointTimer;
/**
* Tolerance is the type of tolerance used to specify if the PID controller is on target.
*
* <p>The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
* specify types of tolerance specifications to use.
*/
public interface Tolerance {
boolean onTarget();
}
/** Used internally for when Tolerance hasn't been set. */
public static class NullTolerance implements Tolerance {
@Override
public boolean onTarget() {
throw new IllegalStateException("No tolerance value set when calling onTarget().");
}
}
public class PercentageTolerance implements Tolerance {
private final double m_percentage;
PercentageTolerance(double value) {
m_percentage = value;
}
@Override
public boolean onTarget() {
return Math.abs(getError()) < m_percentage / 100 * m_inputRange;
}
}
public class AbsoluteTolerance implements Tolerance {
private final double m_value;
AbsoluteTolerance(double value) {
m_value = value;
}
@Override
public boolean onTarget() {
return Math.abs(getError()) < m_value;
}
}
/**
* Allocate a PID object with the given constants for P, I, D, and F.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param Kf the feed forward term
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
requireNonNullParam(source, "PIDSource", "PIDBase");
requireNonNullParam(output, "output", "PIDBase");
m_setpointTimer = new Timer();
m_setpointTimer.start();
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_pidInput = source;
m_filter = LinearFilter.movingAverage(1);
m_pidOutput = output;
instances++;
HAL.report(tResourceType.kResourceType_PIDController, instances);
m_tolerance = new NullTolerance();
SendableRegistry.add(this, "PIDController", instances);
}
/**
* Allocate a PID object with the given constants for P, I, and D.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source the PIDSource object that is used to get values
* @param output the PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDBase(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
this(Kp, Ki, Kd, 0.0, source, output);
}
@Override
public void close() {
SendableRegistry.remove(this);
}
/**
* Read the input, calculate the output accordingly, and write to the output. This should only be
* called by the PIDTask and is created during initialization.
*/
@SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", "PMD.NPathComplexity"})
protected void calculate() {
if (m_pidInput == null || m_pidOutput == null) {
return;
}
boolean enabled;
m_thisMutex.lock();
try {
enabled = m_enabled;
} finally {
m_thisMutex.unlock();
}
if (enabled) {
double input;
// Storage for function inputs
PIDSourceType pidSourceType;
double P;
double I;
double D;
double feedForward = calculateFeedForward();
double minimumOutput;
double maximumOutput;
// Storage for function input-outputs
double prevError;
double error;
double totalError;
m_thisMutex.lock();
try {
input = m_filter.calculate(m_pidInput.pidGet());
pidSourceType = m_pidInput.getPIDSourceType();
P = m_P;
I = m_I;
D = m_D;
minimumOutput = m_minimumOutput;
maximumOutput = m_maximumOutput;
prevError = m_prevError;
error = getContinuousError(m_setpoint - input);
totalError = m_totalError;
} finally {
m_thisMutex.unlock();
}
// Storage for function outputs
double result;
if (pidSourceType.equals(PIDSourceType.kRate)) {
if (P != 0) {
totalError = clamp(totalError + error, minimumOutput / P, maximumOutput / P);
}
result = P * totalError + D * error + feedForward;
} else {
if (I != 0) {
totalError = clamp(totalError + error, minimumOutput / I, maximumOutput / I);
}
result = P * error + I * totalError + D * (error - prevError) + feedForward;
}
result = clamp(result, minimumOutput, maximumOutput);
// Ensures m_enabled check and pidWrite() call occur atomically
m_pidWriteMutex.lock();
try {
m_thisMutex.lock();
try {
if (m_enabled) {
// Don't block other PIDController operations on pidWrite()
m_thisMutex.unlock();
m_pidOutput.pidWrite(result);
}
} finally {
if (m_thisMutex.isHeldByCurrentThread()) {
m_thisMutex.unlock();
}
}
} finally {
m_pidWriteMutex.unlock();
}
m_thisMutex.lock();
try {
m_prevError = error;
m_error = error;
m_totalError = totalError;
m_result = result;
} finally {
m_thisMutex.unlock();
}
}
}
/**
* Calculate the feed forward term.
*
* <p>Both of the provided feed forward calculations are velocity feed forwards. If a different
* feed forward calculation is desired, the user can override this function and provide his or her
* own. This function does no synchronization because the PIDController class only calls it in
* synchronized code, so be careful if calling it oneself.
*
* <p>If a velocity PID controller is being used, the F term should be set to 1 over the maximum
* setpoint for the output. If a position PID controller is being used, the F term should be set
* to 1 over the maximum speed for the output measured in setpoint units per this controller's
* update period (see the default period in this class's constructor).
*/
protected double calculateFeedForward() {
if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
return m_F * getSetpoint();
} else {
double temp = m_F * getDeltaSetpoint();
m_prevSetpoint = m_setpoint;
m_setpointTimer.reset();
return temp;
}
}
/**
* Set the PID Controller gain parameters. Set the proportional, integral, and differential
* coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
*/
@Override
@SuppressWarnings("ParameterName")
public void setPID(double p, double i, double d) {
m_thisMutex.lock();
try {
m_P = p;
m_I = i;
m_D = d;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the PID Controller gain parameters. Set the proportional, integral, and differential
* coefficients.
*
* @param p Proportional coefficient
* @param i Integral coefficient
* @param d Differential coefficient
* @param f Feed forward coefficient
*/
@SuppressWarnings("ParameterName")
public void setPID(double p, double i, double d, double f) {
m_thisMutex.lock();
try {
m_P = p;
m_I = i;
m_D = d;
m_F = f;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Proportional coefficient of the PID controller gain.
*
* @param p Proportional coefficient
*/
@SuppressWarnings("ParameterName")
public void setP(double p) {
m_thisMutex.lock();
try {
m_P = p;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Integral coefficient of the PID controller gain.
*
* @param i Integral coefficient
*/
@SuppressWarnings("ParameterName")
public void setI(double i) {
m_thisMutex.lock();
try {
m_I = i;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Differential coefficient of the PID controller gain.
*
* @param d differential coefficient
*/
@SuppressWarnings("ParameterName")
public void setD(double d) {
m_thisMutex.lock();
try {
m_D = d;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the Feed forward coefficient of the PID controller gain.
*
* @param f feed forward coefficient
*/
@SuppressWarnings("ParameterName")
public void setF(double f) {
m_thisMutex.lock();
try {
m_F = f;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Proportional coefficient.
*
* @return proportional coefficient
*/
@Override
public double getP() {
m_thisMutex.lock();
try {
return m_P;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Integral coefficient.
*
* @return integral coefficient
*/
@Override
public double getI() {
m_thisMutex.lock();
try {
return m_I;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Differential coefficient.
*
* @return differential coefficient
*/
@Override
public double getD() {
m_thisMutex.lock();
try {
return m_D;
} finally {
m_thisMutex.unlock();
}
}
/**
* Get the Feed forward coefficient.
*
* @return feed forward coefficient
*/
public double getF() {
m_thisMutex.lock();
try {
return m_F;
} finally {
m_thisMutex.unlock();
}
}
/**
* Return the current PID result This is always centered on zero and constrained the the max and
* min outs.
*
* @return the latest calculated output
*/
public double get() {
m_thisMutex.lock();
try {
return m_result;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the PID controller to consider the input to be continuous, Rather then using the max and
* min input range as constraints, it considers them to be the same point and automatically
* calculates the shortest route to the setpoint.
*
* @param continuous Set to true turns on continuous, false turns off continuous
*/
public void setContinuous(boolean continuous) {
if (continuous && m_inputRange <= 0) {
throw new IllegalStateException("No input range set when calling setContinuous().");
}
m_thisMutex.lock();
try {
m_continuous = continuous;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the PID controller to consider the input to be continuous, Rather then using the max and
* min input range as constraints, it considers them to be the same point and automatically
* calculates the shortest route to the setpoint.
*/
public void setContinuous() {
setContinuous(true);
}
/**
* Sets the maximum and minimum values expected from the input and setpoint.
*
* @param minimumInput the minimum value expected from the input
* @param maximumInput the maximum value expected from the input
*/
public void setInputRange(double minimumInput, double maximumInput) {
m_thisMutex.lock();
try {
if (minimumInput > maximumInput) {
throw new BoundaryException("Lower bound is greater than upper bound");
}
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
m_inputRange = maximumInput - minimumInput;
} finally {
m_thisMutex.unlock();
}
setSetpoint(m_setpoint);
}
/**
* Sets the minimum and maximum values to write.
*
* @param minimumOutput the minimum percentage to write to the output
* @param maximumOutput the maximum percentage to write to the output
*/
public void setOutputRange(double minimumOutput, double maximumOutput) {
m_thisMutex.lock();
try {
if (minimumOutput > maximumOutput) {
throw new BoundaryException("Lower bound is greater than upper bound");
}
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the setpoint for the PIDController.
*
* @param setpoint the desired setpoint
*/
@Override
public void setSetpoint(double setpoint) {
m_thisMutex.lock();
try {
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput) {
m_setpoint = m_maximumInput;
} else if (setpoint < m_minimumInput) {
m_setpoint = m_minimumInput;
} else {
m_setpoint = setpoint;
}
} else {
m_setpoint = setpoint;
}
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the current setpoint of the PIDController.
*
* @return the current setpoint
*/
@Override
public double getSetpoint() {
m_thisMutex.lock();
try {
return m_setpoint;
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the change in setpoint over time of the PIDController.
*
* @return the change in setpoint over time
*/
public double getDeltaSetpoint() {
m_thisMutex.lock();
try {
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the current difference of the input from the setpoint.
*
* @return the current error
*/
@Override
public double getError() {
m_thisMutex.lock();
try {
return getContinuousError(getSetpoint() - m_filter.calculate(m_pidInput.pidGet()));
} finally {
m_thisMutex.unlock();
}
}
/**
* Returns the current difference of the error over the past few iterations. You can specify the
* number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
* used for the onTarget() function.
*
* @deprecated Use getError(), which is now already filtered.
* @return the current average of the error
*/
@Deprecated
public double getAvgError() {
m_thisMutex.lock();
try {
return getError();
} finally {
m_thisMutex.unlock();
}
}
/**
* Sets what type of input the PID controller will use.
*
* @param pidSource the type of input
*/
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidInput.setPIDSourceType(pidSource);
}
/**
* Returns the type of input the PID controller is using.
*
* @return the PID controller input type
*/
public PIDSourceType getPIDSourceType() {
return m_pidInput.getPIDSourceType();
}
/**
* Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of
* the range or as an absolute value. The Tolerance object encapsulates those options in an
* object. Use it by creating the type of tolerance that you want to use: setTolerance(new
* PIDController.AbsoluteTolerance(0.1))
*
* @deprecated Use setPercentTolerance() instead.
* @param tolerance A tolerance object of the right type, e.g. PercentTolerance or
* AbsoluteTolerance
*/
@Deprecated
public void setTolerance(Tolerance tolerance) {
m_tolerance = tolerance;
}
/**
* Set the absolute error which is considered tolerable for use with OnTarget.
*
* @param absvalue absolute error which is tolerable in the units of the input object
*/
public void setAbsoluteTolerance(double absvalue) {
m_thisMutex.lock();
try {
m_tolerance = new AbsoluteTolerance(absvalue);
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 =
* 15 percent)
*
* @param percentage percent error which is tolerable
*/
public void setPercentTolerance(double percentage) {
m_thisMutex.lock();
try {
m_tolerance = new PercentageTolerance(percentage);
} finally {
m_thisMutex.unlock();
}
}
/**
* Set the number of previous error samples to average for tolerancing. When determining whether a
* mechanism is on target, the user may want to use a rolling average of previous measurements
* instead of a precise position or velocity. This is useful for noisy sensors which return a few
* erroneous measurements when the mechanism is on target. However, the mechanism will not
* register as on target for at least the specified bufLength cycles.
*
* @deprecated Use a LinearFilter as the input.
* @param bufLength Number of previous cycles to average.
*/
@Deprecated
public void setToleranceBuffer(int bufLength) {
m_thisMutex.lock();
try {
m_filter = LinearFilter.movingAverage(bufLength);
} finally {
m_thisMutex.unlock();
}
}
/**
* Return true if the error is within the percentage of the total input range, determined by
* setTolerance. This assumes that the maximum and minimum input were set using setInput.
*
* @return true if the error is less than the tolerance
*/
public boolean onTarget() {
m_thisMutex.lock();
try {
return m_tolerance.onTarget();
} finally {
m_thisMutex.unlock();
}
}
/** Reset the previous error, the integral term, and disable the controller. */
@Override
public void reset() {
m_thisMutex.lock();
try {
m_prevError = 0;
m_totalError = 0;
m_result = 0;
} finally {
m_thisMutex.unlock();
}
}
/**
* Passes the output directly to setSetpoint().
*
* <p>PIDControllers can be nested by passing a PIDController as another PIDController's output.
* In that case, the output of the parent controller becomes the input (i.e., the reference) of
* the child.
*
* <p>It is the caller's responsibility to put the data into a valid form for setSetpoint().
*/
@Override
public void pidWrite(double output) {
setSetpoint(output);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("PIDController");
builder.setSafeState(this::reset);
builder.addDoubleProperty("p", this::getP, this::setP);
builder.addDoubleProperty("i", this::getI, this::setI);
builder.addDoubleProperty("d", this::getD, this::setD);
builder.addDoubleProperty("f", this::getF, this::setF);
builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
}
/**
* Wraps error around for continuous inputs. The original error is returned if continuous mode is
* disabled. This is an unsynchronized function.
*
* @param error The current error of the PID controller.
* @return Error for continuous inputs.
*/
protected double getContinuousError(double error) {
if (m_continuous && m_inputRange > 0) {
error %= m_inputRange;
if (Math.abs(error) > m_inputRange / 2) {
if (error > 0) {
return error - m_inputRange;
} else {
return error + m_inputRange;
}
}
}
return error;
}
private static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
}

172
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
View File

@@ -1,172 +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.wpilibj.smartdashboard.SendableBuilder;
/**
* Class implements a PID Control Loop.
*
* <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
* calculations, as well as writing the given PIDOutput.
*
* <p>This feedback controller runs in discrete time, so time deltas are not used in the integral
* and derivative calculations. Therefore, the sample rate affects the controller's behavior for a
* given set of PID constants.
*
* @deprecated Use {@link edu.wpi.first.wpilibj.controller.PIDController} instead.
*/
@Deprecated(since = "2020", forRemoval = true)
public class PIDController extends PIDBase implements Controller {
Notifier m_controlLoop = new Notifier(this::calculate);
/**
* Allocate a PID object with the given constants for P, I, D, and F.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param Kf the feed forward term
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
* @param period the loop time for doing calculations in seconds. This particularly affects
* calculations of the integral and differential terms. The default is 0.05 (50ms).
*/
@SuppressWarnings("ParameterName")
public PIDController(
double Kp,
double Ki,
double Kd,
double Kf,
PIDSource source,
PIDOutput output,
double period) {
super(Kp, Ki, Kd, Kf, source, output);
m_controlLoop.startPeriodic(period);
}
/**
* Allocate a PID object with the given constants for P, I, D and period.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source the PIDSource object that is used to get values
* @param output the PIDOutput object that is set to the output percentage
* @param period the loop time for doing calculations in seconds. This particularly affects
* calculations of the integral and differential terms. The default is 0.05 (50ms).
*/
@SuppressWarnings("ParameterName")
public PIDController(
double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
this(Kp, Ki, Kd, 0.0, source, output, period);
}
/**
* Allocate a PID object with the given constants for P, I, D, using a 50ms period.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
this(Kp, Ki, Kd, source, output, kDefaultPeriod);
}
/**
* Allocate a PID object with the given constants for P, I, D, using a 50ms period.
*
* @param Kp the proportional coefficient
* @param Ki the integral coefficient
* @param Kd the derivative coefficient
* @param Kf the feed forward term
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output percentage
*/
@SuppressWarnings("ParameterName")
public PIDController(
double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
}
@Override
public void close() {
m_controlLoop.close();
m_thisMutex.lock();
try {
m_pidOutput = null;
m_pidInput = null;
m_controlLoop = null;
} finally {
m_thisMutex.unlock();
}
}
/** Begin running the PIDController. */
@Override
public void enable() {
m_thisMutex.lock();
try {
m_enabled = true;
} finally {
m_thisMutex.unlock();
}
}
/** Stop running the PIDController, this sets the output to zero before stopping. */
@Override
public void disable() {
// Ensures m_enabled check and pidWrite() call occur atomically
m_pidWriteMutex.lock();
try {
m_thisMutex.lock();
try {
m_enabled = false;
} finally {
m_thisMutex.unlock();
}
m_pidOutput.pidWrite(0);
} finally {
m_pidWriteMutex.unlock();
}
}
/** Set the enabled state of the PIDController. */
public void setEnabled(boolean enable) {
if (enable) {
enable();
} else {
disable();
}
}
/** Return true if PIDController is enabled. */
public boolean isEnabled() {
m_thisMutex.lock();
try {
return m_enabled;
} finally {
m_thisMutex.unlock();
}
}
/** Reset the previous error, the integral term, and disable the controller. */
@Override
public void reset() {
disable();
super.reset();
}
@Override
public void initSendable(SendableBuilder builder) {
super.initSendable(builder);
builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled);
}
}

25
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
View File

@@ -1,25 +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;
@Deprecated(since = "2020", forRemoval = true)
@SuppressWarnings("SummaryJavadoc")
public interface PIDInterface {
void setPID(double p, double i, double d);
double getP();
double getI();
double getD();
void setSetpoint(double setpoint);
double getSetpoint();
double getError();
void reset();
}

21
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
View File

@@ -1,21 +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;
/**
* This interface allows PIDController to write it's results to its output.
*
* @deprecated Use DoubleConsumer and new PIDController class.
*/
@FunctionalInterface
@Deprecated(since = "2020", forRemoval = true)
public interface PIDOutput {
/**
* Set the output to the value calculated by PIDController.
*
* @param output the value calculated by PIDController
*/
void pidWrite(double output);
}

34
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
View File

@@ -1,34 +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;
/**
* This interface allows for PIDController to automatically read from this object.
*
* @deprecated Use DoubleSupplier and new PIDController class.
*/
@Deprecated(since = "2020", forRemoval = true)
public interface PIDSource {
/**
* Set which parameter of the device you are using as a process control variable.
*
* @param pidSource An enum to select the parameter.
*/
void setPIDSourceType(PIDSourceType pidSource);
/**
* Get which parameter of the device you are using as a process control variable.
*
* @return the currently selected PID source parameter
*/
PIDSourceType getPIDSourceType();
/**
* Get the result to use in PIDController.
*
* @return the result to use in PIDController
*/
double pidGet();
}

12
wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
View File

@@ -1,12 +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;
/** A description for the type of output value to provide to a PIDController. */
@Deprecated(since = "2020", forRemoval = true)
public enum PIDSourceType {
kDisplacement,
kRate
}

35
wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
View File

@@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* center value - 999 to 2 = linear scaling from "center" to "full reverse" - 1 = minimum pulse
* width (currently .5ms) - 0 = disabled (i.e. PWM output is held low)
*/
public class PWM extends MotorSafety implements Sendable, AutoCloseable {
public class PWM implements Sendable, AutoCloseable {
/** Represents the amount to multiply the minimum servo-pulse pwm period by. */
public enum PeriodMultiplier {
/** Period Multiplier: don't skip pulses. PWM pulses occur every 5.005 ms */
@@ -42,9 +42,24 @@ public class PWM extends MotorSafety implements Sendable, AutoCloseable {
/**
* Allocate a PWM given a channel.
*
* <p>Checks channel value range and allocates the appropriate channel. The allocation is only
* done to help users ensure that they don't double assign channels.
*
* <p>By default, adds itself to SendableRegistry and LiveWindow.
*
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
*/
public PWM(final int channel) {
this(channel, true);
}
/**
* Allocate a PWM given a channel.
*
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
* @param registerSendable If true, adds this instance to SendableRegistry and LiveWindow
*/
public PWM(final int channel, final boolean registerSendable) {
SensorUtil.checkPWMChannel(channel);
m_channel = channel;
@@ -55,9 +70,9 @@ public class PWM extends MotorSafety implements Sendable, AutoCloseable {
PWMJNI.setPWMEliminateDeadband(m_handle, false);
HAL.report(tResourceType.kResourceType_PWM, channel + 1);
SendableRegistry.addLW(this, "PWM", channel);
setSafetyEnabled(false);
if (registerSendable) {
SendableRegistry.addLW(this, "PWM", channel);
}
}
/** Free the resource associated with the PWM channel and set the value to 0. */
@@ -72,16 +87,6 @@ public class PWM extends MotorSafety implements Sendable, AutoCloseable {
m_handle = 0;
}
@Override
public void stopMotor() {
setDisabled();
}
@Override
public String getDescription() {
return "PWM " + getChannel();
}
/**
* Optionally eliminate the deadband from a speed controller.
*
@@ -234,7 +239,7 @@ public class PWM extends MotorSafety implements Sendable, AutoCloseable {
}
}
protected void setZeroLatch() {
public void setZeroLatch() {
PWMJNI.latchPWMZero(m_handle);
}

86
wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
View File

@@ -75,14 +75,27 @@ public final class Preferences {
* @param value the value
* @throws NullPointerException if value is null
*/
public void putString(String key, String value) {
requireNonNullParam(value, "value", "putString");
public void setString(String key, String value) {
requireNonNullParam(value, "value", "setString");
NetworkTableEntry entry = m_table.getEntry(key);
entry.setString(value);
entry.setPersistent();
}
/**
* Puts the given string into the preferences table.
*
* @param key the key
* @param value the value
* @throws NullPointerException if value is null
* @deprecated Use {@link #setString(String, String)}
*/
@Deprecated
public void putString(String key, String value) {
setString(key, value);
}
/**
* Puts the given string into the preferences table if it doesn't already exist.
*
@@ -100,12 +113,24 @@ public final class Preferences {
* @param key the key
* @param value the value
*/
public void putInt(String key, int value) {
public void setInt(String key, int value) {
NetworkTableEntry entry = m_table.getEntry(key);
entry.setDouble(value);
entry.setPersistent();
}
/**
* Puts the given int into the preferences table.
*
* @param key the key
* @param value the value
* @deprecated Use {@link #setInt(String, int)}
*/
@Deprecated
public void putInt(String key, int value) {
setInt(key, value);
}
/**
* Puts the given int into the preferences table if it doesn't already exist.
*
@@ -123,12 +148,23 @@ public final class Preferences {
* @param key the key
* @param value the value
*/
public void putDouble(String key, double value) {
public void setDouble(String key, double value) {
NetworkTableEntry entry = m_table.getEntry(key);
entry.setDouble(value);
entry.setPersistent();
}
/**
* Puts the given double into the preferences table.
*
* @param key the key
* @param value the value
* @deprecated Use {@link #setDouble(String, double)}
*/
public void putDouble(String key, double value) {
setDouble(key, value);
}
/**
* Puts the given double into the preferences table if it doesn't already exist.
*
@@ -146,12 +182,24 @@ public final class Preferences {
* @param key the key
* @param value the value
*/
public void putFloat(String key, float value) {
public void setFloat(String key, float value) {
NetworkTableEntry entry = m_table.getEntry(key);
entry.setDouble(value);
entry.setPersistent();
}
/**
* Puts the given float into the preferences table.
*
* @param key the key
* @param value the value
* @deprecated Use {@link #setFloat(String, float)}
*/
@Deprecated
public void putFloat(String key, float value) {
setFloat(key, value);
}
/**
* Puts the given float into the preferences table if it doesn't already exist.
*
@@ -169,12 +217,24 @@ public final class Preferences {
* @param key the key
* @param value the value
*/
public void putBoolean(String key, boolean value) {
public void setBoolean(String key, boolean value) {
NetworkTableEntry entry = m_table.getEntry(key);
entry.setBoolean(value);
entry.setPersistent();
}
/**
* Puts the given boolean into the preferences table.
*
* @param key the key
* @param value the value
* @deprecated Use {@link #setBoolean(String, boolean)}
*/
@Deprecated
public void putBoolean(String key, boolean value) {
setBoolean(key, value);
}
/**
* Puts the given boolean into the preferences table if it doesn't already exist.
*
@@ -192,12 +252,24 @@ public final class Preferences {
* @param key the key
* @param value the value
*/
public void putLong(String key, long value) {
public void setLong(String key, long value) {
NetworkTableEntry entry = m_table.getEntry(key);
entry.setDouble(value);
entry.setPersistent();
}
/**
* Puts the given long into the preferences table.
*
* @param key the key
* @param value the value
* @deprecated Use {@link #setLong(String, long)}
*/
@Deprecated
public void putLong(String key, long value) {
setLong(key, value);
}
/**
* Puts the given long into the preferences table if it doesn't already exist.
*

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
View File

@@ -4,9 +4,9 @@
package edu.wpi.first.wpilibj;
import edu.wpi.cscore.CameraServerJNI;
import edu.wpi.first.cameraserver.CameraServerShared;
import edu.wpi.first.cameraserver.CameraServerSharedStore;
import edu.wpi.first.cscore.CameraServerJNI;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;

719
wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
View File

@@ -1,719 +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 static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
/**
* Utility class for handling Robot drive based on a definition of the motor configuration. The
* robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and mecanum
* drive trains are supported. In the future other drive types like swerve might be implemented.
* Motor channel numbers are supplied on creation of the class. Those are used for either the drive
* function (intended for hand created drive code, such as autonomous) or with the Tank/Arcade
* functions intended to be used for Operator Control driving.
*
* @deprecated Use {@link edu.wpi.first.wpilibj.drive.DifferentialDrive} or {@link
* edu.wpi.first.wpilibj.drive.MecanumDrive} classes instead.
*/
@Deprecated
@SuppressWarnings("PMD.GodClass")
public class RobotDrive extends MotorSafety implements AutoCloseable {
/** The location of a motor on the robot for the purpose of driving. */
public enum MotorType {
kFrontLeft(0),
kFrontRight(1),
kRearLeft(2),
kRearRight(3);
public final int value;
MotorType(int value) {
this.value = value;
}
}
public static final double kDefaultExpirationTime = 0.1;
public static final double kDefaultSensitivity = 0.5;
public static final double kDefaultMaxOutput = 1.0;
protected static final int kMaxNumberOfMotors = 4;
protected double m_sensitivity;
protected double m_maxOutput;
protected SpeedController m_frontLeftMotor;
protected SpeedController m_frontRightMotor;
protected SpeedController m_rearLeftMotor;
protected SpeedController m_rearRightMotor;
protected boolean m_allocatedSpeedControllers;
protected static boolean kArcadeRatioCurve_Reported;
protected static boolean kTank_Reported;
protected static boolean kArcadeStandard_Reported;
protected static boolean kMecanumCartesian_Reported;
protected static boolean kMecanumPolar_Reported;
/**
* Constructor for RobotDrive with 2 motors specified with channel numbers. Set up parameters for
* a two wheel drive system where the left and right motor pwm channels are specified in the call.
* This call assumes Talons for controlling the motors.
*
* @param leftMotorChannel The PWM channel number that drives the left motor.
* @param rightMotorChannel The PWM channel number that drives the right motor.
*/
public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) {
m_sensitivity = kDefaultSensitivity;
m_maxOutput = kDefaultMaxOutput;
m_frontLeftMotor = null;
m_rearLeftMotor = new Talon(leftMotorChannel);
m_frontRightMotor = null;
m_rearRightMotor = new Talon(rightMotorChannel);
m_allocatedSpeedControllers = true;
setSafetyEnabled(true);
drive(0, 0);
}
/**
* Constructor for RobotDrive with 4 motors specified with channel numbers. Set up parameters for
* a four wheel drive system where all four motor pwm channels are specified in the call. This
* call assumes Talons for controlling the motors.
*
* @param frontLeftMotor Front left motor channel number
* @param rearLeftMotor Rear Left motor channel number
* @param frontRightMotor Front right motor channel number
* @param rearRightMotor Rear Right motor channel number
*/
public RobotDrive(
final int frontLeftMotor,
final int rearLeftMotor,
final int frontRightMotor,
final int rearRightMotor) {
m_sensitivity = kDefaultSensitivity;
m_maxOutput = kDefaultMaxOutput;
m_rearLeftMotor = new Talon(rearLeftMotor);
m_rearRightMotor = new Talon(rearRightMotor);
m_frontLeftMotor = new Talon(frontLeftMotor);
m_frontRightMotor = new Talon(frontRightMotor);
m_allocatedSpeedControllers = true;
setSafetyEnabled(true);
drive(0, 0);
}
/**
* Constructor for RobotDrive with 2 motors specified as SpeedController objects. The
* SpeedController version of the constructor enables programs to use the RobotDrive classes with
* subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
* the curve to suit motor bias or dead-band elimination.
*
* @param leftMotor The left SpeedController object used to drive the robot.
* @param rightMotor the right SpeedController object used to drive the robot.
*/
public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) {
requireNonNullParam(leftMotor, "leftMotor", "RobotDrive");
requireNonNullParam(rightMotor, "rightMotor", "RobotDrive");
m_frontLeftMotor = null;
m_rearLeftMotor = leftMotor;
m_frontRightMotor = null;
m_rearRightMotor = rightMotor;
m_sensitivity = kDefaultSensitivity;
m_maxOutput = kDefaultMaxOutput;
m_allocatedSpeedControllers = false;
setSafetyEnabled(true);
drive(0, 0);
}
/**
* Constructor for RobotDrive with 4 motors specified as SpeedController objects. Speed controller
* input version of RobotDrive (see previous comments).
*
* @param frontLeftMotor The front left SpeedController object used to drive the robot
* @param rearLeftMotor The back left SpeedController object used to drive the robot.
* @param frontRightMotor The front right SpeedController object used to drive the robot.
* @param rearRightMotor The back right SpeedController object used to drive the robot.
*/
public RobotDrive(
SpeedController frontLeftMotor,
SpeedController rearLeftMotor,
SpeedController frontRightMotor,
SpeedController rearRightMotor) {
m_frontLeftMotor = requireNonNullParam(frontLeftMotor, "frontLeftMotor", "RobotDrive");
m_rearLeftMotor = requireNonNullParam(rearLeftMotor, "rearLeftMotor", "RobotDrive");
m_frontRightMotor = requireNonNullParam(frontRightMotor, "frontRightMotor", "RobotDrive");
m_rearRightMotor = requireNonNullParam(rearRightMotor, "rearRightMotor", "RobotDrive");
m_sensitivity = kDefaultSensitivity;
m_maxOutput = kDefaultMaxOutput;
m_allocatedSpeedControllers = false;
setSafetyEnabled(true);
drive(0, 0);
}
/**
* Drive the motors at "outputMagnitude" and "curve". Both outputMagnitude and curve are -1.0 to
* +1.0 values, where 0.0 represents stopped and not turning. {@literal curve < 0 will turn left
* and curve > 0} will turn right.
*
* <p>The algorithm for steering provides a constant turn radius for any normal speed range, both
* forward and backward. Increasing sensitivity causes sharper turns for fixed values of curve.
*
* <p>This function will most likely be used in an autonomous routine.
*
* @param outputMagnitude The speed setting for the outside wheel in a turn, forward or backwards,
* +1 to -1.
* @param curve The rate of turn, constant for different forward speeds. Set {@literal curve < 0
* for left turn or curve > 0 for right turn.} Set curve = e^(-r/w) to get a turn radius r for
* wheelbase w of your robot. Conversely, turn radius r = -ln(curve)*w for a given value of
* curve and wheelbase w.
*/
public void drive(double outputMagnitude, double curve) {
final double leftOutput;
final double rightOutput;
if (!kArcadeRatioCurve_Reported) {
HAL.report(
tResourceType.kResourceType_RobotDrive,
tInstances.kRobotDrive_ArcadeRatioCurve,
getNumMotors());
kArcadeRatioCurve_Reported = true;
}
if (curve < 0) {
double value = Math.log(-curve);
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
if (ratio == 0) {
ratio = 0.0000000001;
}
leftOutput = outputMagnitude / ratio;
rightOutput = outputMagnitude;
} else if (curve > 0) {
double value = Math.log(curve);
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
if (ratio == 0) {
ratio = 0.0000000001;
}
leftOutput = outputMagnitude;
rightOutput = outputMagnitude / ratio;
} else {
leftOutput = outputMagnitude;
rightOutput = outputMagnitude;
}
setLeftRightMotorOutputs(leftOutput, rightOutput);
}
/**
* Provide tank steering using the stored robot configuration. drive the robot using two joystick
* inputs. The Y-axis will be selected from each Joystick object. The calculated values will be
* squared to decrease sensitivity at low speeds.
*
* @param leftStick The joystick to control the left side of the robot.
* @param rightStick The joystick to control the right side of the robot.
*/
public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
requireNonNullParam(leftStick, "leftStick", "tankDrive");
requireNonNullParam(rightStick, "rightStick", "tankDrive");
tankDrive(leftStick.getY(), rightStick.getY(), true);
}
/**
* Provide tank steering using the stored robot configuration. drive the robot using two joystick
* inputs. The Y-axis will be selected from each Joystick object.
*
* @param leftStick The joystick to control the left side of the robot.
* @param rightStick The joystick to control the right side of the robot.
* @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
*/
public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) {
requireNonNullParam(leftStick, "leftStick", "tankDrive");
requireNonNullParam(rightStick, "rightStick", "tankDrive");
tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs);
}
/**
* Provide tank steering using the stored robot configuration. This function lets you pick the
* axis to be used on each Joystick object for the left and right sides of the robot. The
* calculated values will be squared to decrease sensitivity at low speeds.
*
* @param leftStick The Joystick object to use for the left side of the robot.
* @param leftAxis The axis to select on the left side Joystick object.
* @param rightStick The Joystick object to use for the right side of the robot.
* @param rightAxis The axis to select on the right side Joystick object.
*/
public void tankDrive(
GenericHID leftStick, final int leftAxis, GenericHID rightStick, final int rightAxis) {
requireNonNullParam(leftStick, "leftStick", "tankDrive");
requireNonNullParam(rightStick, "rightStick", "tankDrive");
tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true);
}
/**
* Provide tank steering using the stored robot configuration. This function lets you pick the
* axis to be used on each Joystick object for the left and right sides of the robot.
*
* @param leftStick The Joystick object to use for the left side of the robot.
* @param leftAxis The axis to select on the left side Joystick object.
* @param rightStick The Joystick object to use for the right side of the robot.
* @param rightAxis The axis to select on the right side Joystick object.
* @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
*/
public void tankDrive(
GenericHID leftStick,
final int leftAxis,
GenericHID rightStick,
final int rightAxis,
boolean squaredInputs) {
requireNonNullParam(leftStick, "leftStick", "tankDrive");
requireNonNullParam(rightStick, "rightStick", "tankDrive");
tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs);
}
/**
* Provide tank steering using the stored robot configuration. This function lets you directly
* provide joystick values from any source.
*
* @param leftValue The value of the left stick.
* @param rightValue The value of the right stick.
* @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
*/
public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {
if (!kTank_Reported) {
HAL.report(
tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_Tank, getNumMotors());
kTank_Reported = true;
}
leftValue = limit(leftValue);
rightValue = limit(rightValue);
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (squaredInputs) {
leftValue = Math.copySign(leftValue * leftValue, leftValue);
rightValue = Math.copySign(rightValue * rightValue, rightValue);
}
setLeftRightMotorOutputs(leftValue, rightValue);
}
/**
* Provide tank steering using the stored robot configuration. This function lets you directly
* provide joystick values from any source. The calculated values will be squared to decrease
* sensitivity at low speeds.
*
* @param leftValue The value of the left stick.
* @param rightValue The value of the right stick.
*/
public void tankDrive(double leftValue, double rightValue) {
tankDrive(leftValue, rightValue, true);
}
/**
* Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
* axis for the move value and the X axis for the rotate value. (Should add more information here
* regarding the way that arcade drive works.)
*
* @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
* for forwards/backwards and the X-axis will be selected for rotation rate.
* @param squaredInputs If true, the sensitivity will be decreased for small values
*/
public void arcadeDrive(GenericHID stick, boolean squaredInputs) {
// simply call the full-featured arcadeDrive with the appropriate values
arcadeDrive(stick.getY(), stick.getX(), squaredInputs);
}
/**
* Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
* axis for the move value and the X axis for the rotate value. (Should add more information here
* regarding the way that arcade drive works.) The calculated values will be squared to decrease
* sensitivity at low speeds.
*
* @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
* for forwards/backwards and the X-axis will be selected for rotation rate.
*/
public void arcadeDrive(GenericHID stick) {
arcadeDrive(stick, true);
}
/**
* Arcade drive implements single stick driving. Given two joystick instances and two axis,
* compute the values to send to either two or four motors.
*
* @param moveStick The Joystick object that represents the forward/backward direction
* @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically
* Y_AXIS)
* @param rotateStick The Joystick object that represents the rotation value
* @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically
* X_AXIS)
* @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
*/
public void arcadeDrive(
GenericHID moveStick,
final int moveAxis,
GenericHID rotateStick,
final int rotateAxis,
boolean squaredInputs) {
double moveValue = moveStick.getRawAxis(moveAxis);
double rotateValue = rotateStick.getRawAxis(rotateAxis);
arcadeDrive(moveValue, rotateValue, squaredInputs);
}
/**
* Arcade drive implements single stick driving. Given two joystick instances and two axis,
* compute the values to send to either two or four motors. The calculated values will be squared
* to decrease sensitivity at low speeds.
*
* @param moveStick The Joystick object that represents the forward/backward direction
* @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically
* Y_AXIS)
* @param rotateStick The Joystick object that represents the rotation value
* @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically
* X_AXIS)
*/
public void arcadeDrive(
GenericHID moveStick, final int moveAxis, GenericHID rotateStick, final int rotateAxis) {
arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true);
}
/**
* Arcade drive implements single stick driving. This function lets you directly provide joystick
* values from any source.
*
* @param moveValue The value to use for forwards/backwards
* @param rotateValue The value to use for the rotate right/left
* @param squaredInputs If set, decreases the sensitivity at low speeds
*/
public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
// local variables to hold the computed PWM values for the motors
if (!kArcadeStandard_Reported) {
HAL.report(
tResourceType.kResourceType_RobotDrive,
tInstances.kRobotDrive_ArcadeStandard,
getNumMotors());
kArcadeStandard_Reported = true;
}
double leftMotorSpeed;
double rightMotorSpeed;
moveValue = limit(moveValue);
rotateValue = limit(rotateValue);
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
if (squaredInputs) {
// square the inputs (while preserving the sign) to increase fine control
// while permitting full power
moveValue = Math.copySign(moveValue * moveValue, moveValue);
rotateValue = Math.copySign(rotateValue * rotateValue, rotateValue);
}
if (moveValue > 0.0) {
if (rotateValue > 0.0) {
leftMotorSpeed = moveValue - rotateValue;
rightMotorSpeed = Math.max(moveValue, rotateValue);
} else {
leftMotorSpeed = Math.max(moveValue, -rotateValue);
rightMotorSpeed = moveValue + rotateValue;
}
} else {
if (rotateValue > 0.0) {
leftMotorSpeed = -Math.max(-moveValue, rotateValue);
rightMotorSpeed = moveValue + rotateValue;
} else {
leftMotorSpeed = moveValue - rotateValue;
rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
}
}
setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
}
/**
* Arcade drive implements single stick driving. This function lets you directly provide joystick
* values from any source. The calculated values will be squared to decrease sensitivity at low
* speeds.
*
* @param moveValue The value to use for forwards/backwards
* @param rotateValue The value to use for the rotate right/left
*/
public void arcadeDrive(double moveValue, double rotateValue) {
arcadeDrive(moveValue, rotateValue, true);
}
/**
* Drive method for Mecanum wheeled robots.
*
* <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
* so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
* top, the roller axles should form an X across the robot.
*
* <p>This is designed to be directly driven by joystick axes.
*
* @param x The speed that the robot should drive in the X direction. [-1.0..1.0]
* @param y The speed that the robot should drive in the Y direction. This input is inverted to
* match the forward == -1.0 that joysticks produce. [-1.0..1.0]
* @param rotation The rate of rotation for the robot that is completely independent of the
* translation. [-1.0..1.0]
* @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented
* controls.
*/
public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
if (!kMecanumCartesian_Reported) {
HAL.report(
tResourceType.kResourceType_RobotDrive,
tInstances.kRobotDrive_MecanumCartesian,
getNumMotors());
kMecanumCartesian_Reported = true;
}
// Negate y for the joystick.
y = -y;
// Compensate for gyro angle.
double[] rotated = rotateVector(x, y, gyroAngle);
x = rotated[0];
y = rotated[1];
double[] wheelSpeeds = new double[kMaxNumberOfMotors];
wheelSpeeds[MotorType.kFrontLeft.value] = x + y + rotation;
wheelSpeeds[MotorType.kFrontRight.value] = -x + y - rotation;
wheelSpeeds[MotorType.kRearLeft.value] = -x + y + rotation;
wheelSpeeds[MotorType.kRearRight.value] = x + y - rotation;
normalize(wheelSpeeds);
m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
feed();
}
/**
* Drive method for Mecanum wheeled robots.
*
* <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
* so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
* top, the roller axles should form an X across the robot.
*
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
* @param direction The angle the robot should drive in degrees. The direction and magnitude are
* independent of the rotation rate. [-180.0..180.0]
* @param rotation The rate of rotation for the robot that is completely independent of the
* magnitude or direction. [-1.0..1.0]
*/
public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
if (!kMecanumPolar_Reported) {
HAL.report(
tResourceType.kResourceType_RobotDrive,
tInstances.kRobotDrive_MecanumPolar,
getNumMotors());
kMecanumPolar_Reported = true;
}
// Normalized for full power along the Cartesian axes.
magnitude = limit(magnitude) * Math.sqrt(2.0);
// The rollers are at 45 degree angles.
double dirInRad = (direction + 45.0) * Math.PI / 180.0;
double cosD = Math.cos(dirInRad);
double sinD = Math.sin(dirInRad);
double[] wheelSpeeds = new double[kMaxNumberOfMotors];
wheelSpeeds[MotorType.kFrontLeft.value] = sinD * magnitude + rotation;
wheelSpeeds[MotorType.kFrontRight.value] = cosD * magnitude - rotation;
wheelSpeeds[MotorType.kRearLeft.value] = cosD * magnitude + rotation;
wheelSpeeds[MotorType.kRearRight.value] = sinD * magnitude - rotation;
normalize(wheelSpeeds);
m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
feed();
}
/**
* Holonomic Drive method for Mecanum wheeled robots.
*
* <p>This is an alias to mecanumDrive_Polar() for backward compatibility
*
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
* @param direction The direction the robot should drive. The direction and maginitude are
* independent of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely independent of the
* magnitute or direction. [-1.0..1.0]
*/
void holonomicDrive(double magnitude, double direction, double rotation) {
mecanumDrive_Polar(magnitude, direction, rotation);
}
/**
* Set the speed of the right and left motors. This is used once an appropriate drive setup
* function is called such as twoWheelDrive(). The motors are set to "leftSpeed" and "rightSpeed"
* and includes flipping the direction of one side for opposing motors.
*
* @param leftOutput The speed to send to the left side of the robot.
* @param rightOutput The speed to send to the right side of the robot.
*/
public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) {
if (m_frontLeftMotor != null) {
m_frontLeftMotor.set(limit(leftOutput) * m_maxOutput);
}
m_rearLeftMotor.set(limit(leftOutput) * m_maxOutput);
if (m_frontRightMotor != null) {
m_frontRightMotor.set(-limit(rightOutput) * m_maxOutput);
}
m_rearRightMotor.set(-limit(rightOutput) * m_maxOutput);
feed();
}
/** Limit motor values to the -1.0 to +1.0 range. */
protected static double limit(double number) {
if (number > 1.0) {
return 1.0;
}
if (number < -1.0) {
return -1.0;
}
return number;
}
/** Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0. */
protected static void normalize(double[] wheelSpeeds) {
double maxMagnitude = Math.abs(wheelSpeeds[0]);
for (int i = 1; i < kMaxNumberOfMotors; i++) {
double temp = Math.abs(wheelSpeeds[i]);
if (maxMagnitude < temp) {
maxMagnitude = temp;
}
}
if (maxMagnitude > 1.0) {
for (int i = 0; i < kMaxNumberOfMotors; i++) {
wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
}
}
}
/** Rotate a vector in Cartesian space. */
protected static double[] rotateVector(double x, double y, double angle) {
double cosA = Math.cos(angle * (Math.PI / 180.0));
double sinA = Math.sin(angle * (Math.PI / 180.0));
double[] out = new double[2];
out[0] = x * cosA - y * sinA;
out[1] = x * sinA + y * cosA;
return out;
}
/**
* Invert a motor direction. This is used when a motor should run in the opposite direction as the
* drive code would normally run it. Motors that are direct drive would be inverted, the drive
* code assumes that the motors are geared with one reversal.
*
* @param motor The motor index to invert.
* @param isInverted True if the motor should be inverted when operated.
*/
public void setInvertedMotor(MotorType motor, boolean isInverted) {
switch (motor) {
case kFrontLeft:
m_frontLeftMotor.setInverted(isInverted);
break;
case kFrontRight:
m_frontRightMotor.setInverted(isInverted);
break;
case kRearLeft:
m_rearLeftMotor.setInverted(isInverted);
break;
case kRearRight:
m_rearRightMotor.setInverted(isInverted);
break;
default:
throw new IllegalArgumentException("Illegal motor type: " + motor);
}
}
/**
* Set the turning sensitivity.
*
* <p>This only impacts the drive() entry-point.
*
* @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
*/
public void setSensitivity(double sensitivity) {
m_sensitivity = sensitivity;
}
/**
* Configure the scaling factor for using RobotDrive with motor controllers in a mode other than
* PercentVbus.
*
* @param maxOutput Multiplied with the output percentage computed by the drive functions.
*/
public void setMaxOutput(double maxOutput) {
m_maxOutput = maxOutput;
}
/** Free the speed controllers if they were allocated locally. */
@Override
public void close() {
if (m_allocatedSpeedControllers) {
if (m_frontLeftMotor != null) {
((PWM) m_frontLeftMotor).close();
}
if (m_frontRightMotor != null) {
((PWM) m_frontRightMotor).close();
}
if (m_rearLeftMotor != null) {
((PWM) m_rearLeftMotor).close();
}
if (m_rearRightMotor != null) {
((PWM) m_rearRightMotor).close();
}
}
}
@Override
public String getDescription() {
return "Robot Drive";
}
@Override
public void stopMotor() {
if (m_frontLeftMotor != null) {
m_frontLeftMotor.stopMotor();
}
if (m_frontRightMotor != null) {
m_frontRightMotor.stopMotor();
}
if (m_rearLeftMotor != null) {
m_rearLeftMotor.stopMotor();
}
if (m_rearRightMotor != null) {
m_rearRightMotor.stopMotor();
}
feed();
}
protected int getNumMotors() {
int motors = 0;
if (m_frontLeftMotor != null) {
motors++;
}
if (m_frontRightMotor != null) {
motors++;
}
if (m_rearLeftMotor != null) {
motors++;
}
if (m_rearRightMotor != null) {
motors++;
}
return motors;
}
}

93
wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java
View File

@@ -5,102 +5,9 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/** The base interface for objects that can be sent over the network through network tables. */
public interface Sendable {
/**
* Gets the name of this {@link Sendable} object.
*
* @return Name
* @deprecated Use SendableRegistry.getName()
*/
@Deprecated(since = "2020", forRemoval = true)
default String getName() {
return SendableRegistry.getName(this);
}
/**
* Sets the name of this {@link Sendable} object.
*
* @param name name
* @deprecated Use SendableRegistry.setName()
*/
@Deprecated(since = "2020", forRemoval = true)
default void setName(String name) {
SendableRegistry.setName(this, name);
}
/**
* Sets both the subsystem name and device name of this {@link Sendable} object.
*
* @param subsystem subsystem name
* @param name device name
* @deprecated Use SendableRegistry.setName()
*/
@Deprecated(since = "2020", forRemoval = true)
default void setName(String subsystem, String name) {
SendableRegistry.setName(this, subsystem, name);
}
/**
* Sets the name of the sensor with a channel number.
*
* @param moduleType A string that defines the module name in the label for the value
* @param channel The channel number the device is plugged into
* @deprecated Use SendableRegistry.setName()
*/
@Deprecated(since = "2020", forRemoval = true)
default void setName(String moduleType, int channel) {
SendableRegistry.setName(this, moduleType, channel);
}
/**
* Sets the name of the sensor with a module and channel number.
*
* @param moduleType A string that defines the module name in the label for the value
* @param moduleNumber The number of the particular module type
* @param channel The channel number the device is plugged into (usually PWM)
* @deprecated Use SendableRegistry.setName()
*/
@Deprecated(since = "2020", forRemoval = true)
default void setName(String moduleType, int moduleNumber, int channel) {
SendableRegistry.setName(this, moduleType, moduleNumber, channel);
}
/**
* Gets the subsystem name of this {@link Sendable} object.
*
* @return Subsystem name
* @deprecated Use SendableRegistry.getSubsystem()
*/
@Deprecated(since = "2020", forRemoval = true)
default String getSubsystem() {
return SendableRegistry.getSubsystem(this);
}
/**
* Sets the subsystem name of this {@link Sendable} object.
*
* @param subsystem subsystem name
* @deprecated Use SendableRegistry.setSubsystem()
*/
@Deprecated(since = "2020", forRemoval = true)
default void setSubsystem(String subsystem) {
SendableRegistry.setSubsystem(this, subsystem);
}
/**
* Add a child component.
*
* @param child child component
* @deprecated Use SendableRegistry.addChild()
*/
@Deprecated(since = "2020", forRemoval = true)
default void addChild(Object child) {
SendableRegistry.addChild(this, child);
}
/**
* Initializes this {@link Sendable} object.
*

39
wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java
View File

@@ -1,39 +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.wpilibj.smartdashboard.SendableRegistry;
/**
* Base class for all sensors. Stores most recent status information as well as containing utility
* functions for checking channels and error processing.
*
* @deprecated Use Sendable and SendableRegistry
*/
@Deprecated(since = "2020", forRemoval = true)
public abstract class SendableBase implements Sendable, AutoCloseable {
/** Creates an instance of the sensor base. */
public SendableBase() {
this(true);
}
/**
* Creates an instance of the sensor base.
*
* @param addLiveWindow if true, add this Sendable to LiveWindow
*/
public SendableBase(boolean addLiveWindow) {
if (addLiveWindow) {
SendableRegistry.addLW(this, "");
} else {
SendableRegistry.add(this, "");
}
}
@Override
public void close() {
SendableRegistry.remove(this);
}
}

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
View File

@@ -4,13 +4,13 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpiutil.math.MathUtil;
import edu.wpi.first.math.MathUtil;
/**
* A class that limits the rate of change of an input value. Useful for implementing voltage,
* setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
* controlled is a velocity or a voltage; when controlling a position, consider using a {@link
* edu.wpi.first.wpilibj.trajectory.TrapezoidProfile} instead.
* edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
*/
public class SlewRateLimiter {
private final double m_rateLimit;

22
wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
View File

@@ -4,18 +4,22 @@
package edu.wpi.first.wpilibj;
/** Interface for speed controlling devices. */
@SuppressWarnings("removal")
public interface SpeedController extends PIDOutput {
/**
* Interface for motor controlling devices.
*
* @deprecated Use {@link edu.wpi.first.wpilibj.motorcontrol.MotorController}.
*/
@Deprecated(since = "2022", forRemoval = true)
public interface SpeedController {
/**
* Common interface for setting the speed of a speed controller.
* Common interface for setting the speed of a motor controller.
*
* @param speed The speed to set. Value should be between -1.0 and 1.0.
*/
void set(double speed);
/**
* Sets the voltage output of the SpeedController. Compensates for the current bus voltage to
* Sets the voltage output of the MotorController. Compensates for the current bus voltage to
* ensure that the desired voltage is output even if the battery voltage is below 12V - highly
* useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward
* calculation).
@@ -30,27 +34,27 @@ public interface SpeedController extends PIDOutput {
}
/**
* Common interface for getting the current set speed of a speed controller.
* Common interface for getting the current set speed of a motor controller.
*
* @return The current set speed. Value is between -1.0 and 1.0.
*/
double get();
/**
* Common interface for inverting direction of a speed controller.
* Common interface for inverting direction of a motor controller.
*
* @param isInverted The state of inversion true is inverted.
*/
void setInverted(boolean isInverted);
/**
* Common interface for returning if a speed controller is in the inverted state or not.
* Common interface for returning if a motor controller is in the inverted state or not.
*
* @return isInverted The state of the inversion true is inverted.
*/
boolean getInverted();
/** Disable the speed controller. */
/** Disable the motor controller. */
void disable();
/**

20
wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
View File

@@ -4,12 +4,19 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import java.util.Arrays;
/** Allows multiple {@link SpeedController} objects to be linked together. */
public class SpeedControllerGroup implements SpeedController, Sendable, AutoCloseable {
/**
* Allows multiple {@link SpeedController} objects to be linked together.
*
* @deprecated Use {@link edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup}.
*/
@Deprecated(since = "2022", forRemoval = true)
@SuppressWarnings("removal")
public class SpeedControllerGroup implements MotorController, Sendable, AutoCloseable {
private boolean m_isInverted;
private final SpeedController[] m_speedControllers;
private static int instances;
@@ -40,7 +47,7 @@ public class SpeedControllerGroup implements SpeedController, Sendable, AutoClos
SendableRegistry.addChild(this, controller);
}
instances++;
SendableRegistry.addLW(this, "SpeedControllerGroup", instances);
SendableRegistry.addLW(this, "MotorControllerGroup", instances);
}
@Override
@@ -87,14 +94,9 @@ public class SpeedControllerGroup implements SpeedController, Sendable, AutoClos
}
}
@Override
public void pidWrite(double output) {
set(output);
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Speed Controller");
builder.setSmartDashboardType("Motor Controller");
builder.setActuator(true);
builder.setSafeState(this::stopMotor);
builder.addDoubleProperty("Value", this::get, this::set);

5
wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
View File

@@ -137,11 +137,6 @@ public class TimedRobot extends IterativeRobotBase {
NotifierJNI.stopNotifier(m_notifier);
}
/** Get time period between calls to Periodic() functions. */
public double getPeriod() {
return m_period;
}
/**
* Add a callback to run at a specific period.
*

95
wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
View File

@@ -26,15 +26,7 @@ import java.util.List;
* echo is received. The time that the line is high determines the round trip distance (time of
* flight).
*/
public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
/** The units to return when PIDGet is called. */
public enum Unit {
/** Use inches for PIDGet. */
kInches,
/** Use millimeters for PIDGet. */
kMillimeters
}
public class Ultrasonic implements Sendable, AutoCloseable {
// Time (sec) for the ping trigger pulse.
private static final double kPingTime = 10 * 1e-6;
private static final double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
@@ -44,14 +36,12 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
private static volatile boolean m_automaticEnabled;
private DigitalInput m_echoChannel;
private DigitalOutput m_pingChannel;
private boolean m_allocatedChannels;
private final boolean m_allocatedChannels;
private boolean m_enabled;
private Counter m_counter;
// task doing the round-robin automatic sensing
private static Thread m_task;
private Unit m_units;
private static int m_instances;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
private SimDevice m_simDevice;
private SimBoolean m_simRangeValid;
@@ -128,31 +118,16 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
* sending the ping.
* @param echoChannel The digital input channel that receives the echo. The length of time that
* the echo is high represents the round trip time of the ping, and the distance.
* @param units The units returned in either kInches or kMilliMeters
*/
public Ultrasonic(final int pingChannel, final int echoChannel, Unit units) {
public Ultrasonic(final int pingChannel, final int echoChannel) {
m_pingChannel = new DigitalOutput(pingChannel);
m_echoChannel = new DigitalInput(echoChannel);
SendableRegistry.addChild(this, m_pingChannel);
SendableRegistry.addChild(this, m_echoChannel);
m_allocatedChannels = true;
m_units = units;
initialize();
}
/**
* Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
* and Vex ultrasonic sensors. Default unit is inches.
*
* @param pingChannel The digital output channel that sends the pulse to initiate the sensor
* sending the ping.
* @param echoChannel The digital input channel that receives the echo. The length of time that
* the echo is high represents the round trip time of the ping, and the distance.
*/
public Ultrasonic(final int pingChannel, final int echoChannel) {
this(pingChannel, echoChannel, Unit.kInches);
}
/**
* Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
* DigitalOutput for the ping channel.
@@ -160,31 +135,17 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
* @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
* 10uS pulse to start.
* @param echoChannel The digital input object that times the return pulse to determine the range.
* @param units The units returned in either kInches or kMilliMeters
*/
public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel, Unit units) {
public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
requireNonNull(pingChannel, "Provided ping channel was null");
requireNonNull(echoChannel, "Provided echo channel was null");
m_allocatedChannels = false;
m_pingChannel = pingChannel;
m_echoChannel = echoChannel;
m_units = units;
initialize();
}
/**
* Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
* DigitalOutput for the ping channel. Default unit is inches.
*
* @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
* 10uS pulse to start.
* @param echoChannel The digital input object that times the return pulse to determine the range.
*/
public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
this(pingChannel, echoChannel, Unit.kInches);
}
/**
* Destructor for the ultrasonic sensor. Delete the instance of the ultrasonic sensor by freeing
* the allocated digital channels. If the system was in automatic mode (round robin), then it is
@@ -326,54 +287,6 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
return getRangeInches() * 25.4;
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
if (!pidSource.equals(PIDSourceType.kDisplacement)) {
throw new IllegalArgumentException("Only displacement PID is allowed for ultrasonics.");
}
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the range in the current DistanceUnit for the PIDSource base object.
*
* @return The range in DistanceUnit
*/
@Override
public double pidGet() {
switch (m_units) {
case kInches:
return getRangeInches();
case kMillimeters:
return getRangeMM();
default:
return 0.0;
}
}
/**
* Set the current DistanceUnit that should be used for the PIDSource base object.
*
* @param units The DistanceUnit that should be used.
*/
public void setDistanceUnits(Unit units) {
m_units = units;
}
/**
* Get the current DistanceUnit that is used for the PIDSource base object.
*
* @return The type of DistanceUnit that is being used.
*/
public Unit getDistanceUnits() {
return m_units;
}
/**
* Is the ultrasonic enabled.
*

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
View File

@@ -4,10 +4,10 @@
package edu.wpi.first.wpilibj.controller;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
/**
* This holonomic drive controller can be used to follow trajectories using a holonomic drive train

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
View File

@@ -6,10 +6,10 @@ package edu.wpi.first.wpilibj.controller;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpiutil.math.MathUtil;
/** Implements a PID control loop. */
@SuppressWarnings("PMD.TooManyFields")

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
View File

@@ -6,10 +6,10 @@ package edu.wpi.first.wpilibj.controller;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpiutil.math.MathUtil;
/**
* Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should

166
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
View File

@@ -1,166 +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.controller;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
/**
* Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model
* to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law
* in addition to the linear ones we have used so far like PID? If we use the original approach with
* PID controllers for left and right position and velocity states, the controllers only deal with
* the local pose. If the robot deviates from the path, there is no way for the controllers to
* correct and the robot may not reach the desired global pose. This is due to multiple endpoints
* existing for the robot which have the same encoder path arc lengths.
*
* <p>Instead of using wheel path arc lengths (which are in the robot's local coordinate frame),
* nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this
* extra information to guide a linear reference tracker like the PID controllers back in by
* adjusting the references of the PID controllers.
*
* <p>The paper "Control of Wheeled Mobile Robots: An Experimental Overview" describes a nonlinear
* controller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y,
* and theta; and a desired pose consisting of x_d, y_d, and theta_d. We call it Ramsete because
* that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e
* Mobile per i SErvizi e le TEcnologie").
*
* <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
* Engineering in the FIRST Robotics Competition</a> section on Ramsete unicycle controller for a
* derivation and analysis.
*/
public class RamseteController {
@SuppressWarnings("MemberName")
private final double m_b;
@SuppressWarnings("MemberName")
private final double m_zeta;
private Pose2d m_poseError = new Pose2d();
private Pose2d m_poseTolerance = new Pose2d();
private boolean m_enabled = true;
/**
* Construct a Ramsete unicycle controller.
*
* @param b Tuning parameter (b &gt; 0) for which larger values make convergence more aggressive
* like a proportional term.
* @param zeta Tuning parameter (0 &lt; zeta &lt; 1) for which larger values provide more damping
* in response.
*/
@SuppressWarnings("ParameterName")
public RamseteController(double b, double zeta) {
m_b = b;
m_zeta = zeta;
}
/**
* Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 and 0.7
* have been well-tested to produce desirable results.
*/
public RamseteController() {
this(2.0, 0.7);
}
/** Returns true if the pose error is within tolerance of the reference. */
public boolean atReference() {
final var eTranslate = m_poseError.getTranslation();
final var eRotate = m_poseError.getRotation();
final var tolTranslate = m_poseTolerance.getTranslation();
final var tolRotate = m_poseTolerance.getRotation();
return Math.abs(eTranslate.getX()) < tolTranslate.getX()
&& Math.abs(eTranslate.getY()) < tolTranslate.getY()
&& Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
}
/**
* Sets the pose error which is considered tolerable for use with atReference().
*
* @param poseTolerance Pose error which is tolerable.
*/
public void setTolerance(Pose2d poseTolerance) {
m_poseTolerance = poseTolerance;
}
/**
* Returns the next output of the Ramsete controller.
*
* <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
* trajectory.
*
* @param currentPose The current pose.
* @param poseRef The desired pose.
* @param linearVelocityRefMeters The desired linear velocity in meters per second.
* @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
*/
@SuppressWarnings("LocalVariableName")
public ChassisSpeeds calculate(
Pose2d currentPose,
Pose2d poseRef,
double linearVelocityRefMeters,
double angularVelocityRefRadiansPerSecond) {
if (!m_enabled) {
return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
}
m_poseError = poseRef.relativeTo(currentPose);
// Aliases for equation readability
final double eX = m_poseError.getX();
final double eY = m_poseError.getY();
final double eTheta = m_poseError.getRotation().getRadians();
final double vRef = linearVelocityRefMeters;
final double omegaRef = angularVelocityRefRadiansPerSecond;
double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
return new ChassisSpeeds(
vRef * m_poseError.getRotation().getCos() + k * eX,
0.0,
omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
}
/**
* Returns the next output of the Ramsete controller.
*
* <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
* trajectory.
*
* @param currentPose The current pose.
* @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
*/
@SuppressWarnings("LocalVariableName")
public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
return calculate(
currentPose,
desiredState.poseMeters,
desiredState.velocityMetersPerSecond,
desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
}
/**
* Enables and disables the controller for troubleshooting purposes.
*
* @param enabled If the controller is enabled or not.
*/
public void setEnabled(boolean enabled) {
m_enabled = enabled;
}
/**
* Returns sin(x) / x.
*
* @param x Value of which to take sinc(x).
*/
@SuppressWarnings("ParameterName")
private static double sinc(double x) {
if (Math.abs(x) < 1e-9) {
return 1.0 - 1.0 / 6.0 * x * x;
} else {
return Math.sin(x) / x;
}
}
}

58
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
View File

@@ -7,12 +7,11 @@ package edu.wpi.first.wpilibj.drive;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpiutil.math.MathUtil;
import java.util.StringJoiner;
/**
@@ -20,21 +19,21 @@ import java.util.StringJoiner;
* base, "tank drive", or West Coast Drive.
*
* <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
* (e.g., 6WD or 8WD). This class takes a SpeedController per side. For four and six motor
* drivetrains, construct and pass in {@link edu.wpi.first.wpilibj.SpeedControllerGroup} instances
* as follows.
* (e.g., 6WD or 8WD). This class takes a MotorController per side. For four and six motor
* drivetrains, construct and pass in {@link
* edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup} instances as follows.
*
* <p>Four motor drivetrain:
*
* <pre><code>
* public class Robot {
* SpeedController m_frontLeft = new PWMVictorSPX(1);
* SpeedController m_rearLeft = new PWMVictorSPX(2);
* SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
* MotorController m_frontLeft = new PWMVictorSPX(1);
* MotorController m_rearLeft = new PWMVictorSPX(2);
* MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_rearLeft);
*
* SpeedController m_frontRight = new PWMVictorSPX(3);
* SpeedController m_rearRight = new PWMVictorSPX(4);
* SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight);
* MotorController m_frontRight = new PWMVictorSPX(3);
* MotorController m_rearRight = new PWMVictorSPX(4);
* MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_rearRight);
*
* DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
* }
@@ -44,15 +43,15 @@ import java.util.StringJoiner;
*
* <pre><code>
* public class Robot {
* SpeedController m_frontLeft = new PWMVictorSPX(1);
* SpeedController m_midLeft = new PWMVictorSPX(2);
* SpeedController m_rearLeft = new PWMVictorSPX(3);
* SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
* MotorController m_frontLeft = new PWMVictorSPX(1);
* MotorController m_midLeft = new PWMVictorSPX(2);
* MotorController m_rearLeft = new PWMVictorSPX(3);
* MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
*
* SpeedController m_frontRight = new PWMVictorSPX(4);
* SpeedController m_midRight = new PWMVictorSPX(5);
* SpeedController m_rearRight = new PWMVictorSPX(6);
* SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_midRight, m_rearRight);
* MotorController m_frontRight = new PWMVictorSPX(4);
* MotorController m_midRight = new PWMVictorSPX(5);
* MotorController m_rearRight = new PWMVictorSPX(6);
* MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_midRight, m_rearRight);
*
* DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
* }
@@ -86,15 +85,15 @@ import java.util.StringJoiner;
* value can be changed with {@link #setDeadband}.
*
* <p>RobotDrive porting guide: <br>
* {@link #tankDrive(double, double)} is equivalent to {@link
* edu.wpi.first.wpilibj.RobotDrive#tankDrive(double, double)} if a deadband of 0 is used. <br>
* {@link #arcadeDrive(double, double)} is equivalent to {@link
* edu.wpi.first.wpilibj.RobotDrive#arcadeDrive(double, double)} if a deadband of 0 is used and the
* the rotation input is inverted eg arcadeDrive(y, -rotation) <br>
* {@link #curvatureDrive(double, double, boolean)} is similar in concept to {@link
* edu.wpi.first.wpilibj.RobotDrive#drive(double, double)} with the addition of a quick turn mode.
* However, it is not designed to give exactly the same response.
* {@link #tankDrive(double, double)} is equivalent to RobotDrive's tankDrive(double, double) if a
* deadband of 0 is used. <br>
* {@link #arcadeDrive(double, double)} is equivalent to RobotDrive's arcadeDrive(double, double) if
* a deadband of 0 is used and the the rotation input is inverted eg arcadeDrive(y, -rotation) <br>
* {@link #curvatureDrive(double, double, boolean)} is similar in concept to RobotDrive's
* drive(double, double) with the addition of a quick turn mode. However, it is not designed to give
* exactly the same response.
*/
@SuppressWarnings("removal")
public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
public static final double kDefaultQuickStopThreshold = 0.2;
public static final double kDefaultQuickStopAlpha = 0.1;
@@ -113,8 +112,9 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
/**
* Construct a DifferentialDrive.
*
* <p>To pass multiple motors per side, use a {@link SpeedControllerGroup}. If a motor needs to be
* inverted, do so before passing it in.
* <p>To pass multiple motors per side, use a {@link
* edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup}. If a motor needs to be inverted, do
* so before passing it in.
*/
public DifferentialDrive(SpeedController leftMotor, SpeedController rightMotor) {
verify(leftMotor, rightMotor);

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
View File

@@ -7,11 +7,11 @@ package edu.wpi.first.wpilibj.drive;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpiutil.math.MathUtil;
import java.util.StringJoiner;
/**
@@ -39,6 +39,7 @@ import java.util.StringJoiner;
* points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
* positive.
*/
@SuppressWarnings("removal")
public class KilloughDrive extends RobotDriveBase implements Sendable, AutoCloseable {
public static final double kDefaultLeftMotorAngle = 60.0;
public static final double kDefaultRightMotorAngle = 120.0;

16
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
View File

@@ -7,11 +7,11 @@ package edu.wpi.first.wpilibj.drive;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpiutil.math.MathUtil;
import java.util.StringJoiner;
/**
@@ -50,14 +50,14 @@ import java.util.StringJoiner;
* <p>RobotDrive porting guide: <br>
* In MecanumDrive, the right side speed controllers are automatically inverted, while in
* RobotDrive, no speed controllers are automatically inverted. <br>
* {@link #driveCartesian(double, double, double, double)} is equivalent to {@link
* edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Cartesian(double, double, double, double)} if a
* deadband of 0 is used, and the ySpeed and gyroAngle values are inverted compared to RobotDrive
* (eg driveCartesian(xSpeed, -ySpeed, zRotation, -gyroAngle). <br>
* {@link #drivePolar(double, double, double)} is equivalent to {@link
* edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Polar(double, double, double)} if a deadband of 0
* is used.
* {@link #driveCartesian(double, double, double, double)} is equivalent to RobotDrive's
* mecanumDrive_Cartesian(double, double, double, double) if a deadband of 0 is used, and the ySpeed
* and gyroAngle values are inverted compared to RobotDrive (eg driveCartesian(xSpeed, -ySpeed,
* zRotation, -gyroAngle). <br>
* {@link #drivePolar(double, double, double)} is equivalent to RobotDrive's
* mecanumDrive_Polar(double, double, double)} if a deadband of 0 is used.
*/
@SuppressWarnings("removal")
public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
private static int instances;

54
wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java
View File

@@ -1,54 +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.filters;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
/**
* Superclass for filters.
*
* @deprecated This class is no longer used.
*/
@Deprecated(since = "2020", forRemoval = true)
public abstract class Filter implements PIDSource {
private final PIDSource m_source;
public Filter(PIDSource source) {
m_source = source;
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_source.setPIDSourceType(pidSource);
}
@Override
public PIDSourceType getPIDSourceType() {
return m_source.getPIDSourceType();
}
@Override
public abstract double pidGet();
/**
* Returns the current filter estimate without also inserting new data as pidGet() would do.
*
* @return The current filter estimate
*/
public abstract double get();
/** Reset the filter state. */
public abstract void reset();
/**
* Calls PIDGet() of source.
*
* @return Current value of source
*/
protected double pidGetSource() {
return m_source.pidGet();
}
}

187
wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
View File

@@ -1,187 +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.filters;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpiutil.CircularBuffer;
import java.util.Arrays;
/**
* This class implements a linear, digital filter. All types of FIR and IIR filters are supported.
* Static factory methods are provided to create commonly used types of filters.
*
* <p>Filters are of the form: y[n] = (b0*x[n] + b1*x[n-1] + ... + bP*x[n-P]) - (a0*y[n-1] +
* a2*y[n-2] + ... + aQ*y[n-Q])
*
* <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
* the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
* "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
* front of the feedback term! This is a common convention in signal processing.
*
* <p>What can linear filters do? Basically, they can filter, or diminish, the effects of
* undesirable input frequencies. High frequencies, or rapid changes, can be indicative of sensor
* noise or be otherwise undesirable. A "low pass" filter smooths out the signal, reducing the
* impact of these high frequency components. Likewise, a "high pass" filter gets rid of slow-moving
* signal components, letting you detect large changes more easily.
*
* <p>Example FRC applications of filters: - Getting rid of noise from an analog sensor input (note:
* the roboRIO's FPGA can do this faster in hardware) - Smoothing out joystick input to prevent the
* wheels from slipping or the robot from tipping - Smoothing motor commands so that unnecessary
* strain isn't put on electrical or mechanical components - If you use clever gains, you can make a
* PID controller out of this class!
*
* <p>For more on filters, I highly recommend the following articles: http://en.wikipedia
* .org/wiki/Linear_filter http://en.wikipedia.org/wiki/Iir_filter http://en.wikipedia
* .org/wiki/Fir_filter
*
* <p>Note 1: PIDGet() should be called by the user on a known, regular period. You can set up a
* Notifier to do this (look at the WPILib PIDController class), or do it "inline" with code in a
* periodic function.
*
* <p>Note 2: For ALL filters, gains are necessarily a function of frequency. If you make a filter
* that works well for you at, say, 100Hz, you will most definitely need to adjust the gains if you
* then want to run it at 200Hz! Combining this with Note 1 - the impetus is on YOU as a developer
* to make sure PIDGet() gets called at the desired, constant frequency!
*
* @deprecated Use LinearFilter class instead.
*/
@Deprecated
public class LinearDigitalFilter extends Filter {
private static int instances;
private final CircularBuffer m_inputs;
private final CircularBuffer m_outputs;
private final double[] m_inputGains;
private final double[] m_outputGains;
/**
* Create a linear FIR or IIR filter.
*
* @param source The PIDSource object that is used to get values
* @param ffGains The "feed forward" or FIR gains
* @param fbGains The "feed back" or IIR gains
*/
public LinearDigitalFilter(PIDSource source, double[] ffGains, double[] fbGains) {
super(source);
m_inputs = new CircularBuffer(ffGains.length);
m_outputs = new CircularBuffer(fbGains.length);
m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
instances++;
HAL.report(tResourceType.kResourceType_LinearFilter, instances);
}
/**
* Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
* gain = e^(-dt / T), T is the time constant in seconds.
*
* <p>This filter is stable for time constants greater than zero.
*
* @param source The PIDSource object that is used to get values
* @param timeConstant The discrete-time time constant in seconds
* @param period The period in seconds between samples taken by the user
*/
public static LinearDigitalFilter singlePoleIIR(
PIDSource source, double timeConstant, double period) {
double gain = Math.exp(-period / timeConstant);
double[] ffGains = {1.0 - gain};
double[] fbGains = {-gain};
return new LinearDigitalFilter(source, ffGains, fbGains);
}
/**
* Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
* gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
*
* <p>This filter is stable for time constants greater than zero.
*
* @param source The PIDSource object that is used to get values
* @param timeConstant The discrete-time time constant in seconds
* @param period The period in seconds between samples taken by the user
*/
public static LinearDigitalFilter highPass(PIDSource source, double timeConstant, double period) {
double gain = Math.exp(-period / timeConstant);
double[] ffGains = {gain, -gain};
double[] fbGains = {-gain};
return new LinearDigitalFilter(source, ffGains, fbGains);
}
/**
* Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
* x[0]).
*
* <p>This filter is always stable.
*
* @param source The PIDSource object that is used to get values
* @param taps The number of samples to average over. Higher = smoother but slower
* @throws IllegalArgumentException if number of taps is less than 1
*/
public static LinearDigitalFilter movingAverage(PIDSource source, int taps) {
if (taps <= 0) {
throw new IllegalArgumentException("Number of taps was not at least 1");
}
double[] ffGains = new double[taps];
for (int i = 0; i < ffGains.length; i++) {
ffGains[i] = 1.0 / taps;
}
double[] fbGains = new double[0];
return new LinearDigitalFilter(source, ffGains, fbGains);
}
@Override
public double get() {
double retVal = 0.0;
// Calculate the new value
for (int i = 0; i < m_inputGains.length; i++) {
retVal += m_inputs.get(i) * m_inputGains[i];
}
for (int i = 0; i < m_outputGains.length; i++) {
retVal -= m_outputs.get(i) * m_outputGains[i];
}
return retVal;
}
@Override
public void reset() {
m_inputs.clear();
m_outputs.clear();
}
/**
* Calculates the next value of the filter.
*
* @return The filtered value at this step
*/
@Override
public double pidGet() {
double retVal = 0.0;
// Rotate the inputs
m_inputs.addFirst(pidGetSource());
// Calculate the new value
for (int i = 0; i < m_inputGains.length; i++) {
retVal += m_inputs.get(i) * m_inputGains[i];
}
for (int i = 0; i < m_outputGains.length; i++) {
retVal -= m_outputs.get(i) * m_outputGains[i];
}
// Rotate the outputs
m_outputs.addFirst(retVal);
return retVal;
}
}

7
wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.wpilibj.interfaces;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation2d;
/** Interface for yaw rate gyros. */
public interface Gyro extends AutoCloseable {
@@ -50,7 +50,7 @@ public interface Gyro extends AutoCloseable {
double getRate();
/**
* Return the heading of the robot as a {@link edu.wpi.first.wpilibj.geometry.Rotation2d}.
* 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
@@ -61,8 +61,7 @@ public interface Gyro extends AutoCloseable {
*
* <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.wpilibj.geometry.Rotation2d}.
* @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
*/
default Rotation2d getRotation2d() {
return Rotation2d.fromDegrees(-getAngle());

12
wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java
View File

@@ -1,12 +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.interfaces;
import edu.wpi.first.wpilibj.PIDSource;
/** Interface for a Potentiometer. */
public interface Potentiometer extends PIDSource {
double get();
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/DMC60.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* Digilent DMC 60 Speed Controller.
* Digilent DMC 60 Motor Controller.
*
* <p>Note that the DMC uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -25,7 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.997ms = full "reverse"
* </ul>
*/
public class DMC60 extends PWMSpeedController {
public class DMC60 extends PWMMotorController {
/**
* Constructor.
*
@@ -33,14 +33,13 @@ public class DMC60 extends PWMSpeedController {
* the MXP port
*/
public DMC60(final int channel) {
super(channel);
super("DMC60", channel);
setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_DigilentDMC60, getChannel() + 1);
SendableRegistry.setName(this, "DMC60", getChannel());
}
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Jaguar.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* Texas Instruments / Vex Robotics Jaguar Speed Controller as a PWM device.
* Texas Instruments / Vex Robotics Jaguar Motor Controller as a PWM device.
*
* <p>Note that the Jaguar uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -24,7 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.697ms = full "reverse"
* </ul>
*/
public class Jaguar extends PWMSpeedController {
public class Jaguar extends PWMMotorController {
/**
* Constructor.
*
@@ -32,14 +32,13 @@ public class Jaguar extends PWMSpeedController {
* the MXP port
*/
public Jaguar(final int channel) {
super(channel);
super("Jaguar", channel);
setBounds(2.31, 1.55, 1.507, 1.454, 0.697);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.31, 1.55, 1.507, 1.454, 0.697);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_Jaguar, getChannel() + 1);
SendableRegistry.setName(this, "Jaguar", getChannel());
}
}

11
wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.java Normal file
View File

@@ -0,0 +1,11 @@
// 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.motorcontrol;
import edu.wpi.first.wpilibj.SpeedController;
/** Interface for motor controlling devices. */
@SuppressWarnings("removal")
public interface MotorController extends SpeedController {}

98
wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java Normal file
View File

@@ -0,0 +1,98 @@
// 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.motorcontrol;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import java.util.Arrays;
/** Allows multiple {@link MotorController} objects to be linked together. */
public class MotorControllerGroup implements MotorController, Sendable, AutoCloseable {
private boolean m_isInverted;
private final MotorController[] m_motorControllers;
private static int instances;
/**
* Create a new MotorControllerGroup with the provided MotorControllers.
*
* @param motorControllers The MotorControllers to add
*/
@SuppressWarnings("PMD.AvoidArrayLoops")
public MotorControllerGroup(
MotorController motorController, MotorController... motorControllers) {
m_motorControllers = new MotorController[motorControllers.length + 1];
m_motorControllers[0] = motorController;
for (int i = 0; i < motorControllers.length; i++) {
m_motorControllers[i + 1] = motorControllers[i];
}
init();
}
public MotorControllerGroup(MotorController[] motorControllers) {
m_motorControllers = Arrays.copyOf(motorControllers, motorControllers.length);
init();
}
private void init() {
for (MotorController controller : m_motorControllers) {
SendableRegistry.addChild(this, controller);
}
instances++;
SendableRegistry.addLW(this, "MotorControllerGroup", instances);
}
@Override
public void close() {
SendableRegistry.remove(this);
}
@Override
public void set(double speed) {
for (MotorController motorController : m_motorControllers) {
motorController.set(m_isInverted ? -speed : speed);
}
}
@Override
public double get() {
if (m_motorControllers.length > 0) {
return m_motorControllers[0].get() * (m_isInverted ? -1 : 1);
}
return 0.0;
}
@Override
public void setInverted(boolean isInverted) {
m_isInverted = isInverted;
}
@Override
public boolean getInverted() {
return m_isInverted;
}
@Override
public void disable() {
for (MotorController motorController : m_motorControllers) {
motorController.disable();
}
}
@Override
public void stopMotor() {
for (MotorController motorController : m_motorControllers) {
motorController.stopMotor();
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Motor Controller");
builder.setActuator(true);
builder.setSafeState(this::stopMotor);
builder.addDoubleProperty("Value", this::get, this::set);
}
}

18
wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/NidecBrushless.java
View File

@@ -2,16 +2,20 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.MotorSafety;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/** Nidec Brushless Motor. */
public class NidecBrushless extends MotorSafety
implements SpeedController, Sendable, AutoCloseable {
implements MotorController, Sendable, AutoCloseable {
private boolean m_isInverted;
private final DigitalOutput m_dio;
private final PWM m_pwm;
@@ -89,16 +93,6 @@ public class NidecBrushless extends MotorSafety
return m_isInverted;
}
/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @param output Write out the PWM value as was found in the PIDController
*/
@Override
public void pidWrite(double output) {
set(output);
}
/**
* Stop the motor. This is called by the MotorSafety object when it has a timeout for this PWM and
* needs to stop it from running. Calling set() will re-enable the motor.

57
wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java
View File

@@ -2,27 +2,37 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.wpilibj.MotorSafety;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
/** Common base class for all PWM Speed Controllers. */
public abstract class PWMSpeedController extends PWM implements SpeedController {
/** Common base class for all PWM Motor Controllers. */
public abstract class PWMMotorController extends MotorSafety
implements MotorController, Sendable, AutoCloseable {
private boolean m_isInverted;
protected PWM m_pwm;
/**
* Constructor.
*
* @param name Name to use for SendableRegistry
* @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are
* on the MXP port
*/
protected PWMSpeedController(int channel) {
super(channel);
protected PWMMotorController(final String name, final int channel) {
m_pwm = new PWM(channel, false);
SendableRegistry.addLW(this, name, channel);
}
/** Free the resource associated with the PWM channel and set the value to 0. */
@Override
public String getDescription() {
return "PWM " + getChannel();
public void close() {
SendableRegistry.remove(this);
m_pwm.close();
}
/**
@@ -35,20 +45,20 @@ public abstract class PWMSpeedController extends PWM implements SpeedController
*/
@Override
public void set(double speed) {
setSpeed(m_isInverted ? -speed : speed);
m_pwm.setSpeed(m_isInverted ? -speed : speed);
feed();
}
/**
* Get the recently set value of the PWM. This value is affected by the inversion property. If you
* want the value that is sent directly to the SpeedController, use {@link
* want the value that is sent directly to the MotorController, use {@link
* edu.wpi.first.wpilibj.PWM#getSpeed()} instead.
*
* @return The most recently set value for the PWM between -1.0 and 1.0.
*/
@Override
public double get() {
return getSpeed() * (m_isInverted ? -1.0 : 1.0);
return m_pwm.getSpeed() * (m_isInverted ? -1.0 : 1.0);
}
@Override
@@ -63,24 +73,33 @@ public abstract class PWMSpeedController extends PWM implements SpeedController
@Override
public void disable() {
setDisabled();
m_pwm.setDisabled();
}
@Override
public void stopMotor() {
disable();
}
@Override
public String getDescription() {
return "PWM " + getChannel();
}
/**
* Write out the PID value as seen in the PIDOutput base object.
* Gets the PWM channel number.
*
* @param output Write out the PWM value as was found in the PIDController
* @return The channel number.
*/
@Override
public void pidWrite(double output) {
set(output);
public int getChannel() {
return m_pwm.getChannel();
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Speed Controller");
builder.setSmartDashboardType("Motor Controller");
builder.setActuator(true);
builder.setSafeState(this::setDisabled);
builder.addDoubleProperty("Value", this::getSpeed, this::setSpeed);
builder.setSafeState(this::disable);
builder.addDoubleProperty("Value", this::get, this::set);
}
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkMax.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* REV Robotics SPARK MAX Speed Controller with PWM control.
* REV Robotics SPARK MAX Motor Controller with PWM control.
*
* <p>Note that the SPARK MAX uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -25,17 +25,16 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.999ms = full "reverse"
* </ul>
*/
public class PWMSparkMax extends PWMSpeedController {
public class PWMSparkMax extends PWMMotorController {
/** Common initialization code called by all constructors. */
public PWMSparkMax(final int channel) {
super(channel);
super("PWMSparkMax", channel);
setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_RevSparkMaxPWM, getChannel() + 1);
SendableRegistry.setName(this, "PWMSparkMax", getChannel());
}
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonFX.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* Cross the Road Electronics (CTRE) Talon FX Speed Controller with PWM control.
* Cross the Road Electronics (CTRE) Talon FX Motor Controller with PWM control.
*
* <p>Note that the TalonFX uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -25,7 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.997ms = full "reverse"
* </ul>
*/
public class PWMTalonFX extends PWMSpeedController {
public class PWMTalonFX extends PWMMotorController {
/**
* Constructor for a TalonFX connected via PWM.
*
@@ -33,14 +33,13 @@ public class PWMTalonFX extends PWMSpeedController {
* the MXP port
*/
public PWMTalonFX(final int channel) {
super(channel);
super("PWMTalonFX", channel);
setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_TalonFX, getChannel() + 1);
SendableRegistry.setName(this, "PWMTalonFX", getChannel());
}
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonSRX.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM control.
* Cross the Road Electronics (CTRE) Talon SRX Motor Controller with PWM control.
*
* <p>Note that the TalonSRX uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -25,7 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.997ms = full "reverse"
* </ul>
*/
public class PWMTalonSRX extends PWMSpeedController {
public class PWMTalonSRX extends PWMMotorController {
/**
* Constructor for a TalonSRX connected via PWM.
*
@@ -33,14 +33,13 @@ public class PWMTalonSRX extends PWMSpeedController {
* on the MXP port
*/
public PWMTalonSRX(final int channel) {
super(channel);
super("PWMTalonSRX", channel);
setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_PWMTalonSRX, getChannel() + 1);
SendableRegistry.setName(this, "PWMTalonSRX", getChannel());
}
}

17
wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVenom.java
View File

@@ -2,11 +2,11 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* Playing with Fusion Venom Smart Motor with PWM control.
@@ -24,7 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.997ms = full "reverse"
* </ul>
*/
public class PWMVenom extends PWMSpeedController {
public class PWMVenom extends PWMMotorController {
/**
* Constructor for a Venom connected via PWM.
*
@@ -32,14 +32,13 @@ public class PWMVenom extends PWMSpeedController {
* the MXP port
*/
public PWMVenom(final int channel) {
super(channel);
super("PWMVenom", channel);
setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_FusionVenom, getChannel() + 1);
SendableRegistry.setName(this, "PWMVenom", getChannel());
}
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVictorSPX.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* Cross the Road Electronics (CTRE) Victor SPX Speed Controller with PWM control.
* Cross the Road Electronics (CTRE) Victor SPX Motor Controller with PWM control.
*
* <p>Note that the Victor SPX uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -25,7 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.997ms = full "reverse"
* </ul>
*/
public class PWMVictorSPX extends PWMSpeedController {
public class PWMVictorSPX extends PWMMotorController {
/**
* Constructor for a Victor SPX connected via PWM.
*
@@ -33,14 +33,13 @@ public class PWMVictorSPX extends PWMSpeedController {
* are on the MXP port
*/
public PWMVictorSPX(final int channel) {
super(channel);
super("PWMVictorSPX", channel);
setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_PWMVictorSPX, getChannel() + 1);
SendableRegistry.setName(this, "PWMVictorSPX", getChannel());
}
}

29
wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/SD540.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* Mindsensors SD540 Speed Controller.
* Mindsensors SD540 Motor Controller.
*
* <p>Note that the SD540 uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -25,18 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.94ms = full "reverse"
* </ul>
*/
public class SD540 extends PWMSpeedController {
/** Common initialization code called by all constructors. */
protected void initSD540() {
setBounds(2.05, 1.55, 1.50, 1.44, 0.94);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel() + 1);
SendableRegistry.setName(this, "SD540", getChannel());
}
public class SD540 extends PWMMotorController {
/**
* Constructor.
*
@@ -44,7 +33,13 @@ public class SD540 extends PWMSpeedController {
* the MXP port
*/
public SD540(final int channel) {
super(channel);
initSD540();
super("SD540", channel);
m_pwm.setBounds(2.05, 1.55, 1.50, 1.44, 0.94);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel() + 1);
}
}

29
wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Spark.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* REV Robotics SPARK Speed Controller.
* REV Robotics SPARK Motor Controller.
*
* <p>Note that the SPARK uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -25,18 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.999ms = full "reverse"
* </ul>
*/
public class Spark extends PWMSpeedController {
/** Common initialization code called by all constructors. */
protected void initSpark() {
setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
HAL.report(tResourceType.kResourceType_RevSPARK, getChannel() + 1);
SendableRegistry.setName(this, "Spark", getChannel());
}
public class Spark extends PWMMotorController {
/**
* Constructor.
*
@@ -44,7 +33,13 @@ public class Spark extends PWMSpeedController {
* the MXP port
*/
public Spark(final int channel) {
super(channel);
initSpark();
super("Spark", channel);
m_pwm.setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_RevSPARK, getChannel() + 1);
}
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Talon.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller.
* Cross the Road Electronics (CTRE) Talon and Talon SR Motor Controller.
*
* <p>Note that the Talon uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -24,7 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.989ms = full "reverse"
* </ul>
*/
public class Talon extends PWMSpeedController {
public class Talon extends PWMMotorController {
/**
* Constructor for a Talon (original or Talon SR).
*
@@ -32,14 +32,13 @@ public class Talon extends PWMSpeedController {
* the MXP port
*/
public Talon(final int channel) {
super(channel);
super("Talon", channel);
setBounds(2.037, 1.539, 1.513, 1.487, 0.989);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.037, 1.539, 1.513, 1.487, 0.989);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_Talon, getChannel() + 1);
SendableRegistry.setName(this, "Talon", getChannel());
}
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Victor.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* VEX Robotics Victor 888 Speed Controller The Vex Robotics Victor 884 Speed Controller can also be
* VEX Robotics Victor 888 Motor Controller The Vex Robotics Victor 884 Motor Controller can also be
* used with this class but may need to be calibrated per the Victor 884 user manual.
*
* <p>Note that the Victor uses the following bounds for PWM values. These values were determined
@@ -27,7 +27,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>1.026ms = full "reverse"
* </ul>
*/
public class Victor extends PWMSpeedController {
public class Victor extends PWMMotorController {
/**
* Constructor.
*
@@ -35,14 +35,13 @@ public class Victor extends PWMSpeedController {
* the MXP port
*/
public Victor(final int channel) {
super(channel);
super("Victor", channel);
setBounds(2.027, 1.525, 1.507, 1.49, 1.026);
setPeriodMultiplier(PeriodMultiplier.k2X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.027, 1.525, 1.507, 1.49, 1.026);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k2X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_Victor, getChannel() + 1);
SendableRegistry.setName(this, "Victor", getChannel());
}
}

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java → wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/VictorSP.java
View File

@@ -2,14 +2,14 @@
// 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;
package edu.wpi.first.wpilibj.motorcontrol;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
import edu.wpi.first.wpilibj.PWM;
/**
* VEX Robotics Victor SP Speed Controller.
* VEX Robotics Victor SP Motor Controller.
*
* <p>Note that the VictorSP uses the following bounds for PWM values. These values should work
* reasonably well for most controllers, but if users experience issues such as asymmetric behavior
@@ -25,7 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
* <li>0.997ms = full "reverse"
* </ul>
*/
public class VictorSP extends PWMSpeedController {
public class VictorSP extends PWMMotorController {
/**
* Constructor.
*
@@ -33,14 +33,13 @@ public class VictorSP extends PWMSpeedController {
* the MXP port
*/
public VictorSP(final int channel) {
super(channel);
super("VictorSP", channel);
setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
setPeriodMultiplier(PeriodMultiplier.k1X);
setSpeed(0.0);
setZeroLatch();
m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
m_pwm.setSpeed(0.0);
m_pwm.setZeroLatch();
HAL.report(tResourceType.kResourceType_VictorSP, getChannel() + 1);
SendableRegistry.setName(this, "VictorSP", getChannel());
}
}

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

@@ -245,27 +245,27 @@ public enum BuiltInWidgets implements WidgetType {
*/
kEncoder("Encoder"),
/**
* Displays a {@link edu.wpi.first.wpilibj.SpeedController SpeedController}. The speed controller
* will be controllable from the dashboard when test mode is enabled, but will otherwise be
* view-only. <br>
* Displays a {@link edu.wpi.first.wpilibj.motorcontrol.MotorController MotorController}. The
* speed controller will be controllable from the dashboard when test mode is enabled, but will
* otherwise be view-only. <br>
* Supported types:
*
* <ul>
* <li>{@link edu.wpi.first.wpilibj.PWMSpeedController}
* <li>{@link edu.wpi.first.wpilibj.DMC60}
* <li>{@link edu.wpi.first.wpilibj.Jaguar}
* <li>{@link edu.wpi.first.wpilibj.PWMSparkMax}
* <li>{@link edu.wpi.first.wpilibj.PWMTalonFX}
* <li>{@link edu.wpi.first.wpilibj.PWMTalonSRX}
* <li>{@link edu.wpi.first.wpilibj.PWMVenom}
* <li>{@link edu.wpi.first.wpilibj.PWMVictorSPX}
* <li>{@link edu.wpi.first.wpilibj.SD540}
* <li>{@link edu.wpi.first.wpilibj.Spark}
* <li>{@link edu.wpi.first.wpilibj.Talon}
* <li>{@link edu.wpi.first.wpilibj.Victor}
* <li>{@link edu.wpi.first.wpilibj.VictorSP}
* <li>{@link edu.wpi.first.wpilibj.SpeedControllerGroup}
* <li>Any custom subclass of {@code SpeedController}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMMotorController}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.DMC60}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.Jaguar}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMTalonFX}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMTalonSRX}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMVenom}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.SD540}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.Spark}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.Talon}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.Victor}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.VictorSP}
* <li>{@link edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup}
* <li>Any custom subclass of {@code MotorController}
* </ul>
*
* <br>
@@ -277,7 +277,7 @@ public enum BuiltInWidgets implements WidgetType {
* <td>One of {@code ["HORIZONTAL", "VERTICAL"]}</td></tr>
* </table>
*/
kSpeedController("Speed Controller"),
kMotorController("Motor Controller"),
/**
* Displays a command with a toggle button. Pressing the button will start the command, and the
* button will automatically release when the command completes. <br>
@@ -453,7 +453,7 @@ public enum BuiltInWidgets implements WidgetType {
* Supported types:
*
* <ul>
* <li>{@link edu.wpi.cscore.VideoSource} (as long as it is streaming on an MJPEG server)
* <li>{@link edu.wpi.first.cscore.VideoSource} (as long as it is streaming on an MJPEG server)
* </ul>
*
* <br>

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.wpilibj.shuffleboard;
import edu.wpi.cscore.VideoSource;
import edu.wpi.first.cscore.VideoSource;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.wpilibj.shuffleboard;
import edu.wpi.cscore.VideoSource;
import edu.wpi.first.cscore.VideoSource;
import edu.wpi.first.wpilibj.Sendable;
import java.util.List;
import java.util.NoSuchElementException;

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java
View File

@@ -5,8 +5,8 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.AnalogEncoder;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
/** Class to control a simulated analog encoder. */
public class AnalogEncoderSim {

32
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -4,21 +4,21 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.math.StateSpaceUtil;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N7;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.util.Units;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import edu.wpi.first.wpiutil.math.numbers.N7;
/**
* This class simulates the state of the drivetrain. In simulationPeriodic, users should first set
@@ -101,9 +101,9 @@ public class DifferentialDrivetrainSim {
*
* @param drivetrainPlant The {@link LinearSystem} representing the robot's drivetrain. This
* system can be created with {@link
* edu.wpi.first.wpilibj.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor,
* edu.wpi.first.math.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor,
* double, double, double, double, double)} or {@link
* edu.wpi.first.wpilibj.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double,
* edu.wpi.first.math.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double,
* double, double)}.
* @param driveMotor A {@link DCMotor} representing the drivetrain.
* @param gearing The gearingRatio ratio of the robot, as output over input. This must be the same

16
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -4,14 +4,14 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
/** Represents a simulated elevator mechanism. */
public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {

12
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
View File

@@ -4,12 +4,12 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.util.Units;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
/** Represents a simulated flywheel mechanism. */
public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
View File

@@ -4,12 +4,12 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.math.StateSpaceUtil;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.math.StateSpaceUtil;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
import org.ejml.MatrixDimensionException;
import org.ejml.simple.SimpleMatrix;

19
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java
View File

@@ -13,7 +13,6 @@ import edu.wpi.first.hal.SimValue;
import edu.wpi.first.hal.simulation.SimDeviceCallback;
import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
import edu.wpi.first.hal.simulation.SimValueCallback;
import edu.wpi.first.hal.simulation.SimValueCallback2;
/** Class to control the simulation side of a SimDevice. */
public class SimDeviceSim {
@@ -191,22 +190,6 @@ public class SimDeviceSim {
return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueChangedCallback);
}
/**
* Register a callback to be run every time a value is changed on this device.
*
* @param callback the callback
* @param initialNotify should the callback be run with the initial state
* @return the {@link CallbackStore} object associated with this callback. Save a reference to
* this object so GC doesn't cancel the callback.
*/
public CallbackStore registerValueChangedCallback2(
SimValue value, SimValueCallback2 callback, boolean initialNotify) {
int uid =
SimDeviceDataJNI.registerSimValueChangedCallback2(
value.getNativeHandle(), callback, initialNotify);
return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueChangedCallback);
}
/**
* Register a callback for SimDouble.reset() and similar functions. The callback is called with
* the old value.
@@ -216,7 +199,7 @@ public class SimDeviceSim {
* @param initialNotify ignored (present for consistency)
*/
public CallbackStore registerValueResetCallback(
SimValue value, SimValueCallback2 callback, boolean initialNotify) {
SimValue value, SimValueCallback callback, boolean initialNotify) {
int uid =
SimDeviceDataJNI.registerSimValueResetCallback(
value.getNativeHandle(), callback, initialNotify);

16
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -4,14 +4,14 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.NumericalIntegration;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
/** Represents a simulated single jointed arm mechanism. */
public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java
View File

@@ -4,10 +4,10 @@
package edu.wpi.first.wpilibj.smartdashboard;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.ArrayList;
import java.util.List;

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
View File

@@ -4,11 +4,11 @@
package edu.wpi.first.wpilibj.smartdashboard;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java
View File

@@ -4,9 +4,9 @@
package edu.wpi.first.wpilibj.smartdashboard;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.util.Color8Bit;
/**

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.wpilibj.util;
import edu.wpi.first.wpiutil.math.MathUtil;
import edu.wpi.first.math.MathUtil;
import java.util.Objects;
/**

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.wpilibj.util;
import edu.wpi.first.wpiutil.math.MathUtil;
import edu.wpi.first.math.MathUtil;
import java.util.Objects;
/** Represents colors with 8 bits of precision. */

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java
View File

@@ -4,9 +4,9 @@
package edu.wpi.first.wpilibj.vision;
import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.VideoSource;
import edu.wpi.first.cameraserver.CameraServerSharedStore;
import edu.wpi.first.cscore.CvSink;
import edu.wpi.first.cscore.VideoSource;
import org.opencv.core.Mat;
/**

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.wpilibj.vision;
import edu.wpi.cscore.VideoSource;
import edu.wpi.first.cscore.VideoSource;
/**
* A vision thread is a special thread that runs a vision pipeline. It is a <i>daemon</i> thread; it

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java
View File

@@ -13,7 +13,7 @@
* implements VisionRunner.Listener&lt;MyFindTotePipeline&gt; {
*
* // A USB camera connected to the roboRIO.
* private {@link edu.wpi.cscore.VideoSource VideoSource} usbCamera;
* private {@link edu.wpi.first.cscore.VideoSource VideoSource} usbCamera;
*
* // A vision pipeline. This could be handwritten or generated by GRIP.
* // This has to implement {@link edu.wpi.first.wpilibj.vision.VisionPipeline}.