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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2024-12-14 10:45:32 -08:00
parent 6ba7189373 4bca79b9af
commit 945d416d07
43 changed files with 768 additions and 1278 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
View File

@@ -102,10 +102,9 @@ public class HolonomicDriveController {
m_poseError = trajectoryPose.relativeTo(currentPose);
m_rotationError = desiredHeading.minus(currentPose.getRotation());
ChassisSpeeds speeds = new ChassisSpeeds(xFF, yFF, thetaFF);
if (!m_enabled) {
speeds.toRobotRelativeSpeeds(currentPose.getRotation());
return speeds;
return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
}
// Calculate feedback velocities (based on position error).
@@ -113,10 +112,8 @@ public class HolonomicDriveController {
double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
// Return next output.
speeds.vxMetersPerSecond += xFeedback;
speeds.vyMetersPerSecond += yFeedback;
speeds.toRobotRelativeSpeeds(currentPose.getRotation());
return speeds;
return ChassisSpeeds.fromFieldRelativeSpeeds(
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation());
}
/**

77
wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View File

@@ -103,9 +103,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @param omegaRadiansPerSecond Angular velocity.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
* @deprecated Use instance method instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(
double vxMetersPerSecond,
double vyMetersPerSecond,
@@ -143,9 +141,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @param omega Angular velocity.
* @param dt The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
* @deprecated Use instance method instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) {
return discretize(
@@ -166,9 +162,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @param continuousSpeeds The continuous speeds.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
* @deprecated Use instance method instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
return discretize(
continuousSpeeds.vxMetersPerSecond,
@@ -177,36 +171,6 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
dtSeconds);
}
/**
* Discretizes a continuous-time chassis speed.
*
* <p>This function converts this continuous-time chassis speed into a discrete-time one such that
* when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
* velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
* along the y-axis, and omega * dt around the z-axis).
*
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void discretize(double dtSeconds) {
var desiredDeltaPose =
new Pose2d(
vxMetersPerSecond * dtSeconds,
vyMetersPerSecond * dtSeconds,
new Rotation2d(omegaRadiansPerSecond * dtSeconds));
// Find the chassis translation/rotation deltas in the robot frame that move the robot from its
// current pose to the desired pose
var twist = Pose2d.kZero.log(desiredDeltaPose);
// Turn the chassis translation/rotation deltas into average velocities
vxMetersPerSecond = twist.dx / dtSeconds;
vyMetersPerSecond = twist.dy / dtSeconds;
omegaRadiansPerSecond = twist.dtheta / dtSeconds;
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
@@ -220,9 +184,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
* @deprecated Use toRobotRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
@@ -247,9 +209,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
* @deprecated Use toRobotRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
@@ -267,9 +227,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
* @deprecated Use toRobotRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
@@ -279,21 +237,6 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
robotAngle);
}
/**
* Converts this field-relative set of speeds into a robot-relative ChassisSpeeds object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
*/
public void toRobotRelativeSpeeds(Rotation2d robotAngle) {
// CW rotation into chassis frame
var rotated =
new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
vxMetersPerSecond = rotated.getX();
vyMetersPerSecond = rotated.getY();
}
/**
* Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
* object.
@@ -307,9 +250,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
* @deprecated Use toFieldRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
@@ -333,9 +274,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
* @deprecated Use toFieldRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
@@ -353,9 +292,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
* @deprecated Use toFieldRelativeSpeeds instead.
*/
@Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
@@ -365,20 +302,6 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
robotAngle);
}
/**
* Converts this robot-relative set of speeds into a field-relative ChassisSpeeds object.
*
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
*/
public void toFieldRelativeSpeeds(Rotation2d robotAngle) {
// CCW rotation out of chassis frame
var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
vxMetersPerSecond = rotated.getX();
vyMetersPerSecond = rotated.getY();
}
/**
* Adds two ChassisSpeeds and returns the sum.
*

15
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
View File

@@ -4,13 +4,9 @@
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.kinematics.proto.MecanumDriveMotorVoltagesProto;
import edu.wpi.first.math.kinematics.struct.MecanumDriveMotorVoltagesStruct;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
/** Represents the motor voltages for a mecanum drive drivetrain. */
public class MecanumDriveMotorVoltages implements ProtobufSerializable, StructSerializable {
@Deprecated(since = "2025", forRemoval = true)
public class MecanumDriveMotorVoltages {
/** Voltage of the front left motor. */
public double frontLeftVoltage;
@@ -52,11 +48,4 @@ public class MecanumDriveMotorVoltages implements ProtobufSerializable, StructSe
+ "Rear Left: %.2f V, Rear Right: %.2f V)",
frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage);
}
/** MecanumDriveMotorVoltages struct for serialization. */
public static final MecanumDriveMotorVoltagesStruct struct =
new MecanumDriveMotorVoltagesStruct();
/** MecanumDriveMotorVoltages protobuf for serialization. */
public static final MecanumDriveMotorVoltagesProto proto = new MecanumDriveMotorVoltagesProto();
}

42
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProto.java
View File

@@ -1,42 +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.math.kinematics.proto;
import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveMotorVoltages;
import edu.wpi.first.util.protobuf.Protobuf;
import us.hebi.quickbuf.Descriptors.Descriptor;
public final class MecanumDriveMotorVoltagesProto
implements Protobuf<MecanumDriveMotorVoltages, ProtobufMecanumDriveMotorVoltages> {
@Override
public Class<MecanumDriveMotorVoltages> getTypeClass() {
return MecanumDriveMotorVoltages.class;
}
@Override
public Descriptor getDescriptor() {
return ProtobufMecanumDriveMotorVoltages.getDescriptor();
}
@Override
public ProtobufMecanumDriveMotorVoltages createMessage() {
return ProtobufMecanumDriveMotorVoltages.newInstance();
}
@Override
public MecanumDriveMotorVoltages unpack(ProtobufMecanumDriveMotorVoltages msg) {
return new MecanumDriveMotorVoltages(
msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight());
}
@Override
public void pack(ProtobufMecanumDriveMotorVoltages msg, MecanumDriveMotorVoltages value) {
msg.setFrontLeft(value.frontLeftVoltage);
msg.setFrontRight(value.frontRightVoltage);
msg.setRearLeft(value.rearLeftVoltage);
msg.setRearRight(value.rearRightVoltage);
}
}

48
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStruct.java
View File

@@ -1,48 +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.math.kinematics.struct;
import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
import edu.wpi.first.util.struct.Struct;
import java.nio.ByteBuffer;
public final class MecanumDriveMotorVoltagesStruct implements Struct<MecanumDriveMotorVoltages> {
@Override
public Class<MecanumDriveMotorVoltages> getTypeClass() {
return MecanumDriveMotorVoltages.class;
}
@Override
public String getTypeName() {
return "MecanumDriveMotorVoltages";
}
@Override
public int getSize() {
return kSizeDouble * 4;
}
@Override
public String getSchema() {
return "double front_left;double front_right;double rear_left;double rear_right";
}
@Override
public MecanumDriveMotorVoltages unpack(ByteBuffer bb) {
double front_left = bb.getDouble();
double front_right = bb.getDouble();
double rear_left = bb.getDouble();
double rear_right = bb.getDouble();
return new MecanumDriveMotorVoltages(front_left, front_right, rear_left, rear_right);
}
@Override
public void pack(ByteBuffer bb, MecanumDriveMotorVoltages value) {
bb.putDouble(value.frontLeftVoltage);
bb.putDouble(value.frontRightVoltage);
bb.putDouble(value.rearLeftVoltage);
bb.putDouble(value.rearRightVoltage);
}
}

35
wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
View File

@@ -8,8 +8,9 @@ import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
import java.util.function.BiFunction;
import java.util.function.DoubleFunction;
import java.util.function.Function;
import java.util.function.DoubleBinaryOperator;
import java.util.function.DoubleUnaryOperator;
import java.util.function.UnaryOperator;
/** Numerical integration utilities. */
public final class NumericalIntegration {
@@ -25,13 +26,12 @@ public final class NumericalIntegration {
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x) for dt.
*/
@SuppressWarnings("overloads")
public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
public static double rk4(DoubleUnaryOperator f, double x, double dtSeconds) {
final var h = dtSeconds;
final var k1 = f.apply(x);
final var k2 = f.apply(x + h * k1 * 0.5);
final var k3 = f.apply(x + h * k2 * 0.5);
final var k4 = f.apply(x + h * k3);
final var k1 = f.applyAsDouble(x);
final var k2 = f.applyAsDouble(x + h * k1 * 0.5);
final var k3 = f.applyAsDouble(x + h * k2 * 0.5);
final var k4 = f.applyAsDouble(x + h * k3);
return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
@@ -45,15 +45,13 @@ public final class NumericalIntegration {
* @param dtSeconds The time over which to integrate.
* @return The result of Runge Kutta integration (4th order).
*/
@SuppressWarnings("overloads")
public static double rk4(
BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
public static double rk4(DoubleBinaryOperator f, double x, double u, double dtSeconds) {
final var h = dtSeconds;
final var k1 = f.apply(x, u);
final var k2 = f.apply(x + h * k1 * 0.5, u);
final var k3 = f.apply(x + h * k2 * 0.5, u);
final var k4 = f.apply(x + h * k3, u);
final var k1 = f.applyAsDouble(x, u);
final var k2 = f.applyAsDouble(x + h * k1 * 0.5, u);
final var k3 = f.applyAsDouble(x + h * k2 * 0.5, u);
final var k4 = f.applyAsDouble(x + h * k3, u);
return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
@@ -69,7 +67,6 @@ public final class NumericalIntegration {
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@SuppressWarnings("overloads")
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
@@ -94,9 +91,8 @@ public final class NumericalIntegration {
* @param dtSeconds The time over which to integrate.
* @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*/
@SuppressWarnings("overloads")
public static <States extends Num> Matrix<States, N1> rk4(
Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
UnaryOperator<Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
final var h = dtSeconds;
Matrix<States, N1> k1 = f.apply(x);
@@ -145,7 +141,6 @@ public final class NumericalIntegration {
* @param dtSeconds The time over which to integrate.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@SuppressWarnings("overloads")
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
@@ -166,7 +161,6 @@ public final class NumericalIntegration {
* @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@SuppressWarnings("overloads")
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
@@ -291,7 +285,6 @@ public final class NumericalIntegration {
* @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@SuppressWarnings("overloads")
public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rkdp(
BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
double t,