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 'main' into 2022

Browse Source
This commit is contained in:
Peter Johnson
2021-04-19 18:45:31 -07:00
parent 8d961dfd25 aaf24e2552
commit 05ebe93180
44 changed files with 1762 additions and 702 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
View File

@@ -23,6 +23,15 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
protected SimDevice m_simDevice;
protected SimDouble m_simPosition;
/**
* Construct a new AnalogEncoder attached to a specific AnalogIn channel.
*
* @param channel the analog input channel to attach to
*/
public AnalogEncoder(int channel) {
this(new AnalogInput(channel));
}
/**
* Construct a new AnalogEncoder attached to a specific AnalogInput.
*

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

@@ -205,15 +205,8 @@ public class Compressor implements Sendable, AutoCloseable {
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Compressor");
builder.addBooleanProperty(
"Enabled",
this::enabled,
value -> {
if (value) {
start();
} else {
stop();
}
});
"Closed Loop Control", this::getClosedLoopControl, this::setClosedLoopControl);
builder.addBooleanProperty("Enabled", this::enabled, null);
builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null);
}
}

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

@@ -31,6 +31,8 @@ public class HolonomicDriveController {
private final PIDController m_yController;
private final ProfiledPIDController m_thetaController;
private boolean m_firstRun = true;
/**
* Constructs a holonomic drive controller.
*
@@ -82,6 +84,13 @@ public class HolonomicDriveController {
@SuppressWarnings("LocalVariableName")
public ChassisSpeeds calculate(
Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
// If this is the first run, then we need to reset the theta controller to the current pose's
// heading.
if (m_firstRun) {
m_thetaController.reset(currentPose.getRotation().getRadians());
m_firstRun = false;
}
// Calculate feedforward velocities (field-relative).
double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();

157
wpilibj/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java
View File

@@ -1,157 +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.romi;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
public class RomiGyro implements Gyro, Sendable {
private final SimDevice m_simDevice;
private SimDouble m_simRateX;
private SimDouble m_simRateY;
private SimDouble m_simRateZ;
private SimDouble m_simAngleX;
private SimDouble m_simAngleY;
private SimDouble m_simAngleZ;
private double m_angleXOffset;
private double m_angleYOffset;
private double m_angleZOffset;
/** Create a new RomiGyro. */
public RomiGyro() {
m_simDevice = SimDevice.create("Gyro:RomiGyro");
if (m_simDevice != null) {
m_simDevice.createBoolean("init", Direction.kOutput, true);
m_simRateX = m_simDevice.createDouble("rate_x", Direction.kInput, 0.0);
m_simRateY = m_simDevice.createDouble("rate_y", Direction.kInput, 0.0);
m_simRateZ = m_simDevice.createDouble("rate_z", Direction.kInput, 0.0);
m_simAngleX = m_simDevice.createDouble("angle_x", Direction.kInput, 0.0);
m_simAngleY = m_simDevice.createDouble("angle_y", Direction.kInput, 0.0);
m_simAngleZ = m_simDevice.createDouble("angle_z", Direction.kInput, 0.0);
}
}
/**
* Get the rate of turn in degrees-per-second around the X-axis.
*
* @return rate of turn in degrees-per-second
*/
public double getRateX() {
if (m_simRateX != null) {
return m_simRateX.get();
}
return 0.0;
}
/**
* Get the rate of turn in degrees-per-second around the Y-axis.
*
* @return rate of turn in degrees-per-second
*/
public double getRateY() {
if (m_simRateY != null) {
return m_simRateY.get();
}
return 0.0;
}
/**
* Get the rate of turn in degrees-per-second around the Z-axis.
*
* @return rate of turn in degrees-per-second
*/
public double getRateZ() {
if (m_simRateZ != null) {
return m_simRateZ.get();
}
return 0.0;
}
/**
* Get the currently reported angle around the X-axis.
*
* @return current angle around X-axis in degrees
*/
public double getAngleX() {
if (m_simAngleX != null) {
return m_simAngleX.get() - m_angleXOffset;
}
return 0.0;
}
/**
* Get the currently reported angle around the X-axis.
*
* @return current angle around Y-axis in degrees
*/
public double getAngleY() {
if (m_simAngleY != null) {
return m_simAngleY.get() - m_angleYOffset;
}
return 0.0;
}
/**
* Get the currently reported angle around the Z-axis.
*
* @return current angle around Z-axis in degrees
*/
public double getAngleZ() {
if (m_simAngleZ != null) {
return m_simAngleZ.get() - m_angleZOffset;
}
return 0.0;
}
/** Reset the gyro angles to 0. */
@Override
public void reset() {
if (m_simAngleX != null) {
m_angleXOffset = m_simAngleX.get();
m_angleYOffset = m_simAngleY.get();
m_angleZOffset = m_simAngleZ.get();
}
}
@Override
public void calibrate() {
// no-op - Romi Gyro calibration is done via web UI
}
@Override
public double getAngle() {
return getAngleZ();
}
@Override
public double getRate() {
return getRateZ();
}
@Override
public void close() throws Exception {
if (m_simDevice != null) {
m_simDevice.close();
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}

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

@@ -8,6 +8,9 @@ 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;
import java.util.List;
@@ -82,6 +85,19 @@ public class FieldObject2d {
updateEntry();
}
/**
* Sets poses from a trajectory.
*
* @param trajectory The trajectory from which the poses should be added.
*/
public synchronized void setTrajectory(Trajectory trajectory) {
m_poses.clear();
for (Trajectory.State state : trajectory.getStates()) {
m_poses.add(state.poseMeters);
}
updateEntry();
}
/**
* Get multiple poses.
*
@@ -101,20 +117,39 @@ public class FieldObject2d {
return;
}
double[] arr = new double[m_poses.size() * 3];
int ndx = 0;
for (Pose2d pose : m_poses) {
Translation2d translation = pose.getTranslation();
arr[ndx + 0] = translation.getX();
arr[ndx + 1] = translation.getY();
arr[ndx + 2] = pose.getRotation().getDegrees();
ndx += 3;
}
if (m_poses.size() < (255 / 3)) {
double[] arr = new double[m_poses.size() * 3];
int ndx = 0;
for (Pose2d pose : m_poses) {
Translation2d translation = pose.getTranslation();
arr[ndx + 0] = translation.getX();
arr[ndx + 1] = translation.getY();
arr[ndx + 2] = pose.getRotation().getDegrees();
ndx += 3;
}
if (setDefault) {
m_entry.setDefaultDoubleArray(arr);
if (setDefault) {
m_entry.setDefaultDoubleArray(arr);
} else {
m_entry.setDoubleArray(arr);
}
} else {
m_entry.setDoubleArray(arr);
// send as raw array of doubles if too big for NT array
ByteBuffer output = ByteBuffer.allocate(m_poses.size() * 3 * 8);
output.order(ByteOrder.BIG_ENDIAN);
for (Pose2d pose : m_poses) {
Translation2d translation = pose.getTranslation();
output.putDouble(translation.getX());
output.putDouble(translation.getY());
output.putDouble(pose.getRotation().getDegrees());
}
if (setDefault) {
m_entry.setDefaultRaw(output.array());
} else {
m_entry.forceSetRaw(output.array());
}
}
}
@@ -125,17 +160,36 @@ public class FieldObject2d {
}
double[] arr = m_entry.getDoubleArray((double[]) null);
if (arr == null) {
return;
}
if (arr != null) {
if ((arr.length % 3) != 0) {
return;
}
if ((arr.length % 3) != 0) {
return;
}
m_poses.clear();
for (int i = 0; i < arr.length; i += 3) {
m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
}
} else {
// read as raw array of doubles
byte[] data = m_entry.getRaw((byte[]) null);
if (data == null) {
return;
}
m_poses.clear();
for (int i = 0; i < arr.length; i += 3) {
m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
// must be triples of doubles
if ((data.length % (3 * 8)) != 0) {
return;
}
ByteBuffer input = ByteBuffer.wrap(data);
input.order(ByteOrder.BIG_ENDIAN);
m_poses.clear();
for (int i = 0; i < (data.length / (3 * 8)); i++) {
double x = input.getDouble();
double y = input.getDouble();
double rot = input.getDouble();
m_poses.add(new Pose2d(x, y, Rotation2d.fromDegrees(rot)));
}
}
}