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

[docs] Add missing JavaDocs (#6146)

Browse Source
This commit is contained in:
Tyler Veness
2024-01-04 08:38:06 -08:00
committed by GitHub
parent 6e58db398d
commit f29a7d2e50
145 changed files with 1106 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.math;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
/** Computer vision utility functions. */
public final class ComputerVisionUtil {
private ComputerVisionUtil() {
throw new AssertionError("utility class");

1
wpimath/src/main/java/edu/wpi/first/math/DARE.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math;
import org.ejml.simple.SimpleMatrix;
/** DARE solver utility functions. */
public final class DARE {
private DARE() {
throw new UnsupportedOperationException("This is a utility class!");

3
wpimath/src/main/java/edu/wpi/first/math/InterpolatingMatrixTreeMap.java
View File

@@ -17,6 +17,9 @@ import java.util.TreeMap;
public class InterpolatingMatrixTreeMap<K extends Number, R extends Num, C extends Num> {
private final TreeMap<K, Matrix<R, C>> m_map = new TreeMap<>();
/** Default constructor. */
public InterpolatingMatrixTreeMap() {}
/**
* Inserts a key-value pair.
*

3
wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
View File

@@ -40,7 +40,10 @@ public class MatBuilder<R extends Num, C extends Num> {
return new Matrix<>(new SimpleMatrix(rows.getNum(), cols.getNum(), true, data));
}
/** Number of rows. */
protected final Nat<R> m_rows;
/** Number of columns. */
protected final Nat<C> m_cols;
/**

1
wpimath/src/main/java/edu/wpi/first/math/MathShared.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math;
/** WPIMath utility functions. */
public interface MathShared {
/**
* Report an error.

1
wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math;
import edu.wpi.first.util.WPIUtilJNI;
/** Storage for MathShared object. */
public final class MathSharedStore {
private static MathShared mathShared;

20
wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
View File

@@ -4,15 +4,35 @@
package edu.wpi.first.math;
/** WPIMath usage reporting IDs. */
public enum MathUsageId {
/** DifferentialDriveKinematics. */
kKinematics_DifferentialDrive,
/** MecanumDriveKinematics. */
kKinematics_MecanumDrive,
/** SwerveDriveKinematics. */
kKinematics_SwerveDrive,
/** TrapezoidProfile. */
kTrajectory_TrapezoidProfile,
/** LinearFilter. */
kFilter_Linear,
/** DifferentialDriveOdometry. */
kOdometry_DifferentialDrive,
/** SwerveDriveOdometry. */
kOdometry_SwerveDrive,
/** MecanumDriveOdometry. */
kOdometry_MecanumDrive,
/** PIDController. */
kController_PIDController2,
/** ProfiledPIDController. */
kController_ProfiledPIDController,
}

1
wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math;
/** Math utility functions. */
public final class MathUtil {
private MathUtil() {
throw new AssertionError("utility class");

1
wpimath/src/main/java/edu/wpi/first/math/Matrix.java
View File

@@ -24,6 +24,7 @@ import org.ejml.simple.SimpleMatrix;
* @param <C> The number of columns in this matrix.
*/
public class Matrix<R extends Num, C extends Num> {
/** Storage for underlying EJML matrix. */
protected final SimpleMatrix m_storage;
/**

3
wpimath/src/main/java/edu/wpi/first/math/Num.java
View File

@@ -6,6 +6,9 @@ package edu.wpi.first.math;
/** A number expressed as a java class. */
public abstract class Num {
/** Default constructor. */
public Num() {}
/**
* The number this is backing.
*

31
wpimath/src/main/java/edu/wpi/first/math/Pair.java
View File

@@ -4,23 +4,54 @@
package edu.wpi.first.math;
/**
* Represents a pair of two objects.
*
* @param <A> The first object's type.
* @param <B> The second object's type.
*/
public class Pair<A, B> {
private final A m_first;
private final B m_second;
/**
* Constructs a pair.
*
* @param first The first object.
* @param second The second object.
*/
public Pair(A first, B second) {
m_first = first;
m_second = second;
}
/**
* Returns the first object.
*
* @return The first object.
*/
public A getFirst() {
return m_first;
}
/**
* Returns the second object.
*
* @return The second object.
*/
public B getSecond() {
return m_second;
}
/**
* Returns a pair comprised of the two given objects.
*
* @param <A> The first object's type.
* @param <B> The second object's type.
* @param a The first object.
* @param b The second object.
* @return A pair comprised of the two given objects.
*/
public static <A, B> Pair<A, B> of(A a, B b) {
return new Pair<>(a, b);
}

1
wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
View File

@@ -11,6 +11,7 @@ import edu.wpi.first.math.numbers.N4;
import java.util.Random;
import org.ejml.simple.SimpleMatrix;
/** State-space utilities. */
public final class StateSpaceUtil {
private static Random rand = new Random();

10
wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
View File

@@ -14,12 +14,22 @@ import edu.wpi.first.util.struct.StructSerializable;
* against the force of gravity on a beam suspended at an angle).
*/
public class ArmFeedforward implements ProtobufSerializable, StructSerializable {
/** The static gain, in volts. */
public final double ks;
/** The gravity gain, in volts. */
public final double kg;
/** The velocity gain, in volt seconds per radian. */
public final double kv;
/** The acceleration gain, in volt seconds² per radian. */
public final double ka;
/** Arm feedforward protobuf for serialization. */
public static final ArmFeedforwardProto proto = new ArmFeedforwardProto();
/** Arm feedforward struct for serialization. */
public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct();
/**

13
wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java
View File

@@ -11,16 +11,29 @@ import edu.wpi.first.util.struct.StructSerializable;
/** Motor voltages for a differential drive. */
public class DifferentialDriveWheelVoltages implements ProtobufSerializable, StructSerializable {
/** Left wheel voltage. */
public double left;
/** Right wheel voltage. */
public double right;
/** DifferentialDriveWheelVoltages protobuf for serialization. */
public static final DifferentialDriveWheelVoltagesProto proto =
new DifferentialDriveWheelVoltagesProto();
/** DifferentialDriveWheelVoltages struct for serialization. */
public static final DifferentialDriveWheelVoltagesStruct struct =
new DifferentialDriveWheelVoltagesStruct();
/** Default constructor. */
public DifferentialDriveWheelVoltages() {}
/**
* Constructs a DifferentialDriveWheelVoltages.
*
* @param left Left wheel voltage.
* @param right Right wheel voltage.
*/
public DifferentialDriveWheelVoltages(double left, double right) {
this.left = left;
this.right = right;

10
wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
View File

@@ -17,12 +17,22 @@ import edu.wpi.first.util.struct.StructSerializable;
* against the force of gravity).
*/
public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable {
/** The static gain. */
public final double ks;
/** The gravity gain. */
public final double kg;
/** The velocity gain. */
public final double kv;
/** The acceleration gain. */
public final double ka;
/** ElevatorFeedforward protobuf for serialization. */
public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
/** ElevatorFeedforward struct for serialization. */
public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
/**

5
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -10,8 +10,13 @@ import edu.wpi.first.math.system.plant.LinearSystemId;
/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
public class SimpleMotorFeedforward {
/** The static gain. */
public final double ks;
/** The velocity gain. */
public final double kv;
/** The acceleration gain. */
public final double ka;
/**

15
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
View File

@@ -13,11 +13,19 @@ import java.util.List;
import java.util.Map;
import java.util.function.BiConsumer;
/**
* This class incorporates time-delayed measurements into a Kalman filter's state estimate.
*
* @param <S> The number of states.
* @param <I> The number of inputs.
* @param <O> The number of outputs.
*/
public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O extends Num> {
private static final int kMaxPastObserverStates = 300;
private final List<Map.Entry<Double, ObserverSnapshot>> m_pastObserverSnapshots;
/** Default constructor. */
KalmanFilterLatencyCompensator() {
m_pastObserverSnapshots = new ArrayList<>();
}
@@ -164,9 +172,16 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
/** This class contains all the information about our observer at a given time. */
public class ObserverSnapshot {
/** The state estimate. */
public final Matrix<S, N1> xHat;
/** The error covariance. */
public final Matrix<S, S> errorCovariances;
/** The inputs. */
public final Matrix<I, N1> inputs;
/** The local measurements. */
public final Matrix<O, N1> localMeasurements;
private ObserverSnapshot(

59
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
View File

@@ -8,24 +8,83 @@ import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
/**
* Interface for Kalman filters for use with KalmanFilterLatencyCompensator.
*
* @param <States> The number of states.
* @param <Inputs> The number of inputs.
* @param <Outputs> The number of outputs.
*/
public interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
/**
* Returns the error covariance matrix.
*
* @return The error covariance matrix.
*/
Matrix<States, States> getP();
/**
* Returns an element of the error covariance matrix.
*
* @param i The row.
* @param j The column.
* @return An element of the error covariance matrix.
*/
double getP(int i, int j);
/**
* Sets the error covariance matrix.
*
* @param newP The error covariance matrix.
*/
void setP(Matrix<States, States> newP);
/**
* Returns the state estimate.
*
* @return The state estimate.
*/
Matrix<States, N1> getXhat();
/**
* Returns an element of the state estimate.
*
* @param i The row.
* @return An element of the state estimate.
*/
double getXhat(int i);
/**
* Sets the state estimate.
*
* @param xHat The state estimate.
*/
void setXhat(Matrix<States, N1> xHat);
/**
* Sets an element of the state estimate.
*
* @param i The row.
* @param value The value.
*/
void setXhat(int i, double value);
/** Resets the observer. */
void reset();
/**
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
*/
void predict(Matrix<Inputs, N1> u, double dtSeconds);
/**
* Correct the state estimate x-hat using the measurements in y.
*
* @param u Same control input used in the predict step.
* @param y Measurement vector.
*/
void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y);
}

1
wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
View File

@@ -119,6 +119,7 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
reset();
}
/** Resets the observer. */
public final void reset() {
m_xHat = new Matrix<>(m_states, Nat.N1());
}

4
wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
View File

@@ -11,9 +11,13 @@ import edu.wpi.first.math.MathSharedStore;
* baseline for a specified period of time before the filtered value changes.
*/
public class Debouncer {
/** Type of debouncing to perform. */
public enum DebounceType {
/** Rising edge. */
kRising,
/** Falling edge. */
kFalling,
/** Both rising and falling edges. */
kBoth
}

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -326,6 +326,9 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
}
}
public static final Pose2dStruct struct = new Pose2dStruct();
/** Pose2d protobuf for serialization. */
public static final Pose2dProto proto = new Pose2dProto();
/** Pose2d struct for serialization. */
public static final Pose2dStruct struct = new Pose2dStruct();
}

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -323,6 +323,9 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
}
}
public static final Pose3dStruct struct = new Pose3dStruct();
/** Pose3d protobuf for serialization. */
public static final Pose3dProto proto = new Pose3dProto();
/** Pose3d struct for serialization. */
public static final Pose3dStruct struct = new Pose3dStruct();
}

6
wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
View File

@@ -17,6 +17,7 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;
/** Represents a quaternion. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Quaternion implements ProtobufSerializable, StructSerializable {
@@ -405,6 +406,9 @@ public class Quaternion implements ProtobufSerializable, StructSerializable {
return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ());
}
public static final QuaternionStruct struct = new QuaternionStruct();
/** Quaternion protobuf for serialization. */
public static final QuaternionProto proto = new QuaternionProto();
/** Quaternion struct for serialization. */
public static final QuaternionStruct struct = new QuaternionStruct();
}

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -275,6 +275,9 @@ public class Rotation2d
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
public static final Rotation2dStruct struct = new Rotation2dStruct();
/** Rotation2d protobuf for serialization. */
public static final Rotation2dProto proto = new Rotation2dProto();
/** Rotation2d struct for serialization. */
public static final Rotation2dStruct struct = new Rotation2dStruct();
}

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -432,6 +432,9 @@ public class Rotation3d
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}
public static final Rotation3dStruct struct = new Rotation3dStruct();
/** Rotation3d protobuf for serialization. */
public static final Rotation3dProto proto = new Rotation3dProto();
/** Rotation3d struct for serialization. */
public static final Rotation3dStruct struct = new Rotation3dStruct();
}

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
View File

@@ -184,6 +184,9 @@ public class Transform2d implements ProtobufSerializable, StructSerializable {
return Objects.hash(m_translation, m_rotation);
}
public static final Transform2dStruct struct = new Transform2dStruct();
/** Transform2d protobuf for serialization. */
public static final Transform2dProto proto = new Transform2dProto();
/** Transform2d struct for serialization. */
public static final Transform2dStruct struct = new Transform2dStruct();
}

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
View File

@@ -178,6 +178,9 @@ public class Transform3d implements ProtobufSerializable, StructSerializable {
return Objects.hash(m_translation, m_rotation);
}
public static final Transform3dStruct struct = new Transform3dStruct();
/** Transform3d protobuf for serialization. */
public static final Transform3dProto proto = new Transform3dProto();
/** Transform3d struct for serialization. */
public static final Transform3dStruct struct = new Transform3dStruct();
}

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -250,6 +250,9 @@ public class Translation2d
MathUtil.interpolate(this.getY(), endValue.getY(), t));
}
public static final Translation2dStruct struct = new Translation2dStruct();
/** Translation2d protobuf for serialization. */
public static final Translation2dProto proto = new Translation2dProto();
/** Translation2d struct for serialization. */
public static final Translation2dStruct struct = new Translation2dStruct();
}

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -253,6 +253,9 @@ public class Translation3d
MathUtil.interpolate(this.getZ(), endValue.getZ(), t));
}
public static final Translation3dStruct struct = new Translation3dStruct();
/** Translation3d protobuf for serialization. */
public static final Translation3dProto proto = new Translation3dProto();
/** Translation3d struct for serialization. */
public static final Translation3dStruct struct = new Translation3dStruct();
}

6
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
View File

@@ -26,6 +26,7 @@ public class Twist2d implements ProtobufSerializable, StructSerializable {
/** Angular "dtheta" component (radians). */
public double dtheta;
/** Default constructor. */
public Twist2d() {}
/**
@@ -67,6 +68,9 @@ public class Twist2d implements ProtobufSerializable, StructSerializable {
return Objects.hash(dx, dy, dtheta);
}
public static final Twist2dStruct struct = new Twist2dStruct();
/** Twist2d protobuf for serialization. */
public static final Twist2dProto proto = new Twist2dProto();
/** Twist2d struct for serialization. */
public static final Twist2dStruct struct = new Twist2dStruct();
}

6
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
View File

@@ -35,6 +35,7 @@ public class Twist3d implements ProtobufSerializable, StructSerializable {
/** Rotation vector z component (radians). */
public double rz;
/** Default constructor. */
public Twist3d() {}
/**
@@ -87,6 +88,9 @@ public class Twist3d implements ProtobufSerializable, StructSerializable {
return Objects.hash(dx, dy, dz, rx, ry, rz);
}
public static final Twist3dStruct struct = new Twist3dStruct();
/** Twist3d protobuf for serialization. */
public static final Twist3dProto proto = new Twist3dProto();
/** Twist3d struct for serialization. */
public static final Twist3dStruct struct = new Twist3dStruct();
}

1
wpimath/src/main/java/edu/wpi/first/math/interpolation/InterpolatingDoubleTreeMap.java
View File

@@ -9,6 +9,7 @@ package edu.wpi.first.math.interpolation;
* from points that are defined. This uses linear interpolation.
*/
public class InterpolatingDoubleTreeMap extends InterpolatingTreeMap<Double, Double> {
/** Default constructor. */
public InterpolatingDoubleTreeMap() {
super(InverseInterpolator.forDouble(), Interpolator.forDouble());
}

5
wpimath/src/main/java/edu/wpi/first/math/interpolation/Interpolator.java
View File

@@ -24,6 +24,11 @@ public interface Interpolator<T> {
*/
T interpolate(T startValue, T endValue, double t);
/**
* Returns interpolator for Double.
*
* @return Interpolator for Double.
*/
static Interpolator<Double> forDouble() {
return MathUtil::interpolate;
}

5
wpimath/src/main/java/edu/wpi/first/math/interpolation/InverseInterpolator.java
View File

@@ -24,6 +24,11 @@ public interface InverseInterpolator<T> {
*/
double inverseInterpolate(T startValue, T endValue, T q);
/**
* Returns inverse interpolator for Double.
*
* @return Inverse interpolator for Double.
*/
static InverseInterpolator<Double> forDouble() {
return MathUtil::inverseInterpolate;
}

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

@@ -40,7 +40,10 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
/** Represents the angular velocity of the robot frame. (CCW is +) */
public double omegaRadiansPerSecond;
/** ChassisSpeeds protobuf for serialization. */
public static final ChassisSpeedsProto proto = new ChassisSpeedsProto();
/** ChassisSpeeds struct for serialization. */
public static final ChassisSpeedsStruct struct = new ChassisSpeedsStruct();
/** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
View File

@@ -28,10 +28,14 @@ public class DifferentialDriveKinematics
implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>,
ProtobufSerializable,
StructSerializable {
/** Differential drive trackwidth. */
public final double trackWidthMeters;
/** DifferentialDriveKinematics protobuf for serialization. */
public static final DifferentialDriveKinematicsProto proto =
new DifferentialDriveKinematicsProto();
/** DifferentialDriveKinematics struct for serialization. */
public static final DifferentialDriveKinematicsStruct struct =
new DifferentialDriveKinematicsStruct();

1
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
View File

@@ -11,6 +11,7 @@ import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import java.util.Objects;
/** Represents the wheel positions for a differential drive drivetrain. */
public class DifferentialDriveWheelPositions
implements WheelPositions<DifferentialDriveWheelPositions> {
/** Distance measured by the left side. */

3
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
View File

@@ -22,8 +22,11 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
/** Speed of the right side of the robot. */
public double rightMetersPerSecond;
/** DifferentialDriveWheelSpeeds protobuf for serialization. */
public static final DifferentialDriveWheelSpeedsProto proto =
new DifferentialDriveWheelSpeedsProto();
/** DifferentialDriveWheelSpeeds struct for serialization. */
public static final DifferentialDriveWheelSpeedsStruct struct =
new DifferentialDriveWheelSpeedsStruct();

23
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
View File

@@ -48,7 +48,10 @@ public class MecanumDriveKinematics
private Translation2d m_prevCoR = new Translation2d();
/** MecanumDriveKinematics protobuf for serialization. */
public static final MecanumDriveKinematicsProto proto = new MecanumDriveKinematicsProto();
/** MecanumDriveKinematics struct for serialization. */
public static final MecanumDriveKinematicsStruct struct = new MecanumDriveKinematicsStruct();
/**
@@ -217,18 +220,38 @@ public class MecanumDriveKinematics
m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
}
/**
* Returns the front-left wheel translation.
*
* @return The front-left wheel translation.
*/
public Translation2d getFrontLeft() {
return m_frontLeftWheelMeters;
}
/**
* Returns the front-right wheel translation.
*
* @return The front-right wheel translation.
*/
public Translation2d getFrontRight() {
return m_frontRightWheelMeters;
}
/**
* Returns the rear-left wheel translation.
*
* @return The rear-left wheel translation.
*/
public Translation2d getRearLeft() {
return m_rearLeftWheelMeters;
}
/**
* Returns the rear-right wheel translation.
*
* @return The rear-right wheel translation.
*/
public Translation2d getRearRight() {
return m_rearRightWheelMeters;
}

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
View File

@@ -15,6 +15,7 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;
/** Represents the wheel positions for a mecanum drive drivetrain. */
public class MecanumDriveWheelPositions
implements WheelPositions<MecanumDriveWheelPositions>,
ProtobufSerializable,
@@ -31,9 +32,12 @@ public class MecanumDriveWheelPositions
/** Distance measured by the rear right wheel. */
public double rearRightMeters;
/** MecanumDriveWheelPositions protobuf for serialization. */
public static final MecanumDriveWheelPositionsProto proto = new MecanumDriveWheelPositionsProto();
/** MecanumDriveWheelPositions struct for serialization. */
public static final MecanumDriveWheelPositionsStruct struct =
new MecanumDriveWheelPositionsStruct();
public static final MecanumDriveWheelPositionsProto proto = new MecanumDriveWheelPositionsProto();
/** Constructs a MecanumDriveWheelPositions with zeros for all member fields. */
public MecanumDriveWheelPositions() {}

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
View File

@@ -14,6 +14,7 @@ import edu.wpi.first.units.Velocity;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
/** Represents the wheel speeds for a mecanum drive drivetrain. */
public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSerializable {
/** Speed of the front left wheel. */
public double frontLeftMetersPerSecond;
@@ -27,9 +28,12 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
/** Speed of the rear right wheel. */
public double rearRightMetersPerSecond;
public static final MecanumDriveWheelSpeedsStruct struct = new MecanumDriveWheelSpeedsStruct();
/** MecanumDriveWheelSpeeds protobuf for serialization. */
public static final MecanumDriveWheelSpeedsProto proto = new MecanumDriveWheelSpeedsProto();
/** MecanumDriveWheelSpeeds struct for serialization. */
public static final MecanumDriveWheelSpeedsStruct struct = new MecanumDriveWheelSpeedsStruct();
/** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */
public MecanumDriveWheelSpeeds() {}

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -41,7 +41,9 @@ import org.ejml.simple.SimpleMatrix;
*/
public class SwerveDriveKinematics
implements Kinematics<SwerveDriveKinematics.SwerveDriveWheelStates, SwerveDriveWheelPositions> {
/** Wrapper class for swerve module states. */
public static class SwerveDriveWheelStates {
/** Swerve module states. */
public SwerveModuleState[] states;
/**

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java
View File

@@ -7,7 +7,9 @@ package edu.wpi.first.math.kinematics;
import java.util.Arrays;
import java.util.Objects;
/** Represents the wheel positions for a swerve drive drivetrain. */
public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
/** The distances driven by the wheels. */
public SwerveModulePosition[] positions;
/**

5
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
View File

@@ -29,9 +29,12 @@ public class SwerveModulePosition
/** Angle of the module. */
public Rotation2d angle = Rotation2d.fromDegrees(0);
public static final SwerveModulePositionStruct struct = new SwerveModulePositionStruct();
/** SwerveModulePosition protobuf for serialization. */
public static final SwerveModulePositionProto proto = new SwerveModulePositionProto();
/** SwerveModulePosition struct for serialization. */
public static final SwerveModulePositionStruct struct = new SwerveModulePositionStruct();
/** Constructs a SwerveModulePosition with zeros for distance and angle. */
public SwerveModulePosition() {}

5
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
View File

@@ -25,9 +25,12 @@ public class SwerveModuleState
/** Angle of the module. */
public Rotation2d angle = Rotation2d.fromDegrees(0);
public static final SwerveModuleStateStruct struct = new SwerveModuleStateStruct();
/** SwerveModuleState protobuf for serialization. */
public static final SwerveModuleStateProto proto = new SwerveModuleStateProto();
/** SwerveModuleState struct for serialization. */
public static final SwerveModuleStateStruct struct = new SwerveModuleStateStruct();
/** Constructs a SwerveModuleState with zeros for speed and angle. */
public SwerveModuleState() {}

5
wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java
View File

@@ -6,6 +6,11 @@ package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.interpolation.Interpolatable;
/**
* Interface for wheel positions.
*
* @param <T> Wheel positions type.
*/
public interface WheelPositions<T extends WheelPositions<T>> extends Interpolatable<T> {
/**
* Returns a copy of this instance.

1
wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math.spline;
import org.ejml.simple.SimpleMatrix;
/** Represents a hermite spline of degree 3. */
public class CubicHermiteSpline extends Spline {
private static SimpleMatrix hermiteBasis;
private final SimpleMatrix m_coefficients;

4
wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
View File

@@ -8,10 +8,10 @@ import edu.wpi.first.math.geometry.Pose2d;
/** Represents a pair of a pose and a curvature. */
public class PoseWithCurvature {
// Represents the pose.
/** Represents the pose. */
public Pose2d poseMeters;
// Represents the curvature.
/** Represents the curvature. */
public double curvatureRadPerMeter;
/**

1
wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math.spline;
import org.ejml.simple.SimpleMatrix;
/** Represents a hermite spline of degree 5. */
public class QuinticHermiteSpline extends Spline {
private static SimpleMatrix hermiteBasis;
private final SimpleMatrix m_coefficients;

3
wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
View File

@@ -99,7 +99,10 @@ public abstract class Spline {
* the value of x[2] is the second derivative in the x dimension.
*/
public static class ControlVector {
/** The x components of the control vector. */
public double[] x;
/** The y components of the control vector. */
public double[] y;
/**

1
wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
View File

@@ -10,6 +10,7 @@ import java.util.Arrays;
import java.util.List;
import org.ejml.simple.SimpleMatrix;
/** Helper class that is used to generate cubic and quintic splines from user provided waypoints. */
public final class SplineHelper {
/** Private constructor because this is a utility class. */
private SplineHelper() {}

1
wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
View File

@@ -56,6 +56,7 @@ public final class SplineParameterizer {
}
}
/** Exception for malformed splines. */
public static class MalformedSplineException extends RuntimeException {
/**
* Create a new exception with the given message.

1
wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
View File

@@ -9,6 +9,7 @@ import edu.wpi.first.math.Num;
import edu.wpi.first.math.Pair;
import org.ejml.simple.SimpleMatrix;
/** Discretization helper functions. */
public final class Discretization {
private Discretization() {
// Utility class

12
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
View File

@@ -8,6 +8,18 @@ import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.numbers.N1;
/**
* A plant defined using state-space notation.
*
* <p>A plant is a mathematical model of a system's dynamics.
*
* <p>For more on the underlying math, read
* https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
*
* @param <States> Number of states.
* @param <Inputs> Number of inputs.
* @param <Outputs> Number of outputs.
*/
public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
/** Continuous system matrix. */
private final Matrix<States, States> m_A;

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

@@ -11,6 +11,7 @@ import java.util.function.BiFunction;
import java.util.function.DoubleFunction;
import java.util.function.Function;
/** Numerical integration utilities. */
public final class NumericalIntegration {
private NumericalIntegration() {
// utility Class

1
wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
View File

@@ -11,6 +11,7 @@ import edu.wpi.first.math.numbers.N1;
import java.util.function.BiFunction;
import java.util.function.Function;
/** Numerical Jacobian utilities. */
public final class NumericalJacobian {
private NumericalJacobian() {
// Utility Class.

18
wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
View File

@@ -12,16 +12,34 @@ import edu.wpi.first.util.struct.StructSerializable;
/** Holds the constants for a DC motor. */
public class DCMotor implements ProtobufSerializable, StructSerializable {
/** Voltage at which the motor constants were measured. */
public final double nominalVoltageVolts;
/** Torque when stalled. */
public final double stallTorqueNewtonMeters;
/** Current draw when stalled. */
public final double stallCurrentAmps;
/** Current draw under no load. */
public final double freeCurrentAmps;
/** Angular velocity under no load. */
public final double freeSpeedRadPerSec;
/** Motor internal resistance. */
public final double rOhms;
/** Motor velocity constant. */
public final double KvRadPerSecPerVolt;
/** Motor torque constant. */
public final double KtNMPerAmp;
/** DCMotor protobuf for serialization. */
public static final DCMotorProto proto = new DCMotorProto();
/** DCMotor struct for serialization. */
public static final DCMotorStruct struct = new DCMotorStruct();
/**

1
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -12,6 +12,7 @@ import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
/** Linear system ID utility functions. */
public final class LinearSystemId {
private LinearSystemId() {
// Utility class

21
wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java
View File

@@ -39,10 +39,20 @@ import java.util.Objects;
public class ExponentialProfile {
private final Constraints m_constraints;
/** Profile timing. */
public static class ProfileTiming {
/** Profile inflection time. */
public final double inflectionTime;
/** Total profile time. */
public final double totalTime;
/**
* Constructs a ProfileTiming.
*
* @param inflectionTime Profile inflection time.
* @param totalTime Total profile time.
*/
protected ProfileTiming(double inflectionTime, double totalTime) {
this.inflectionTime = inflectionTime;
this.totalTime = totalTime;
@@ -55,14 +65,19 @@ public class ExponentialProfile {
* @return if the profile is finished at time t.
*/
public boolean isFinished(double t) {
return t > inflectionTime;
return t >= inflectionTime;
}
}
/** Profile constraints. */
public static class Constraints {
/** Maximum unsigned input voltage. */
public final double maxInput;
/** The State-Space 1x1 system matrix. */
public final double A;
/** The State-Space 1x1 input matrix. */
public final double B;
/**
@@ -112,11 +127,15 @@ public class ExponentialProfile {
}
}
/** Profile state. */
public static class State {
/** The position at this state. */
public double position;
/** The velocity at this state. */
public double velocity;
/** Default constructor. */
public State() {}
/**

13
wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
View File

@@ -20,6 +20,7 @@ import java.util.stream.Collectors;
* represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
*/
public class Trajectory implements ProtobufSerializable {
/** Trajectory protobuf for serialization. */
public static final TrajectoryProto proto = new TrajectoryProto();
private final double m_totalTimeSeconds;
@@ -269,28 +270,30 @@ public class Trajectory implements ProtobufSerializable {
* represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
*/
public static class State implements ProtobufSerializable {
/** Trajectory.State protobuf for serialization. */
public static final TrajectoryStateProto proto = new TrajectoryStateProto();
// The time elapsed since the beginning of the trajectory.
/** The time elapsed since the beginning of the trajectory. */
@JsonProperty("time")
public double timeSeconds;
// The speed at that point of the trajectory.
/** The speed at that point of the trajectory. */
@JsonProperty("velocity")
public double velocityMetersPerSecond;
// The acceleration at that point of the trajectory.
/** The acceleration at that point of the trajectory. */
@JsonProperty("acceleration")
public double accelerationMetersPerSecondSq;
// The pose at that point of the trajectory.
/** The pose at that point of the trajectory. */
@JsonProperty("pose")
public Pose2d poseMeters;
// The curvature at that point of the trajectory.
/** The curvature at that point of the trajectory. */
@JsonProperty("curvature")
public double curvatureRadPerMeter;
/** Default constructor. */
public State() {
poseMeters = new Pose2d();
}

22
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
View File

@@ -19,6 +19,7 @@ import java.util.Collection;
import java.util.List;
import java.util.function.BiConsumer;
/** Helper class used to generate trajectories with various constraints. */
public final class TrajectoryGenerator {
private static final Trajectory kDoNothingTrajectory =
new Trajectory(List.of(new Trajectory.State()));
@@ -263,16 +264,27 @@ public final class TrajectoryGenerator {
return splinePoints;
}
// Work around type erasure signatures
/** Control vector list type that works around type erasure signatures. */
public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
public ControlVectorList(int initialCapacity) {
super(initialCapacity);
}
/** Default constructor. */
public ControlVectorList() {
super();
}
/**
* Constructs a ControlVectorList.
*
* @param initialCapacity The initial list capacity.
*/
public ControlVectorList(int initialCapacity) {
super(initialCapacity);
}
/**
* Constructs a ControlVectorList.
*
* @param collection A collection of spline control vectors.
*/
public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
super(collection);
}

6
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
View File

@@ -315,7 +315,13 @@ public final class TrajectoryParameterizer {
}
}
/** Exception for trajectory generation failure. */
public static class TrajectoryGenerationException extends RuntimeException {
/**
* Constructs a TrajectoryGenerationException.
*
* @param message Exception message.
*/
public TrajectoryGenerationException(String message) {
super(message);
}

7
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
View File

@@ -12,6 +12,7 @@ import java.nio.file.Path;
import java.util.ArrayList;
import java.util.List;
/** Trajectory utilities. */
public final class TrajectoryUtil {
private TrajectoryUtil() {
throw new UnsupportedOperationException("This is a utility class!");
@@ -111,7 +112,13 @@ public final class TrajectoryUtil {
return WPIMathJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
}
/** Exception for trajectory serialization failure. */
public static class TrajectorySerializationException extends RuntimeException {
/**
* Constructs a TrajectorySerializationException.
*
* @param message The exception message.
*/
public TrajectorySerializationException(String message) {
super(message);
}

27
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
View File

@@ -54,9 +54,12 @@ public class TrapezoidProfile {
private double m_endFullSpeed;
private double m_endDeccel;
/** Profile constraints. */
public static class Constraints {
/** Maximum velocity. */
public final double maxVelocity;
/** Maximum acceleration. */
public final double maxAcceleration;
/**
@@ -71,24 +74,48 @@ public class TrapezoidProfile {
MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
}
/**
* Construct constraints for a TrapezoidProfile.
*
* @param <U> Unit type.
* @param maxVelocity maximum velocity
* @param maxAcceleration maximum acceleration
*/
public <U extends Unit<U>> Constraints(
Measure<Velocity<U>> maxVelocity, Measure<Velocity<Velocity<U>>> maxAcceleration) {
this(maxVelocity.baseUnitMagnitude(), maxAcceleration.baseUnitMagnitude());
}
}
/** Profile state. */
public static class State {
/** The position at this state. */
public double position;
/** The velocity at this state. */
public double velocity;
/** Default constructor. */
public State() {}
/**
* Constructs constraints for a Trapezoid Profile.
*
* @param position The position at this state.
* @param velocity The velocity at this state.
*/
public State(double position, double velocity) {
this.position = position;
this.velocity = velocity;
}
/**
* Constructs constraints for a Trapezoid Profile.
*
* @param <U> Unit type.
* @param position The position at this state.
* @param velocity The velocity at this state.
*/
public <U extends Unit<U>> State(Measure<U> position, Measure<Velocity<U>> velocity) {
this(position.baseUnitMagnitude(), velocity.baseUnitMagnitude());
}

3
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
View File

@@ -37,7 +37,10 @@ public interface TrajectoryConstraint {
/** Represents a minimum and maximum acceleration. */
class MinMax {
/** The minimum acceleration. */
public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
/** The maximum acceleration. */
public double maxAccelerationMetersPerSecondSq = Double.MAX_VALUE;
/**