The sync time is the time to hold output so LEDs enable. Default set for WS2812B and WS2815. * * @param handle the Addressable LED handle - * @param syncTimeMicroSeconds the sync time (default 280us) + * @param syncTime the sync time in microseconds (default 280 μs) * @see "HAL_SetAddressableLEDSyncTime" */ - public static native void setSyncTime(int handle, int syncTimeMicroSeconds); + public static native void setSyncTime(int handle, int syncTime); /** * Starts the output. diff --git a/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java b/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java index 4e4529c0f87..b921bc34610 100644 --- a/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java @@ -89,10 +89,10 @@ public class DIOJNI extends JNIWrapper { * going at any time. * * @param dioPortHandle the digital port handle - * @param pulseLengthSeconds the active length of the pulse (in seconds) + * @param pulseLength the active length of the pulse in seconds * @see "HAL_Pulse" */ - public static native void pulse(int dioPortHandle, double pulseLengthSeconds); + public static native void pulse(int dioPortHandle, double pulseLength); /** * Generates a single digital pulse on multiple channels. @@ -101,10 +101,10 @@ public class DIOJNI extends JNIWrapper { * any time. * * @param channelMask the channel mask - * @param pulseLengthSeconds the active length of the pulse (in seconds) + * @param pulseLength the active length of the pulse in seconds * @see "HAL_PulseMultiple" */ - public static native void pulseMultiple(long channelMask, double pulseLengthSeconds); + public static native void pulseMultiple(long channelMask, double pulseLength); /** * Checks a DIO line to see if it is currently generating a pulse. diff --git a/hal/src/main/native/include/hal/AddressableLED.h b/hal/src/main/native/include/hal/AddressableLED.h index 3181e7e2bbe..e1f6a5d465b 100644 --- a/hal/src/main/native/include/hal/AddressableLED.h +++ b/hal/src/main/native/include/hal/AddressableLED.h @@ -81,17 +81,15 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle, * needs to be set for those. * * @param[in] handle the Addressable LED handle - * @param[in] highTime0NanoSeconds high time for 0 bit (default 400ns) - * @param[in] lowTime0NanoSeconds low time for 0 bit (default 900ns) - * @param[in] highTime1NanoSeconds high time for 1 bit (default 900ns) - * @param[in] lowTime1NanoSeconds low time for 1 bit (default 600ns) + * @param[in] highTime0 high time for 0 bit in nanoseconds (default 400 ns) + * @param[in] lowTime0 low time for 0 bit in nanoseconds (default 900 ns) + * @param[in] highTime1 high time for 1 bit in nanoseconds (default 900 ns) + * @param[in] lowTime1 low time for 1 bit in nanoseconds (default 600 ns) * @param[out] status the error code, or 0 for success */ void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle, - int32_t highTime0NanoSeconds, - int32_t lowTime0NanoSeconds, - int32_t highTime1NanoSeconds, - int32_t lowTime1NanoSeconds, + int32_t highTime0, int32_t lowTime0, + int32_t highTime1, int32_t lowTime1, int32_t* status); /** @@ -101,12 +99,11 @@ void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle, * WS2812B and WS2815. * * @param[in] handle the Addressable LED handle - * @param[in] syncTimeMicroSeconds the sync time (default 280us) + * @param[in] syncTime the sync time in microseconds (default 280 μs) * @param[out] status the error code, or 0 for success */ void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle, - int32_t syncTimeMicroSeconds, - int32_t* status); + int32_t syncTime, int32_t* status); /** * Starts the output. diff --git a/hal/src/main/native/include/hal/DIO.h b/hal/src/main/native/include/hal/DIO.h index e9e91197819..218637ca376 100644 --- a/hal/src/main/native/include/hal/DIO.h +++ b/hal/src/main/native/include/hal/DIO.h @@ -158,10 +158,10 @@ HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status); * single pulse going at any time. * * @param[in] dioPortHandle the digital port handle - * @param[in] pulseLengthSeconds the active length of the pulse (in seconds) + * @param[in] pulseLength the active length of the pulse in seconds * @param[out] status Error status variable. 0 on success. */ -void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds, +void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength, int32_t* status); /** @@ -171,10 +171,10 @@ void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds, * single pulse going at any time. * * @param[in] channelMask the channel mask - * @param[in] pulseLengthSeconds the active length of the pulse (in seconds) - * @param[out] status Error status variable. 0 on success. + * @param[in] pulseLength the active length of the pulse in seconds + * @param[out] status Error status variable. 0 on success. */ -void HAL_PulseMultiple(uint32_t channelMask, double pulseLengthSeconds, +void HAL_PulseMultiple(uint32_t channelMask, double pulseLength, int32_t* status); /** diff --git a/hal/src/main/native/sim/AddressableLED.cpp b/hal/src/main/native/sim/AddressableLED.cpp index bce3f1ddfde..bdc9a6924bd 100644 --- a/hal/src/main/native/sim/AddressableLED.cpp +++ b/hal/src/main/native/sim/AddressableLED.cpp @@ -146,15 +146,12 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle, } void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle, - int32_t highTime0NanoSeconds, - int32_t lowTime0NanoSeconds, - int32_t highTime1NanoSeconds, - int32_t lowTime1NanoSeconds, + int32_t highTime0, int32_t lowTime0, + int32_t highTime1, int32_t lowTime1, int32_t* status) {} void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle, - int32_t syncTimeMicroSeconds, - int32_t* status) {} + int32_t syncTime, int32_t* status) {} void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle, int32_t* status) { diff --git a/hal/src/main/native/sim/DutyCycle.cpp b/hal/src/main/native/sim/DutyCycle.cpp index c044a1cf7aa..80c9ddbbc51 100644 --- a/hal/src/main/native/sim/DutyCycle.cpp +++ b/hal/src/main/native/sim/DutyCycle.cpp @@ -107,9 +107,8 @@ int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle, return 0; } - double periodSeconds = 1.0 / SimDutyCycleData[dutyCycle->index].frequency; - double periodNanoSeconds = periodSeconds * 1e9; - return periodNanoSeconds * SimDutyCycleData[dutyCycle->index].output; + double period = 1e9 / SimDutyCycleData[dutyCycle->index].frequency; // ns + return period * SimDutyCycleData[dutyCycle->index].output; } int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle, diff --git a/hal/src/main/native/systemcore/AddressableLED.cpp b/hal/src/main/native/systemcore/AddressableLED.cpp index 76ce8513441..31f11a0dec2 100644 --- a/hal/src/main/native/systemcore/AddressableLED.cpp +++ b/hal/src/main/native/systemcore/AddressableLED.cpp @@ -54,18 +54,15 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle, } void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle, - int32_t highTime0NanoSeconds, - int32_t lowTime0NanoSeconds, - int32_t highTime1NanoSeconds, - int32_t lowTime1NanoSeconds, + int32_t highTime0, int32_t lowTime0, + int32_t highTime1, int32_t lowTime1, int32_t* status) { *status = HAL_HANDLE_ERROR; return; } void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle, - int32_t syncTimeMicroSeconds, - int32_t* status) { + int32_t syncTime, int32_t* status) { *status = HAL_HANDLE_ERROR; return; } diff --git a/hal/src/main/native/systemcore/DIO.cpp b/hal/src/main/native/systemcore/DIO.cpp index 8b51aeed8b1..2d86ec8e259 100644 --- a/hal/src/main/native/systemcore/DIO.cpp +++ b/hal/src/main/native/systemcore/DIO.cpp @@ -177,13 +177,13 @@ HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) { } } -void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds, +void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength, int32_t* status) { *status = HAL_HANDLE_ERROR; return; } -void HAL_PulseMultiple(uint32_t channelMask, double pulseLengthSeconds, +void HAL_PulseMultiple(uint32_t channelMask, double pulseLength, int32_t* status) { *status = HAL_HANDLE_ERROR; return; diff --git a/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h b/sysid/src/main/native/include/sysid/analysis/TrackwidthAnalysis.h similarity index 84% rename from sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h rename to sysid/src/main/native/include/sysid/analysis/TrackwidthAnalysis.h index d086022bff1..ecc319a180a 100644 --- a/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h +++ b/sysid/src/main/native/include/sysid/analysis/TrackwidthAnalysis.h @@ -10,14 +10,14 @@ namespace sysid { /** - * Calculates the track width given the left distance, right distance, and + * Calculates the trackwidth given the left distance, right distance, and * accumulated gyro angle. * * @param l The distance traveled by the left side of the drivetrain. * @param r The distance traveled by the right side of the drivetrain. * @param accum The accumulated gyro angle. */ -constexpr double CalculateTrackWidth(double l, double r, +constexpr double CalculateTrackwidth(double l, double r, units::radian_t accum) { // The below comes from solving ω = (vr − vl) / 2r for 2r. return (gcem::abs(r) + gcem::abs(l)) / gcem::abs(accum.value()); diff --git a/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/TrackwidthAnalysisTest.cpp similarity index 64% rename from sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp rename to sysid/src/test/native/cpp/analysis/TrackwidthAnalysisTest.cpp index e7b662fe9d0..d5e2dc16675 100644 --- a/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp +++ b/sysid/src/test/native/cpp/analysis/TrackwidthAnalysisTest.cpp @@ -4,9 +4,9 @@ #include -#include "sysid/analysis/TrackWidthAnalysis.h" +#include "sysid/analysis/TrackwidthAnalysis.h" -TEST(TrackWidthAnalysisTest, Calculate) { - double result = sysid::CalculateTrackWidth(-0.5386, 0.5386, 90_deg); +TEST(TrackwidthAnalysisTest, Calculate) { + double result = sysid::CalculateTrackwidth(-0.5386, 0.5386, 90_deg); EXPECT_NEAR(result, 0.6858, 1E-4); } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java index 8ba81cfb16c..49f68dc5918 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java @@ -121,12 +121,12 @@ public static Command print(String message) { /** * Constructs a command that does nothing, finishing after a specified duration. * - * @param seconds after how long the command finishes + * @param time after how long the command finishes in seconds * @return the command * @see WaitCommand */ - public static Command waitSeconds(double seconds) { - return new WaitCommand(seconds); + public static Command wait(double time) { + return new WaitCommand(time); } /** diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java index a3d6c92d79d..56bcc8e851f 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -48,7 +48,7 @@ public class MecanumControllerCommand extends Command { private final MecanumDriveKinematics m_kinematics; private final HolonomicDriveController m_controller; private final Supplier m_desiredRotation; - private final double m_maxWheelVelocityMetersPerSecond; + private final double m_maxWheelVelocity; private final PIDController m_frontLeftController; private final PIDController m_rearLeftController; private final PIDController m_frontRightController; @@ -79,7 +79,7 @@ public class MecanumControllerCommand extends Command { * @param thetaController The Trajectory Tracker PID controller for angle for the robot. * @param desiredRotation The angle that the robot should be facing. This is sampled at each time * step. - * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel in m/s. * @param frontLeftController The front left wheel velocity PID. * @param rearLeftController The rear left wheel velocity PID. * @param frontRightController The front right wheel velocity PID. @@ -98,7 +98,7 @@ public MecanumControllerCommand( PIDController yController, ProfiledPIDController thetaController, Supplier desiredRotation, - double maxWheelVelocityMetersPerSecond, + double maxWheelVelocity, PIDController frontLeftController, PIDController rearLeftController, PIDController frontRightController, @@ -120,7 +120,7 @@ public MecanumControllerCommand( m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand"); - m_maxWheelVelocityMetersPerSecond = maxWheelVelocityMetersPerSecond; + m_maxWheelVelocity = maxWheelVelocity; m_frontLeftController = requireNonNullParam(frontLeftController, "frontLeftController", "MecanumControllerCommand"); @@ -266,8 +266,7 @@ public MecanumControllerCommand( xController, yController, thetaController, - () -> - trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(), + () -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(), maxWheelVelocityMetersPerSecond, frontLeftController, rearLeftController, @@ -299,7 +298,7 @@ public MecanumControllerCommand( * @param xController The Trajectory Tracker PID controller for the robot's x position. * @param yController The Trajectory Tracker PID controller for the robot's y position. * @param thetaController The Trajectory Tracker PID controller for angle for the robot. - * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel in m/s. * @param frontLeftController The front left wheel velocity PID. * @param rearLeftController The rear left wheel velocity PID. * @param frontRightController The front right wheel velocity PID. @@ -318,7 +317,7 @@ public MecanumControllerCommand( PIDController xController, PIDController yController, ProfiledPIDController thetaController, - double maxWheelVelocityMetersPerSecond, + double maxWheelVelocity, PIDController frontLeftController, PIDController rearLeftController, PIDController frontRightController, @@ -334,7 +333,7 @@ public MecanumControllerCommand( xController, yController, thetaController, - maxWheelVelocityMetersPerSecond, + maxWheelVelocity, frontLeftController, rearLeftController, frontRightController, @@ -362,7 +361,7 @@ public MecanumControllerCommand( * @param thetaController The Trajectory Tracker PID controller for angle for the robot. * @param desiredRotation The angle that the robot should be facing. This is sampled at each time * step. - * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel in m/s. * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds. * @param requirements The subsystems to require. */ @@ -375,7 +374,7 @@ public MecanumControllerCommand( PIDController yController, ProfiledPIDController thetaController, Supplier desiredRotation, - double maxWheelVelocityMetersPerSecond, + double maxWheelVelocity, Consumer outputWheelSpeeds, Subsystem... requirements) { m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand"); @@ -392,7 +391,7 @@ public MecanumControllerCommand( m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand"); - m_maxWheelVelocityMetersPerSecond = maxWheelVelocityMetersPerSecond; + m_maxWheelVelocity = maxWheelVelocity; m_frontLeftController = null; m_rearLeftController = null; @@ -430,7 +429,7 @@ public MecanumControllerCommand( * @param xController The Trajectory Tracker PID controller for the robot's x position. * @param yController The Trajectory Tracker PID controller for the robot's y position. * @param thetaController The Trajectory Tracker PID controller for angle for the robot. - * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel. + * @param maxWheelVelocity The maximum velocity of a drivetrain wheel. * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds. * @param requirements The subsystems to require. */ @@ -441,7 +440,7 @@ public MecanumControllerCommand( PIDController xController, PIDController yController, ProfiledPIDController thetaController, - double maxWheelVelocityMetersPerSecond, + double maxWheelVelocity, Consumer outputWheelSpeeds, Subsystem... requirements) { this( @@ -451,9 +450,8 @@ public MecanumControllerCommand( xController, yController, thetaController, - () -> - trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(), - maxWheelVelocityMetersPerSecond, + () -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(), + maxWheelVelocity, outputWheelSpeeds, requirements); } @@ -462,18 +460,16 @@ public MecanumControllerCommand( public void initialize() { var initialState = m_trajectory.sample(0); - var initialXVelocity = - initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getCos(); - var initialYVelocity = - initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin(); + var initialXVelocity = initialState.velocity * initialState.pose.getRotation().getCos(); + var initialYVelocity = initialState.velocity * initialState.pose.getRotation().getSin(); MecanumDriveWheelSpeeds prevSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0)); - m_prevFrontLeftSpeedSetpoint = prevSpeeds.frontLeftMetersPerSecond; - m_prevRearLeftSpeedSetpoint = prevSpeeds.rearLeftMetersPerSecond; - m_prevFrontRightSpeedSetpoint = prevSpeeds.frontRightMetersPerSecond; - m_prevRearRightSpeedSetpoint = prevSpeeds.rearRightMetersPerSecond; + m_prevFrontLeftSpeedSetpoint = prevSpeeds.frontLeft; + m_prevRearLeftSpeedSetpoint = prevSpeeds.rearLeft; + m_prevFrontRightSpeedSetpoint = prevSpeeds.frontRight; + m_prevRearRightSpeedSetpoint = prevSpeeds.rearRight; m_timer.restart(); } @@ -488,12 +484,12 @@ public void execute() { m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get()); var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds); - targetWheelSpeeds = targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond); + targetWheelSpeeds = targetWheelSpeeds.desaturate(m_maxWheelVelocity); - double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond; - double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond; - double frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond; - double rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond; + double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft; + double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft; + double frontRightSpeedSetpoint = targetWheelSpeeds.frontRight; + double rearRightSpeedSetpoint = targetWheelSpeeds.rearRight; double frontLeftOutput; double rearLeftOutput; @@ -516,22 +512,22 @@ public void execute() { frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate( - m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint); + m_currentWheelSpeeds.get().frontLeft, frontLeftSpeedSetpoint); rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate( - m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint); + m_currentWheelSpeeds.get().rearLeft, rearLeftSpeedSetpoint); frontRightOutput = frontRightFeedforward + m_frontRightController.calculate( - m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint); + m_currentWheelSpeeds.get().frontRight, frontRightSpeedSetpoint); rearRightOutput = rearRightFeedforward + m_rearRightController.calculate( - m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint); + m_currentWheelSpeeds.get().rearRight, rearRightSpeedSetpoint); m_outputDriveVoltages.accept( frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput); @@ -553,7 +549,7 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds()); + return m_timer.hasElapsed(m_trajectory.getTotalTime()); } /** A consumer to represent an operation on the voltages of a mecanum drive. */ diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java index 424a288b03b..06d63ebb2e1 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java @@ -123,8 +123,7 @@ public SwerveControllerCommand( xController, yController, thetaController, - () -> - trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(), + () -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(), outputModuleStates, requirements); } @@ -162,8 +161,7 @@ public SwerveControllerCommand( pose, kinematics, controller, - () -> - trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(), + () -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(), outputModuleStates, requirements); } @@ -233,6 +231,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds()); + return m_timer.hasElapsed(m_trajectory.getTotalTime()); } } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java index 9ac412ce50b..5f5f3731f5e 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java @@ -60,24 +60,24 @@ void cleanupAll() { private static final double kAngularTolerance = 1 / 12.0; private static final double kWheelBase = 0.5; - private static final double kTrackWidth = 0.5; + private static final double kTrackwidth = 0.5; private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + new Translation2d(kWheelBase / 2, kTrackwidth / 2), + new Translation2d(kWheelBase / 2, -kTrackwidth / 2), + new Translation2d(-kWheelBase / 2, kTrackwidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackwidth / 2)); private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry( m_kinematics, Rotation2d.kZero, new MecanumDriveWheelPositions(), Pose2d.kZero); public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) { - this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond; - this.m_rearLeftSpeed = wheelSpeeds.rearLeftMetersPerSecond; - this.m_frontRightSpeed = wheelSpeeds.frontRightMetersPerSecond; - this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond; + this.m_frontLeftSpeed = wheelSpeeds.frontLeft; + this.m_rearLeftSpeed = wheelSpeeds.rearLeft; + this.m_frontRightSpeed = wheelSpeeds.frontRight; + this.m_rearRightSpeed = wheelSpeeds.rearRight; } public MecanumDriveWheelPositions getCurrentWheelDistances() { @@ -87,7 +87,7 @@ public MecanumDriveWheelPositions getCurrentWheelDistances() { public Pose2d getRobotPose() { m_odometry.update(m_angle, getCurrentWheelDistances()); - return m_odometry.getPoseMeters(); + return m_odometry.getPose(); } @Test @@ -101,7 +101,7 @@ void testReachesReference() { var config = new TrajectoryConfig(8.8, 0.1); final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); - final var endState = trajectory.sample(trajectory.getTotalTimeSeconds()); + final var endState = trajectory.sample(trajectory.getTotalTime()); final var command = new MecanumControllerCommand( @@ -120,7 +120,7 @@ void testReachesReference() { command.initialize(); while (!command.isFinished()) { command.execute(); - m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation(); + m_angle = trajectory.sample(m_timer.get()).pose.getRotation(); m_frontLeftDistance += m_frontLeftSpeed * 0.005; m_rearLeftDistance += m_rearLeftSpeed * 0.005; m_frontRightDistance += m_frontRightSpeed * 0.005; @@ -131,11 +131,11 @@ void testReachesReference() { command.end(true); assertAll( - () -> assertEquals(endState.poseMeters.getX(), getRobotPose().getX(), kxTolerance), - () -> assertEquals(endState.poseMeters.getY(), getRobotPose().getY(), kyTolerance), + () -> assertEquals(endState.pose.getX(), getRobotPose().getX(), kxTolerance), + () -> assertEquals(endState.pose.getY(), getRobotPose().getY(), kyTolerance), () -> assertEquals( - endState.poseMeters.getRotation().getRadians(), + endState.pose.getRotation().getRadians(), getRobotPose().getRotation().getRadians(), kAngularTolerance)); } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java index 0abe4990486..2d91497f347 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java @@ -67,14 +67,14 @@ void cleanup() { private static final double kAngularTolerance = 1 / 12.0; private static final double kWheelBase = 0.5; - private static final double kTrackWidth = 0.5; + private static final double kTrackwidth = 0.5; private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + new Translation2d(kWheelBase / 2, kTrackwidth / 2), + new Translation2d(kWheelBase / 2, -kTrackwidth / 2), + new Translation2d(-kWheelBase / 2, kTrackwidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackwidth / 2)); private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, Rotation2d.kZero, m_modulePositions, Pose2d.kZero); @@ -86,7 +86,7 @@ public void setModuleStates(SwerveModuleState[] moduleStates) { public Pose2d getRobotPose() { m_odometry.update(m_angle, m_modulePositions); - return m_odometry.getPoseMeters(); + return m_odometry.getPose(); } @Test @@ -100,7 +100,7 @@ void testReachesReference() { var config = new TrajectoryConfig(8.8, 0.1); final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); - final var endState = trajectory.sample(trajectory.getTotalTimeSeconds()); + final var endState = trajectory.sample(trajectory.getTotalTime()); final var command = new SwerveControllerCommand( @@ -118,10 +118,10 @@ void testReachesReference() { command.initialize(); while (!command.isFinished()) { command.execute(); - m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation(); + m_angle = trajectory.sample(m_timer.get()).pose.getRotation(); for (int i = 0; i < m_modulePositions.length; i++) { - m_modulePositions[i].distanceMeters += m_moduleStates[i].speedMetersPerSecond * 0.005; + m_modulePositions[i].distance += m_moduleStates[i].speed * 0.005; m_modulePositions[i].angle = m_moduleStates[i].angle; } @@ -131,11 +131,11 @@ void testReachesReference() { command.end(true); assertAll( - () -> assertEquals(endState.poseMeters.getX(), getRobotPose().getX(), kxTolerance), - () -> assertEquals(endState.poseMeters.getY(), getRobotPose().getY(), kyTolerance), + () -> assertEquals(endState.pose.getX(), getRobotPose().getX(), kxTolerance), + () -> assertEquals(endState.pose.getY(), getRobotPose().getY(), kyTolerance), () -> assertEquals( - endState.poseMeters.getRotation().getRadians(), + endState.pose.getRotation().getRadians(), getRobotPose().getRotation().getRadians(), kAngularTolerance)); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp index c79432d99eb..04af56a682d 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp @@ -51,13 +51,13 @@ class MecanumControllerCommandTest : public ::testing::Test { static constexpr units::radian_t kAngularTolerance{1 / 12.0}; static constexpr units::meter_t kWheelBase{0.5}; - static constexpr units::meter_t kTrackWidth{0.5}; + static constexpr units::meter_t kTrackwidth{0.5}; frc::MecanumDriveKinematics m_kinematics{ - frc::Translation2d{kWheelBase / 2, kTrackWidth / 2}, - frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2}, - frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2}, - frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}}; + frc::Translation2d{kWheelBase / 2, kTrackwidth / 2}, + frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2}, + frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2}, + frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}}; frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad, getCurrentWheelDistances(), diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp index b96a1e1c139..82036514e45 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp @@ -51,13 +51,13 @@ class SwerveControllerCommandTest : public ::testing::Test { static constexpr units::radian_t kAngularTolerance{1 / 12.0}; static constexpr units::meter_t kWheelBase{0.5}; - static constexpr units::meter_t kTrackWidth{0.5}; + static constexpr units::meter_t kTrackwidth{0.5}; frc::SwerveDriveKinematics<4> m_kinematics{ - frc::Translation2d{kWheelBase / 2, kTrackWidth / 2}, - frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2}, - frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2}, - frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}}; + frc::Translation2d{kWheelBase / 2, kTrackwidth / 2}, + frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2}, + frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2}, + frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}}; frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad, m_modulePositions, frc::Pose2d{0_m, 0_m, 0_rad}}; diff --git a/wpilibc/src/main/native/cpp/SharpIR.cpp b/wpilibc/src/main/native/cpp/SharpIR.cpp index fca2d9b2533..6d1b562016a 100644 --- a/wpilibc/src/main/native/cpp/SharpIR.cpp +++ b/wpilibc/src/main/native/cpp/SharpIR.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -15,29 +16,30 @@ using namespace frc; SharpIR SharpIR::GP2Y0A02YK0F(int channel) { - return SharpIR(channel, 62.28, -1.092, 20, 150.0); + return SharpIR(channel, 62.28, -1.092, 20_cm, 150_cm); } SharpIR SharpIR::GP2Y0A21YK0F(int channel) { - return SharpIR(channel, 26.449, -1.226, 10.0, 80.0); + return SharpIR(channel, 26.449, -1.226, 10_cm, 80_cm); } SharpIR SharpIR::GP2Y0A41SK0F(int channel) { - return SharpIR(channel, 12.354, -1.07, 4.0, 30.0); + return SharpIR(channel, 12.354, -1.07, 4_cm, 30_cm); } SharpIR SharpIR::GP2Y0A51SK0F(int channel) { - return SharpIR(channel, 5.2819, -1.161, 2.0, 15.0); + return SharpIR(channel, 5.2819, -1.161, 2_cm, 15_cm); } -SharpIR::SharpIR(int channel, double a, double b, double minCM, double maxCM) - : m_sensor(channel), m_A(a), m_B(b), m_minCM(minCM), m_maxCM(maxCM) { +SharpIR::SharpIR(int channel, double a, double b, units::meter_t min, + units::meter_t max) + : m_sensor(channel), m_A(a), m_B(b), m_min(min), m_max(max) { HAL_ReportUsage("IO", channel, "SharpIR"); wpi::SendableRegistry::Add(this, "SharpIR", channel); m_simDevice = hal::SimDevice("SharpIR", m_sensor.GetChannel()); if (m_simDevice) { - m_simRange = m_simDevice.CreateDouble("Range (cm)", false, 0.0); + m_simRange = m_simDevice.CreateDouble("Range (m)", false, 0.0); m_sensor.SetSimDevice(m_simDevice); } } @@ -46,19 +48,16 @@ int SharpIR::GetChannel() const { return m_sensor.GetChannel(); } -units::centimeter_t SharpIR::GetRange() const { - double distance; - +units::meter_t SharpIR::GetRange() const { if (m_simRange) { - distance = m_simRange.Get(); + return std::clamp(units::meter_t{m_simRange.Get()}, m_min, m_max); } else { // Don't allow zero/negative values auto v = std::max(m_sensor.GetVoltage(), 0.00001); - distance = m_A * std::pow(v, m_B); - } - // Always constrain output - return units::centimeter_t{std::max(std::min(distance, m_maxCM), m_minCM)}; + return std::clamp(units::meter_t{m_A * std::pow(v, m_B) * 1e-2}, m_min, + m_max); + } } void SharpIR::InitSendable(wpi::SendableBuilder& builder) { diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 19f3ab6c7d3..b61b6ad6c9d 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -18,11 +18,11 @@ using namespace frc; using namespace frc::sim; DifferentialDrivetrainSim::DifferentialDrivetrainSim( - LinearSystem<2, 2, 2> plant, units::meter_t trackWidth, DCMotor driveMotor, + LinearSystem<2, 2, 2> plant, units::meter_t trackwidth, DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius, const std::array& measurementStdDevs) : m_plant(std::move(plant)), - m_rb(trackWidth / 2.0), + m_rb(trackwidth / 2.0), m_wheelRadius(wheelRadius), m_motor(driveMotor), m_originalGearing(gearRatio), @@ -36,11 +36,11 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim( DifferentialDrivetrainSim::DifferentialDrivetrainSim( frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J, units::kilogram_t mass, units::meter_t wheelRadius, - units::meter_t trackWidth, const std::array& measurementStdDevs) + units::meter_t trackwidth, const std::array& measurementStdDevs) : DifferentialDrivetrainSim( frc::LinearSystemId::DrivetrainVelocitySystem( - driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing), - trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {} + driveMotor, mass, wheelRadius, trackwidth / 2.0, J, gearing), + trackwidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {} Eigen::Vector2d DifferentialDrivetrainSim::ClampInput( const Eigen::Vector2d& u) { diff --git a/wpilibc/src/main/native/cpp/simulation/SharpIRSim.cpp b/wpilibc/src/main/native/cpp/simulation/SharpIRSim.cpp index 46922aa6e4a..d5abf6cc423 100644 --- a/wpilibc/src/main/native/cpp/simulation/SharpIRSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SharpIRSim.cpp @@ -16,9 +16,9 @@ SharpIRSim::SharpIRSim(const SharpIR& sharpIR) SharpIRSim::SharpIRSim(int channel) { frc::sim::SimDeviceSim deviceSim{"SharpIR", channel}; - m_simRange = deviceSim.GetDouble("Range (cm)"); + m_simRange = deviceSim.GetDouble("Range (m)"); } -void SharpIRSim::SetRange(units::centimeter_t rng) { - m_simRange.Set(rng.value()); +void SharpIRSim::SetRange(units::meter_t range) { + m_simRange.Set(range.value()); } diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h index c7af8528607..d7e7ca40a51 100644 --- a/wpilibc/src/main/native/include/frc/AddressableLED.h +++ b/wpilibc/src/main/native/include/frc/AddressableLED.h @@ -133,10 +133,10 @@ class AddressableLED { *

By default, the driver is set up to drive WS2812B and WS2815, so nothing * needs to be set for those. * - * @param highTime0 high time for 0 bit (default 400ns) - * @param lowTime0 low time for 0 bit (default 900ns) - * @param highTime1 high time for 1 bit (default 900ns) - * @param lowTime1 low time for 1 bit (default 600ns) + * @param highTime0 high time for 0 bit (default 400 ns) + * @param lowTime0 low time for 0 bit (default 900 ns) + * @param highTime1 high time for 1 bit (default 900 ns) + * @param lowTime1 low time for 1 bit (default 600 ns) */ void SetBitTiming(units::nanosecond_t highTime0, units::nanosecond_t lowTime0, units::nanosecond_t highTime1, diff --git a/wpilibc/src/main/native/include/frc/SharpIR.h b/wpilibc/src/main/native/include/frc/SharpIR.h index 4dd7e433192..df637a4dd99 100644 --- a/wpilibc/src/main/native/include/frc/SharpIR.h +++ b/wpilibc/src/main/native/include/frc/SharpIR.h @@ -63,10 +63,11 @@ class SharpIR : public wpi::Sendable, public wpi::SendableHelper { * @param channel Analog input channel the sensor is connected to * @param a Constant A * @param b Constant B - * @param minCM Minimum distance to report in centimeters - * @param maxCM Maximum distance to report in centimeters + * @param min Minimum distance to report + * @param max Maximum distance to report */ - SharpIR(int channel, double a, double b, double minCM, double maxCM); + SharpIR(int channel, double a, double b, units::meter_t min, + units::meter_t max); /** * Get the analog input channel number. @@ -80,7 +81,7 @@ class SharpIR : public wpi::Sendable, public wpi::SendableHelper { * * @return range of the target returned by the sensor */ - units::centimeter_t GetRange() const; + units::meter_t GetRange() const; void InitSendable(wpi::SendableBuilder& builder) override; @@ -92,8 +93,8 @@ class SharpIR : public wpi::Sendable, public wpi::SendableHelper { double m_A; double m_B; - double m_minCM; - double m_maxCM; + units::meter_t m_min; + units::meter_t m_max; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h index 644e348afd8..b6bf76076fa 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h @@ -25,7 +25,7 @@ class DifferentialDrivetrainSim { * system can be created with * LinearSystemId::DrivetrainVelocitySystem() or * LinearSystemId::IdentifyDrivetrainSystem(). - * @param trackWidth The robot's track width. + * @param trackwidth The robot's trackwidth. * @param driveMotor A DCMotor representing the left side of the drivetrain. * @param gearingRatio The gearingRatio ratio of the left side, as output over * input. This must be the same ratio as the ratio used to @@ -41,7 +41,7 @@ class DifferentialDrivetrainSim { * starting point. */ DifferentialDrivetrainSim( - LinearSystem<2, 2, 2> plant, units::meter_t trackWidth, + LinearSystem<2, 2, 2> plant, units::meter_t trackwidth, DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius, const std::array& measurementStdDevs = {}); @@ -56,7 +56,7 @@ class DifferentialDrivetrainSim { * center. * @param mass The mass of the drivebase. * @param wheelRadius The radius of the wheels on the drivetrain. - * @param trackWidth The robot's track width, or distance between left and + * @param trackwidth The robot's trackwidth, or distance between left and * right wheels. * @param measurementStdDevs Standard deviations for measurements, in the form * [x, y, heading, left velocity, right velocity, @@ -70,7 +70,7 @@ class DifferentialDrivetrainSim { DifferentialDrivetrainSim( frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J, units::kilogram_t mass, units::meter_t wheelRadius, - units::meter_t trackWidth, + units::meter_t trackwidth, const std::array& measurementStdDevs = {}); /** diff --git a/wpilibc/src/main/native/include/frc/simulation/SharpIRSim.h b/wpilibc/src/main/native/include/frc/simulation/SharpIRSim.h index afbc0f4f9a2..e4c98b03c63 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SharpIRSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SharpIRSim.h @@ -31,9 +31,9 @@ class SharpIRSim { /** * Set the range returned by the distance sensor. * - * @param rng range of the target returned by the sensor + * @param range range of the target returned by the sensor */ - void SetRange(units::centimeter_t rng); + void SetRange(units::meter_t range); private: hal::SimDouble m_simRange; diff --git a/wpilibc/src/test/native/cpp/SharpIRTest.cpp b/wpilibc/src/test/native/cpp/SharpIRTest.cpp index e5445eb709d..8456a565e34 100644 --- a/wpilibc/src/test/native/cpp/SharpIRTest.cpp +++ b/wpilibc/src/test/native/cpp/SharpIRTest.cpp @@ -13,11 +13,11 @@ TEST(SharpIRTest, SimDevices) { SharpIR s = SharpIR::GP2Y0A02YK0F(1); SharpIRSim sim(s); - EXPECT_EQ(20, s.GetRange().value()); + EXPECT_EQ(0.2, s.GetRange().value()); - sim.SetRange(units::centimeter_t{30}); - EXPECT_EQ(30, s.GetRange().value()); + sim.SetRange(30_cm); + EXPECT_EQ(0.3, s.GetRange().value()); - sim.SetRange(units::centimeter_t{300}); - EXPECT_EQ(150, s.GetRange().value()); + sim.SetRange(300_cm); + EXPECT_EQ(1.5, s.GetRange().value()); } diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h index 85bccc7f521..bc57f727d46 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h @@ -56,7 +56,7 @@ class Drivetrain { void UpdateOdometry(); private: - static constexpr units::meter_t kTrackWidth = 0.381_m * 2; + static constexpr units::meter_t kTrackwidth = 0.381_m * 2; static constexpr double kWheelRadius = 0.0508; // meters static constexpr int kEncoderResolution = 4096; @@ -73,7 +73,7 @@ class Drivetrain { frc::AnalogGyro m_gyro{0}; - frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; + frc::DifferentialDriveKinematics m_kinematics{kTrackwidth}; frc::DifferentialDriveOdometry m_odometry{ m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()}, units::meter_t{m_rightEncoder.GetDistance()}}; diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h index 724a79fb908..d0976216da1 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h @@ -111,7 +111,7 @@ class Drivetrain { nt::DoubleArrayEntry& cameraToObjectEntry); private: - static constexpr units::meter_t kTrackWidth = 0.381_m * 2; + static constexpr units::meter_t kTrackwidth = 0.381_m * 2; static constexpr units::meter_t kWheelRadius = 0.0508_m; static constexpr int kEncoderResolution = 4096; @@ -146,7 +146,7 @@ class Drivetrain { frc::AnalogGyro m_gyro{0}; - frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; + frc::DifferentialDriveKinematics m_kinematics{kTrackwidth}; // Gains are for example purposes only - must be determined for your own // robot! @@ -172,5 +172,5 @@ class Drivetrain { frc::LinearSystemId::IdentifyDrivetrainSystem( 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq); frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ - m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; + m_drivetrainSystem, kTrackwidth, frc::DCMotor::CIM(2), 8, 2_in}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp index 99aed4e627a..9fd38043e1f 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp @@ -7,10 +7,10 @@ namespace DriveConstants { const frc::MecanumDriveKinematics kDriveKinematics{ - frc::Translation2d{kWheelBase / 2, kTrackWidth / 2}, - frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2}, - frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2}, - frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}}; + frc::Translation2d{kWheelBase / 2, kTrackwidth / 2}, + frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2}, + frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2}, + frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}}; } // namespace DriveConstants diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h index 7dde79c308a..098dfa2028c 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h @@ -43,17 +43,17 @@ inline constexpr bool kRearLeftEncoderReversed = true; inline constexpr bool kFrontRightEncoderReversed = false; inline constexpr bool kRearRightEncoderReversed = true; -inline constexpr auto kTrackWidth = +inline constexpr auto kTrackwidth = 0.5_m; // Distance between centers of right and left wheels on robot inline constexpr auto kWheelBase = 0.7_m; // Distance between centers of front and back wheels on robot extern const frc::MecanumDriveKinematics kDriveKinematics; inline constexpr int kEncoderCPR = 1024; -inline constexpr double kWheelDiameterMeters = 0.15; +inline constexpr auto kWheelDiameter = 0.15_m; inline constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterMeters * std::numbers::pi) / + (kWheelDiameter.value() * std::numbers::pi) / static_cast(kEncoderCPR); // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h index 09974806e53..21378f8671b 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h @@ -72,7 +72,7 @@ class Drivetrain { void Periodic(); private: - static constexpr units::meter_t kTrackWidth = 0.381_m * 2; + static constexpr units::meter_t kTrackwidth = 0.381_m * 2; static constexpr double kWheelRadius = 0.0508; // meters static constexpr int kEncoderResolution = 4096; @@ -89,7 +89,7 @@ class Drivetrain { frc::AnalogGyro m_gyro{0}; - frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; + frc::DifferentialDriveKinematics m_kinematics{kTrackwidth}; frc::DifferentialDriveOdometry m_odometry{ m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()}, units::meter_t{m_rightEncoder.GetDistance()}}; @@ -106,5 +106,5 @@ class Drivetrain { frc::LinearSystemId::IdentifyDrivetrainSystem( 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq); frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ - m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; + m_drivetrainSystem, kTrackwidth, frc::DCMotor::CIM(2), 8, 2_in}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h index 5e95629f184..b8c0f76940a 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h @@ -80,10 +80,10 @@ inline constexpr double kPRearRightVel = 0.5; namespace ModuleConstants { inline constexpr int kEncoderCPR = 1024; -inline constexpr double kWheelDiameterMeters = 0.15; +inline constexpr auto kWheelDiameter = 0.15_m; inline constexpr double kDriveEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterMeters * std::numbers::pi) / + (kWheelDiameter.value() * std::numbers::pi) / static_cast(kEncoderCPR); inline constexpr double kTurningEncoderDistancePerPulse = diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h index fd959307a7d..9fc7386cafe 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h @@ -88,16 +88,16 @@ class DriveSubsystem : public frc2::SubsystemBase { */ void ResetOdometry(frc::Pose2d pose); - units::meter_t kTrackWidth = + units::meter_t kTrackwidth = 0.5_m; // Distance between centers of right and left wheels on robot units::meter_t kWheelBase = 0.7_m; // Distance between centers of front and back wheels on robot frc::SwerveDriveKinematics<4> kDriveKinematics{ - frc::Translation2d{kWheelBase / 2, kTrackWidth / 2}, - frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2}, - frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2}, - frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}}; + frc::Translation2d{kWheelBase / 2, kTrackwidth / 2}, + frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2}, + frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2}, + frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}}; private: // Components (e.g. motor controllers and sensors) should generally be diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java index a2c7dd55b0a..09ecb3b28cb 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java @@ -73,22 +73,13 @@ public void setData(AddressableLEDBuffer buffer) { *

By default, the driver is set up to drive WS2812B and WS2815, so nothing needs to be set for * those. * - * @param highTime0NanoSeconds high time for 0 bit (default 400ns) - * @param lowTime0NanoSeconds low time for 0 bit (default 900ns) - * @param highTime1NanoSeconds high time for 1 bit (default 900ns) - * @param lowTime1NanoSeconds low time for 1 bit (default 600ns) + * @param highTime0 high time for 0 bit in nanoseconds (default 400 ns) + * @param lowTime0 low time for 0 bit in nanoseconds (default 900 ns) + * @param highTime1 high time for 1 bit in nanoseconds (default 900 ns) + * @param lowTime1 low time for 1 bit in nanoseconds (default 600 ns) */ - public void setBitTiming( - int highTime0NanoSeconds, - int lowTime0NanoSeconds, - int highTime1NanoSeconds, - int lowTime1NanoSeconds) { - AddressableLEDJNI.setBitTiming( - m_handle, - highTime0NanoSeconds, - lowTime0NanoSeconds, - highTime1NanoSeconds, - lowTime1NanoSeconds); + public void setBitTiming(int highTime0, int lowTime0, int highTime1, int lowTime1) { + AddressableLEDJNI.setBitTiming(m_handle, highTime0, lowTime0, highTime1, lowTime1); } /** @@ -96,10 +87,10 @@ public void setBitTiming( * *

The sync time is the time to hold output so LEDs enable. Default set for WS2812B and WS2815. * - * @param syncTimeMicroSeconds the sync time (default 280us) + * @param syncTime the sync time in microseconds (default 280 μs) */ - public void setSyncTime(int syncTimeMicroSeconds) { - AddressableLEDJNI.setSyncTime(m_handle, syncTimeMicroSeconds); + public void setSyncTime(int syncTime) { + AddressableLEDJNI.setSyncTime(m_handle, syncTime); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java index ca8508422a7..c848c0a37ed 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java @@ -83,10 +83,10 @@ public int getChannel() { *

Send a single pulse on the digital output line where the pulse duration is specified in * seconds. Maximum of 65535 microseconds. * - * @param pulseLengthSeconds The pulse length in seconds + * @param pulseLength The pulse length in seconds */ - public void pulse(final double pulseLengthSeconds) { - DIOJNI.pulse(m_handle, pulseLengthSeconds); + public void pulse(final double pulseLength) { + DIOJNI.pulse(m_handle, pulseLength); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index 36721e355aa..c75c9ea1b2b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -1148,19 +1148,19 @@ public static AllianceStationID getRawAllianceStation() { /** * Wait for a DS connection. * - * @param timeoutSeconds timeout in seconds. 0 for infinite. + * @param timeout timeout in seconds. 0 for infinite. * @return true if connected, false if timeout */ - public static boolean waitForDsConnection(double timeoutSeconds) { + public static boolean waitForDsConnection(double timeout) { int event = WPIUtilJNI.createEvent(true, false); DriverStationJNI.provideNewDataEventHandle(event); boolean result; try { - if (timeoutSeconds == 0) { + if (timeout == 0) { WPIUtilJNI.waitForObject(event); result = true; } else { - result = !WPIUtilJNI.waitForObjectTimeout(event, timeoutSeconds); + result = !WPIUtilJNI.waitForObjectTimeout(event, timeout); } } catch (InterruptedException ex) { Thread.currentThread().interrupt(); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java index 1529af5bc69..b60b4604314 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java @@ -31,11 +31,11 @@ public class Notifier implements AutoCloseable { // The time, in seconds, at which the callback should be called. Has the same // zero as RobotController.getFPGATime(). - private double m_expirationTimeSeconds; + private double m_expirationTime; - // If periodic, stores the callback period; if single, stores the time until - // the callback call. - private double m_periodSeconds; + // If periodic, stores the callback period in seconds; if single, stores the time until + // the callback call in seconds. + private double m_period; // True if the callback is periodic private boolean m_periodic; @@ -75,7 +75,7 @@ private void updateAlarm(long triggerTimeMicroS) { /** Update the alarm hardware to reflect the next alarm. */ private void updateAlarm() { - updateAlarm((long) (m_expirationTimeSeconds * 1e6)); + updateAlarm((long) (m_expirationTime * 1e6)); } /** @@ -109,7 +109,7 @@ public Notifier(Runnable callback) { try { threadHandler = m_callback; if (m_periodic) { - m_expirationTimeSeconds += m_periodSeconds; + m_expirationTime += m_period; updateAlarm(); } else { // Need to update the alarm to cause it to wait again @@ -172,14 +172,14 @@ public void setCallback(Runnable callback) { /** * Run the callback once after the given delay. * - * @param delaySeconds Time in seconds to wait before the callback is called. + * @param delay Time in seconds to wait before the callback is called. */ - public void startSingle(double delaySeconds) { + public void startSingle(double delay) { m_processLock.lock(); try { m_periodic = false; - m_periodSeconds = delaySeconds; - m_expirationTimeSeconds = RobotController.getFPGATime() * 1e-6 + delaySeconds; + m_period = delay; + m_expirationTime = RobotController.getFPGATime() * 1e-6 + delay; updateAlarm(); } finally { m_processLock.unlock(); @@ -192,15 +192,15 @@ public void startSingle(double delaySeconds) { *

The user-provided callback should be written so that it completes before the next time it's * scheduled to run. * - * @param periodSeconds Period in seconds after which to to call the callback starting one period - * after the call to this method. + * @param period Period in seconds after which to to call the callback starting one period after + * the call to this method. */ - public void startPeriodic(double periodSeconds) { + public void startPeriodic(double period) { m_processLock.lock(); try { m_periodic = true; - m_periodSeconds = periodSeconds; - m_expirationTimeSeconds = RobotController.getFPGATime() * 1e-6 + periodSeconds; + m_period = period; + m_expirationTime = RobotController.getFPGATime() * 1e-6 + period; updateAlarm(); } finally { m_processLock.unlock(); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java index 0d48873eb35..9f5464257bc 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java @@ -8,6 +8,7 @@ import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDevice.Direction; import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -28,47 +29,47 @@ public class SharpIR implements Sendable, AutoCloseable { private final double m_A; private final double m_B; - private final double m_minCM; - private final double m_maxCM; + private final double m_min; // m + private final double m_max; // m /** - * Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20cm to 150cm. + * Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20 cm to 150 cm. * * @param channel Analog input channel the sensor is connected to * @return sensor object */ public static SharpIR GP2Y0A02YK0F(int channel) { - return new SharpIR(channel, 62.28, -1.092, 20, 150.0); + return new SharpIR(channel, 62.28, -1.092, 0.2, 1.5); } /** - * Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10cm to 80cm. + * Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10 cm to 80 cm. * * @param channel Analog input channel the sensor is connected to * @return sensor object */ public static SharpIR GP2Y0A21YK0F(int channel) { - return new SharpIR(channel, 26.449, -1.226, 10.0, 80.0); + return new SharpIR(channel, 26.449, -1.226, 0.1, 0.8); } /** - * Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4cm to 30cm. + * Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4 cm to 30 cm. * * @param channel Analog input channel the sensor is connected to * @return sensor object */ public static SharpIR GP2Y0A41SK0F(int channel) { - return new SharpIR(channel, 12.354, -1.07, 4.0, 30.0); + return new SharpIR(channel, 12.354, -1.07, 0.04, 0.3); } /** - * Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2cm to 15cm. + * Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2 cm to 15 cm. * * @param channel Analog input channel the sensor is connected to * @return sensor object */ public static SharpIR GP2Y0A51SK0F(int channel) { - return new SharpIR(channel, 5.2819, -1.161, 2.0, 15.0); + return new SharpIR(channel, 5.2819, -1.161, 0.02, 0.15); } /** @@ -78,24 +79,24 @@ public static SharpIR GP2Y0A51SK0F(int channel) { * @param channel AnalogInput channel * @param a Constant A * @param b Constant B - * @param minCM Minimum distance to report in centimeters - * @param maxCM Maximum distance to report in centimeters + * @param min Minimum distance to report in meters + * @param max Maximum distance to report in meters */ @SuppressWarnings("this-escape") - public SharpIR(int channel, double a, double b, double minCM, double maxCM) { + public SharpIR(int channel, double a, double b, double min, double max) { m_sensor = new AnalogInput(channel); m_A = a; m_B = b; - m_minCM = minCM; - m_maxCM = maxCM; + m_min = min; + m_max = max; HAL.reportUsage("IO", channel, "SharpIR"); SendableRegistry.add(this, "SharpIR", channel); m_simDevice = SimDevice.create("SharpIR", m_sensor.getChannel()); if (m_simDevice != null) { - m_simRange = m_simDevice.createDouble("Range (cm)", Direction.kInput, 0.0); + m_simRange = m_simDevice.createDouble("Range (m)", Direction.kInput, 0.0); m_sensor.setSimDevice(m_simDevice); } } @@ -123,37 +124,24 @@ public int getChannel() { } /** - * Get the range in inches from the distance sensor. + * Get the range in meters from the distance sensor. * - * @return range in inches of the target returned by the sensor + * @return range in meters of the target returned by the sensor */ - public double getRangeInches() { - return getRangeCM() / 2.54; - } - - /** - * Get the range in centimeters from the distance sensor. - * - * @return range in centimeters of the target returned by the sensor - */ - public double getRangeCM() { - double distance; - + public double getRange() { if (m_simRange != null) { - distance = m_simRange.get(); + return MathUtil.clamp(m_simRange.get(), m_min, m_max); } else { // Don't allow zero/negative values var v = Math.max(m_sensor.getVoltage(), 0.00001); - distance = m_A * Math.pow(v, m_B); - } - // Always constrain output - return Math.max(Math.min(distance, m_maxCM), m_minCM); + return MathUtil.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max); + } } @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Ultrasonic"); - builder.addDoubleProperty("Value", this::getRangeInches, null); + builder.addDoubleProperty("Value", this::getRange, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java index 9ffb4c6b055..8008ec4a77c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java @@ -125,11 +125,11 @@ public boolean isDisabled() { *

On the PH, the timing can be controlled in 0.001 second increments, with a maximum of 65.534 * seconds. * - * @param durationSeconds The duration of the pulse in seconds. + * @param duration The duration of the pulse in seconds. * @see #startPulse() */ - public void setPulseDuration(double durationSeconds) { - long durationMS = (long) (durationSeconds * 1000); + public void setPulseDuration(double duration) { + long durationMS = (long) (duration * 1000); m_module.setOneShotDuration(m_channel, (int) durationMS); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 50c853deb4b..0b80745ed01 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -30,18 +30,18 @@ static class Callback implements Comparable { * Construct a callback container. * * @param func The callback to run. - * @param startTimeUs The common starting point for all callback scheduling in microseconds. - * @param periodUs The period at which to run the callback in microseconds. - * @param offsetUs The offset from the common starting time in microseconds. + * @param startTime The common starting point for all callback scheduling in microseconds. + * @param period The period at which to run the callback in microseconds. + * @param offset The offset from the common starting time in microseconds. */ - Callback(Runnable func, long startTimeUs, long periodUs, long offsetUs) { + Callback(Runnable func, long startTime, long period, long offset) { this.func = func; - this.period = periodUs; + this.period = period; this.expirationTime = - startTimeUs - + offsetUs + startTime + + offset + this.period - + (RobotController.getFPGATime() - startTimeUs) / this.period * this.period; + + (RobotController.getFPGATime() - startTime) / this.period * this.period; } @Override @@ -177,10 +177,10 @@ public long getLoopStartTime() { * synchronously. Interactions between them are thread-safe. * * @param callback The callback to run. - * @param periodSeconds The period at which to run the callback in seconds. + * @param period The period at which to run the callback in seconds. */ - public final void addPeriodic(Runnable callback, double periodSeconds) { - m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (periodSeconds * 1e6), 0)); + public final void addPeriodic(Runnable callback, double period) { + m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0)); } /** @@ -190,14 +190,13 @@ public final void addPeriodic(Runnable callback, double periodSeconds) { * synchronously. Interactions between them are thread-safe. * * @param callback The callback to run. - * @param periodSeconds The period at which to run the callback in seconds. - * @param offsetSeconds The offset from the common starting time in seconds. This is useful for + * @param period The period at which to run the callback in seconds. + * @param offset The offset from the common starting time in seconds. This is useful for * scheduling a callback in a different timeslot relative to TimedRobot. */ - public final void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) { + public final void addPeriodic(Runnable callback, double period, double offset) { m_callbacks.add( - new Callback( - callback, m_startTimeUs, (long) (periodSeconds * 1e6), (long) (offsetSeconds * 1e6))); + new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6))); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java index 6d00d6402bb..2023f040fa8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -19,13 +19,13 @@ */ public class Watchdog implements Closeable, Comparable { // Used for timeout print rate-limiting - private static final long kMinPrintPeriodMicroS = (long) 1e6; + private static final long kMinPrintPeriod = (long) 1e6; // μs - private double m_startTimeSeconds; - private double m_timeoutSeconds; - private double m_expirationTimeSeconds; + private double m_startTime; + private double m_timeout; + private double m_expirationTime; private final Runnable m_callback; - private double m_lastTimeoutPrintSeconds; + private double m_lastTimeoutPrint; // s boolean m_isExpired; @@ -46,11 +46,11 @@ public class Watchdog implements Closeable, Comparable { /** * Watchdog constructor. * - * @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution. + * @param timeout The watchdog's timeout in seconds with microsecond resolution. * @param callback This function is called when the timeout expires. */ - public Watchdog(double timeoutSeconds, Runnable callback) { - m_timeoutSeconds = timeoutSeconds; + public Watchdog(double timeout, Runnable callback) { + m_timeout = timeout; m_callback = callback; m_tracer = new Tracer(); } @@ -63,19 +63,19 @@ public void close() { @Override public boolean equals(Object obj) { return obj instanceof Watchdog watchdog - && Double.compare(m_expirationTimeSeconds, watchdog.m_expirationTimeSeconds) == 0; + && Double.compare(m_expirationTime, watchdog.m_expirationTime) == 0; } @Override public int hashCode() { - return Double.hashCode(m_expirationTimeSeconds); + return Double.hashCode(m_expirationTime); } @Override public int compareTo(Watchdog rhs) { // Elements with sooner expiration times are sorted as lesser. The head of // Java's PriorityQueue is the least element. - return Double.compare(m_expirationTimeSeconds, rhs.m_expirationTimeSeconds); + return Double.compare(m_expirationTime, rhs.m_expirationTime); } /** @@ -84,25 +84,25 @@ public int compareTo(Watchdog rhs) { * @return The time in seconds since the watchdog was last fed. */ public double getTime() { - return Timer.getFPGATimestamp() - m_startTimeSeconds; + return Timer.getFPGATimestamp() - m_startTime; } /** * Sets the watchdog's timeout. * - * @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution. + * @param timeout The watchdog's timeout in seconds with microsecond resolution. */ - public void setTimeout(double timeoutSeconds) { - m_startTimeSeconds = Timer.getFPGATimestamp(); + public void setTimeout(double timeout) { + m_startTime = Timer.getFPGATimestamp(); m_tracer.clearEpochs(); m_queueMutex.lock(); try { - m_timeoutSeconds = timeoutSeconds; + m_timeout = timeout; m_isExpired = false; m_watchdogs.remove(this); - m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds; + m_expirationTime = m_startTime + m_timeout; m_watchdogs.add(this); updateAlarm(); } finally { @@ -118,7 +118,7 @@ public void setTimeout(double timeoutSeconds) { public double getTimeout() { m_queueMutex.lock(); try { - return m_timeoutSeconds; + return m_timeout; } finally { m_queueMutex.unlock(); } @@ -168,7 +168,7 @@ public void reset() { /** Enables the watchdog timer. */ public void enable() { - m_startTimeSeconds = Timer.getFPGATimestamp(); + m_startTime = Timer.getFPGATimestamp(); m_tracer.clearEpochs(); m_queueMutex.lock(); @@ -176,7 +176,7 @@ public void enable() { m_isExpired = false; m_watchdogs.remove(this); - m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds; + m_expirationTime = m_startTime + m_timeout; m_watchdogs.add(this); updateAlarm(); } finally { @@ -212,7 +212,7 @@ private static void updateAlarm() { NotifierJNI.cancelNotifierAlarm(m_notifier); } else { NotifierJNI.updateNotifierAlarm( - m_notifier, (long) (m_watchdogs.peek().m_expirationTimeSeconds * 1e6)); + m_notifier, (long) (m_watchdogs.peek().m_expirationTime * 1e6)); } } @@ -241,11 +241,11 @@ private static void schedulerFunc() { Watchdog watchdog = m_watchdogs.poll(); double now = curTime * 1e-6; - if (now - watchdog.m_lastTimeoutPrintSeconds > kMinPrintPeriodMicroS) { - watchdog.m_lastTimeoutPrintSeconds = now; + if (now - watchdog.m_lastTimeoutPrint > kMinPrintPeriod) { + watchdog.m_lastTimeoutPrint = now; if (!watchdog.m_suppressTimeoutMessage) { DriverStation.reportWarning( - String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeoutSeconds), false); + String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout), false); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java index f89b6440a8f..a9e7d361fba 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java @@ -4,19 +4,11 @@ package edu.wpi.first.wpilibj.simulation; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; - 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.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularAcceleration; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated DC motor mechanism. */ @@ -27,8 +19,8 @@ public class DCMotorSim extends LinearSystemSim { // The gearing from the motors to the output. private final double m_gearing; - // The moment of inertia for the DC motor mechanism. - private final double m_jKgMetersSquared; + // The moment of inertia for the DC motor mechanism in kg-m². + private final double m_j; /** * Creates a simulated DC motor mechanism. @@ -64,36 +56,36 @@ public DCMotorSim(LinearSystem plant, DCMotor gearbox, double... mea // // B = GKₜ/(RJ) // J = GKₜ/(RB) - m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(1, 1) / plant.getB(1, 0); - m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(1, 0)); + m_gearing = -gearbox.Kv * plant.getA(1, 1) / plant.getB(1, 0); + m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(1, 0)); } /** * Sets the state of the DC motor. * - * @param angularPositionRad The new position in radians. - * @param angularVelocityRadPerSec The new velocity in radians per second. + * @param angularPosition The new position in radians. + * @param angularVelocity The new velocity in radians per second. */ - public void setState(double angularPositionRad, double angularVelocityRadPerSec) { - setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec)); + public void setState(double angularPosition, double angularVelocity) { + setState(VecBuilder.fill(angularPosition, angularVelocity)); } /** * Sets the DC motor's angular position. * - * @param angularPositionRad The new position in radians. + * @param angularPosition The new position in radians. */ - public void setAngle(double angularPositionRad) { - setState(angularPositionRad, getAngularVelocityRadPerSec()); + public void setAngle(double angularPosition) { + setState(angularPosition, getAngularVelocity()); } /** * Sets the DC motor's angular velocity. * - * @param angularVelocityRadPerSec The new velocity in radians per second. + * @param angularVelocity The new velocity in radians per second. */ - public void setAngularVelocity(double angularVelocityRadPerSec) { - setState(getAngularPositionRad(), angularVelocityRadPerSec); + public void setAngularVelocity(double angularVelocity) { + setState(getAngularPosition(), angularVelocity); } /** @@ -108,10 +100,10 @@ public double getGearing() { /** * Returns the moment of inertia of the DC motor. * - * @return The DC motor's moment of inertia. + * @return The DC motor's moment of inertia in kg-m². */ - public double getJKgMetersSquared() { - return m_jKgMetersSquared; + public double getJ() { + return m_j; } /** @@ -126,91 +118,46 @@ public DCMotor getGearbox() { /** * Returns the DC motor's position. * - * @return The DC motor's position. + * @return The DC motor's position in meters. */ - public double getAngularPositionRad() { + public double getAngularPosition() { return getOutput(0); } - /** - * Returns the DC motor's position in rotations. - * - * @return The DC motor's position in rotations. - */ - public double getAngularPositionRotations() { - return Units.radiansToRotations(getAngularPositionRad()); - } - - /** - * Returns the DC motor's position. - * - * @return The DC motor's position - */ - public Angle getAngularPosition() { - return Radians.of(getAngularPositionRad()); - } - /** * Returns the DC motor's velocity. * * @return The DC motor's velocity. */ - public double getAngularVelocityRadPerSec() { + public double getAngularVelocity() { return getOutput(1); } /** - * Returns the DC motor's velocity in RPM. - * - * @return The DC motor's velocity in RPM. - */ - public double getAngularVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec()); - } - - /** - * Returns the DC motor's velocity. - * - * @return The DC motor's velocity - */ - public AngularVelocity getAngularVelocity() { - return RadiansPerSecond.of(getAngularVelocityRadPerSec()); - } - - /** - * Returns the DC motor's acceleration in Radians Per Second Squared. + * Returns the DC motor's acceleration. * - * @return The DC motor's acceleration in Radians Per Second Squared. + * @return The DC motor's acceleration in rad/s². */ - public double getAngularAccelerationRadPerSecSq() { + public double getAngularAcceleration() { var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u)); return acceleration.get(1, 0); } /** - * Returns the DC motor's acceleration. - * - * @return The DC motor's acceleration. - */ - public AngularAcceleration getAngularAcceleration() { - return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq()); - } - - /** - * Returns the DC motor's torque in Newton-Meters. + * Returns the DC motor's torque in. * - * @return The DC motor's torque in Newton-Meters. + * @return The DC motor's torque in Newton-meters. */ - public double getTorqueNewtonMeters() { - return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared; + public double getTorque() { + return getAngularAcceleration() * m_j; } /** * Returns the DC motor's current draw. * - * @return The DC motor's current draw. + * @return The DC motor's current draw in amps. */ - public double getCurrentDrawAmps() { + public double getCurrentDraw() { // I = V / R - omega / (Kv * R) // Reductions are output over input, so a reduction of 2:1 means the motor is spinning // 2x faster than the output diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index 6e25b344393..5ecfd55fcae 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -42,7 +42,7 @@ public class DifferentialDrivetrainSim { private final double m_originalGearing; private final Matrix m_measurementStdDevs; private double m_currentGearing; - private final double m_wheelRadiusMeters; + private final double m_wheelRadius; private Matrix m_u; private Matrix m_x; @@ -57,10 +57,10 @@ public class DifferentialDrivetrainSim { * @param driveMotor A {@link DCMotor} representing the left side of the drivetrain. * @param gearing The gearing ratio between motor and wheel, as output over input. This must be * the same ratio as the ratio used to identify or create the drivetrainPlant. - * @param jKgMetersSquared The moment of inertia of the drivetrain about its center. - * @param massKg The mass of the drivebase. - * @param wheelRadiusMeters The radius of the wheels on the drivetrain. - * @param trackWidthMeters The robot's track width, or distance between left and right wheels. + * @param j The moment of inertia of the drivetrain about its center in kg-m². + * @param mass The mass of the drivebase in kg. + * @param wheelRadius The radius of the wheels on the drivetrain in meters. + * @param trackwidth The robot's trackwidth, or distance between left and right wheels, in meters. * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, * left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is * desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 @@ -70,23 +70,18 @@ public class DifferentialDrivetrainSim { public DifferentialDrivetrainSim( DCMotor driveMotor, double gearing, - double jKgMetersSquared, - double massKg, - double wheelRadiusMeters, - double trackWidthMeters, + double j, + double mass, + double wheelRadius, + double trackwidth, Matrix measurementStdDevs) { this( LinearSystemId.createDrivetrainVelocitySystem( - driveMotor, - massKg, - wheelRadiusMeters, - trackWidthMeters / 2.0, - jKgMetersSquared, - gearing), + driveMotor, mass, wheelRadius, trackwidth / 2.0, j, gearing), driveMotor, gearing, - trackWidthMeters, - wheelRadiusMeters, + trackwidth, + wheelRadius, measurementStdDevs); } @@ -102,9 +97,9 @@ public DifferentialDrivetrainSim( * @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 * ratio as the ratio used to identify or create the drivetrainPlant. - * @param trackWidthMeters The distance between the two sides of the drivetrain. Can be found with - * SysId. - * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in meters. + * @param trackwidth The distance between the two sides of the drivetrain in meters. Can be found + * with SysId. + * @param wheelRadius The radius of the wheels on the drivetrain, in meters. * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, * left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is * desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 @@ -115,15 +110,15 @@ public DifferentialDrivetrainSim( LinearSystem plant, DCMotor driveMotor, double gearing, - double trackWidthMeters, - double wheelRadiusMeters, + double trackwidth, + double wheelRadius, Matrix measurementStdDevs) { this.m_plant = plant; - this.m_rb = trackWidthMeters / 2.0; + this.m_rb = trackwidth / 2.0; this.m_motor = driveMotor; this.m_originalGearing = gearing; this.m_measurementStdDevs = measurementStdDevs; - m_wheelRadiusMeters = wheelRadiusMeters; + m_wheelRadius = wheelRadius; m_currentGearing = m_originalGearing; m_x = new Matrix<>(Nat.N7(), Nat.N1()); @@ -145,10 +140,10 @@ public void setInputs(double leftVoltageVolts, double rightVoltageVolts) { /** * Update the drivetrain states with the current time difference. * - * @param dtSeconds the time difference + * @param dt the time difference in seconds */ - public void update(double dtSeconds) { - m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dtSeconds); + public void update(double dt) { + m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dt); m_y = m_x; if (m_measurementStdDevs != null) { m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs)); @@ -195,38 +190,38 @@ public Pose2d getPose() { } /** - * Get the right encoder position in meters. + * Get the right encoder position. * - * @return The encoder position. + * @return The encoder position in meters. */ - public double getRightPositionMeters() { + public double getRightPosition() { return getOutput(State.kRightPosition); } /** - * Get the right encoder velocity in meters per second. + * Get the right encoder velocity. * - * @return The encoder velocity. + * @return The encoder velocity in meters per second. */ - public double getRightVelocityMetersPerSecond() { + public double getRightVelocity() { return getOutput(State.kRightVelocity); } /** - * Get the left encoder position in meters. + * Get the left encoder position. * - * @return The encoder position. + * @return The encoder position in meters. */ - public double getLeftPositionMeters() { + public double getLeftPosition() { return getOutput(State.kLeftPosition); } /** - * Get the left encoder velocity in meters per second. + * Get the left encoder velocity. * - * @return The encoder velocity. + * @return The encoder velocity in meters per second. */ - public double getLeftVelocityMetersPerSecond() { + public double getLeftVelocity() { return getOutput(State.kLeftVelocity); } @@ -235,9 +230,9 @@ public double getLeftVelocityMetersPerSecond() { * * @return the drivetrain's left side current draw, in amps */ - public double getLeftCurrentDrawAmps() { + public double getLeftCurrentDraw() { return m_motor.getCurrent( - getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(0, 0)) + getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadius, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); } @@ -246,9 +241,9 @@ public double getLeftCurrentDrawAmps() { * * @return the drivetrain's right side current draw, in amps */ - public double getRightCurrentDrawAmps() { + public double getRightCurrentDraw() { return m_motor.getCurrent( - getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(1, 0)) + getState(State.kRightVelocity) * m_currentGearing / m_wheelRadius, m_u.get(1, 0)) * Math.signum(m_u.get(1, 0)); } @@ -257,8 +252,8 @@ public double getRightCurrentDrawAmps() { * * @return the current draw, in amps */ - public double getCurrentDrawAmps() { - return getLeftCurrentDrawAmps() + getRightCurrentDrawAmps(); + public double getCurrentDraw() { + return getLeftCurrentDraw() + getRightCurrentDraw(); } /** @@ -472,8 +467,7 @@ public static DifferentialDrivetrainSim createKitbotSim( * @param motor The motors installed in the bot. * @param gearing The gearing reduction used. * @param wheelSize The wheel size. - * @param jKgMetersSquared The moment of inertia of the drivebase. This can be calculated using - * SysId. + * @param j The moment of inertia of the drivebase in kg-m². This can be calculated using SysId. * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, * left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is * desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 @@ -485,12 +479,12 @@ public static DifferentialDrivetrainSim createKitbotSim( KitbotMotor motor, KitbotGearing gearing, KitbotWheelSize wheelSize, - double jKgMetersSquared, + double j, Matrix measurementStdDevs) { return new DifferentialDrivetrainSim( motor.value, gearing.value, - jKgMetersSquared, + j, Units.lbsToKilograms(60), wheelSize.value / 2.0, Units.inchesToMeters(26), diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 2f7ce3e768d..f18ad7a49d7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -36,10 +36,10 @@ public class ElevatorSim extends LinearSystemSim { * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, * double, double)}. * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. + * @param minHeight The min allowable height of the elevator in meters. + * @param maxHeight The max allowable height of the elevator in meters. * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeightMeters The starting height of the elevator. + * @param startingHeight The starting height of the elevator in meters. * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 1 element for position. */ @@ -47,18 +47,18 @@ public class ElevatorSim extends LinearSystemSim { public ElevatorSim( LinearSystem plant, DCMotor gearbox, - double minHeightMeters, - double maxHeightMeters, + double minHeight, + double maxHeight, boolean simulateGravity, - double startingHeightMeters, + double startingHeight, double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; - m_minHeight = minHeightMeters; - m_maxHeight = maxHeightMeters; + m_minHeight = minHeight; + m_maxHeight = maxHeight; m_simulateGravity = simulateGravity; - setState(startingHeightMeters, 0); + setState(startingHeight, 0); } /** @@ -67,10 +67,10 @@ public ElevatorSim( * @param kV The velocity gain. * @param kA The acceleration gain. * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. + * @param minHeight The min allowable height of the elevator in meters. + * @param maxHeight The max allowable height of the elevator in meters. * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeightMeters The starting height of the elevator. + * @param startingHeight The starting height of the elevator in meters. * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 1 element for position. */ @@ -78,18 +78,18 @@ public ElevatorSim( double kV, double kA, DCMotor gearbox, - double minHeightMeters, - double maxHeightMeters, + double minHeight, + double maxHeight, boolean simulateGravity, - double startingHeightMeters, + double startingHeight, double... measurementStdDevs) { this( LinearSystemId.identifyPositionSystem(kV, kA), gearbox, - minHeightMeters, - maxHeightMeters, + minHeight, + maxHeight, simulateGravity, - startingHeightMeters, + startingHeight, measurementStdDevs); } @@ -98,32 +98,32 @@ public ElevatorSim( * * @param gearbox The type of and number of motors in the elevator gearbox. * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param carriageMassKg The mass of the elevator carriage. - * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. + * @param carriageMass The mass of the elevator carriage in kg. + * @param drumRadius The radius of the drum that the elevator spool is wrapped around in meters. + * @param minHeight The min allowable height of the elevator in meters. + * @param maxHeight The max allowable height of the elevator in meters. * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeightMeters The starting height of the elevator. + * @param startingHeight The starting height of the elevator in meters. * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 1 element for position. */ public ElevatorSim( DCMotor gearbox, double gearing, - double carriageMassKg, - double drumRadiusMeters, - double minHeightMeters, - double maxHeightMeters, + double carriageMass, + double drumRadius, + double minHeight, + double maxHeight, boolean simulateGravity, - double startingHeightMeters, + double startingHeight, double... measurementStdDevs) { this( - LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), + LinearSystemId.createElevatorSystem(gearbox, carriageMass, drumRadius, gearing), gearbox, - minHeightMeters, - maxHeightMeters, + minHeight, + maxHeight, simulateGravity, - startingHeightMeters, + startingHeight, measurementStdDevs); } @@ -131,33 +131,31 @@ public ElevatorSim( * Sets the elevator's state. The new position will be limited between the minimum and maximum * allowed heights. * - * @param positionMeters The new position in meters. - * @param velocityMetersPerSecond New velocity in meters per second. + * @param position The new position in meters. + * @param velocity New velocity in meters per second. */ - public final void setState(double positionMeters, double velocityMetersPerSecond) { - setState( - VecBuilder.fill( - MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond)); + public final void setState(double position, double velocity) { + setState(VecBuilder.fill(MathUtil.clamp(position, m_minHeight, m_maxHeight), velocity)); } /** * Returns whether the elevator would hit the lower limit. * - * @param elevatorHeightMeters The elevator height. + * @param elevatorHeight The elevator height in meters. * @return Whether the elevator would hit the lower limit. */ - public boolean wouldHitLowerLimit(double elevatorHeightMeters) { - return elevatorHeightMeters <= this.m_minHeight; + public boolean wouldHitLowerLimit(double elevatorHeight) { + return elevatorHeight <= this.m_minHeight; } /** * Returns whether the elevator would hit the upper limit. * - * @param elevatorHeightMeters The elevator height. + * @param elevatorHeight The elevator height in meters. * @return Whether the elevator would hit the upper limit. */ - public boolean wouldHitUpperLimit(double elevatorHeightMeters) { - return elevatorHeightMeters >= this.m_maxHeight; + public boolean wouldHitUpperLimit(double elevatorHeight) { + return elevatorHeight >= this.m_maxHeight; } /** @@ -166,7 +164,7 @@ public boolean wouldHitUpperLimit(double elevatorHeightMeters) { * @return Whether the elevator has hit the lower limit. */ public boolean hasHitLowerLimit() { - return wouldHitLowerLimit(getPositionMeters()); + return wouldHitLowerLimit(getPosition()); } /** @@ -175,43 +173,42 @@ public boolean hasHitLowerLimit() { * @return Whether the elevator has hit the upper limit. */ public boolean hasHitUpperLimit() { - return wouldHitUpperLimit(getPositionMeters()); + return wouldHitUpperLimit(getPosition()); } /** * Returns the position of the elevator. * - * @return The position of the elevator. + * @return The position of the elevator in meters. */ - public double getPositionMeters() { + public double getPosition() { return getOutput(0); } /** * Returns the velocity of the elevator. * - * @return The velocity of the elevator. + * @return The velocity of the elevator in meters per second. */ - public double getVelocityMetersPerSecond() { + public double getVelocity() { return getOutput(1); } /** * Returns the elevator current draw. * - * @return The elevator current draw. + * @return The elevator current draw in amps. */ - public double getCurrentDrawAmps() { + public double getCurrentDraw() { // I = V / R - omega / (Kv * R) // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is // spinning 10x faster than the output // v = r w, so w = v/r double kA = 1 / m_plant.getB().get(1, 0); double kV = -m_plant.getA().get(1, 1) * kA; - double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.KvRadPerSecPerVolt; + double motorVelocity = m_x.get(1, 0) * kV * m_gearbox.Kv; var appliedVoltage = m_u.get(0, 0); - return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) - * Math.signum(appliedVoltage); + return m_gearbox.getCurrent(motorVelocity, appliedVoltage) * Math.signum(appliedVoltage); } /** @@ -229,10 +226,10 @@ public void setInputVoltage(double volts) { * * @param currentXhat The current state estimate. * @param u The system inputs (voltage). - * @param dtSeconds The time difference between controller updates. + * @param dt The time difference between controller updates in seconds. */ @Override - protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { + protected Matrix updateX(Matrix currentXhat, Matrix u, double dt) { // Calculate updated x-hat from Runge-Kutta. var updatedXhat = NumericalIntegration.rkdp( @@ -245,7 +242,7 @@ protected Matrix updateX(Matrix currentXhat, Matrix u, d }, currentXhat, u, - dtSeconds); + dt); // We check for collisions after updating x-hat. if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java index 95d01e16217..e9e72b0894c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java @@ -4,17 +4,11 @@ package edu.wpi.first.wpilibj.simulation; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; - import edu.wpi.first.math.VecBuilder; 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; -import edu.wpi.first.units.measure.AngularAcceleration; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated flywheel mechanism. */ @@ -26,7 +20,7 @@ public class FlywheelSim extends LinearSystemSim { private final double m_gearing; // The moment of inertia for the flywheel mechanism. - private final double m_jKgMetersSquared; + private final double m_j; /** * Creates a simulated flywheel mechanism. @@ -60,17 +54,17 @@ public FlywheelSim( // // B = GKₜ/(RJ) // J = GKₜ/(RB) - m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(0, 0) / plant.getB(0, 0); - m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(0, 0)); + m_gearing = -gearbox.Kv * plant.getA(0, 0) / plant.getB(0, 0); + m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(0, 0)); } /** * Sets the flywheel's angular velocity. * - * @param velocityRadPerSec The new velocity in radians per second. + * @param velocity The new velocity in radians per second. */ - public void setAngularVelocity(double velocityRadPerSec) { - setState(VecBuilder.fill(velocityRadPerSec)); + public void setAngularVelocity(double velocity) { + setState(VecBuilder.fill(velocity)); } /** @@ -83,12 +77,12 @@ public double getGearing() { } /** - * Returns the moment of inertia in kilograms meters squared. + * Returns the moment of inertia. * - * @return The flywheel's moment of inertia. + * @return The flywheel's moment of inertia in kg-m². */ - public double getJKgMetersSquared() { - return m_jKgMetersSquared; + public double getJ() { + return m_j; } /** @@ -100,67 +94,40 @@ public DCMotor getGearbox() { return m_gearbox; } - /** - * Returns the flywheel's velocity in Radians Per Second. - * - * @return The flywheel's velocity in Radians Per Second. - */ - public double getAngularVelocityRadPerSec() { - return getOutput(0); - } - - /** - * Returns the flywheel's velocity in RPM. - * - * @return The flywheel's velocity in RPM. - */ - public double getAngularVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec()); - } - /** * Returns the flywheel's velocity. * - * @return The flywheel's velocity + * @return The flywheel's velocity in rad/s. */ - public AngularVelocity getAngularVelocity() { - return RadiansPerSecond.of(getAngularVelocityRadPerSec()); + public double getAngularVelocity() { + return getOutput(0); } /** - * Returns the flywheel's acceleration in Radians Per Second Squared. + * Returns the flywheel's acceleration. * - * @return The flywheel's acceleration in Radians Per Second Squared. + * @return The flywheel's acceleration in rad/s². */ - public double getAngularAccelerationRadPerSecSq() { + public double getAngularAcceleration() { var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u)); return acceleration.get(0, 0); } /** - * Returns the flywheel's acceleration. - * - * @return The flywheel's acceleration. - */ - public AngularAcceleration getAngularAcceleration() { - return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq()); - } - - /** - * Returns the flywheel's torque in Newton-Meters. + * Returns the flywheel's torque. * - * @return The flywheel's torque in Newton-Meters. + * @return The flywheel's torque in Newton-meters. */ - public double getTorqueNewtonMeters() { - return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared; + public double getTorque() { + return getAngularAcceleration() * m_j; } /** * Returns the flywheel's current draw. * - * @return The flywheel's current draw. + * @return The flywheel's current draw in amps. */ - public double getCurrentDrawAmps() { + public double getCurrentDraw() { // I = V / R - omega / (Kv * R) // Reductions are output over input, so a reduction of 2:1 means the motor is spinning // 2x faster than the flywheel diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java index ec296fde3d8..87ecdae577f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java @@ -72,11 +72,11 @@ public LinearSystemSim( /** * Updates the simulation. * - * @param dtSeconds The time between updates. + * @param dt The time between updates in seconds. */ - public void update(double dtSeconds) { + public void update(double dt) { // Update X. By default, this is the linear system dynamics X = Ax + Bu - m_x = updateX(m_x, m_u, dtSeconds); + m_x = updateX(m_x, m_u, dt); // y = cx + du m_y = m_plant.calculateY(m_x, m_u); @@ -171,12 +171,12 @@ public void setState(Matrix state) { * * @param currentXhat The current state estimate. * @param u The system inputs (usually voltage). - * @param dtSeconds The time difference between controller updates. + * @param dt The time difference between controller updates in seconds. * @return The new state. */ protected Matrix updateX( - Matrix currentXhat, Matrix u, double dtSeconds) { - return m_plant.calculateX(currentXhat, u, dtSeconds); + Matrix currentXhat, Matrix u, double dt) { + return m_plant.calculateX(currentXhat, u, dt); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java index eaf3dcb8b10..4d90f2c5c0b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java @@ -5,7 +5,6 @@ package edu.wpi.first.wpilibj.simulation; import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.SharpIR; /** Simulation class for Sharp IR sensors. */ @@ -28,24 +27,15 @@ public SharpIRSim(SharpIR sharpIR) { */ public SharpIRSim(int channel) { SimDeviceSim simDevice = new SimDeviceSim("SharpIR", channel); - m_simRange = simDevice.getDouble("Range (cm)"); + m_simRange = simDevice.getDouble("Range (m)"); } /** - * Set the range in inches returned by the distance sensor. + * Set the range in meters returned by the distance sensor. * - * @param inches range in inches of the target returned by the sensor + * @param range range in meters of the target returned by the sensor */ - public void setRangeInches(double inches) { - m_simRange.set(Units.inchesToMeters(inches) * 100.0); - } - - /** - * Set the range in centimeters returned by the distance sensor. - * - * @param cm range in centimeters of the target returned by the sensor - */ - public void setRangeCm(double cm) { - m_simRange.set(cm); + public void setRange(double range) { + m_simRange.set(range); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java index f637b01d708..4cf35ab4266 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java @@ -65,18 +65,18 @@ public static boolean isTimingPaused() { /** * Advance the simulator time and wait for all notifiers to run. * - * @param deltaSeconds the amount to advance (in seconds) + * @param delta the amount to advance in seconds */ - public static void stepTiming(double deltaSeconds) { - SimulatorJNI.stepTiming((long) (deltaSeconds * 1e6)); + public static void stepTiming(double delta) { + SimulatorJNI.stepTiming((long) (delta * 1e6)); } /** * Advance the simulator time and return immediately. * - * @param deltaSeconds the amount to advance (in seconds) + * @param delta the amount to advance in seconds */ - public static void stepTimingAsync(double deltaSeconds) { - SimulatorJNI.stepTimingAsync((long) (deltaSeconds * 1e6)); + public static void stepTimingAsync(double delta) { + SimulatorJNI.stepTimingAsync((long) (delta * 1e6)); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index 7d7ae78284d..af8a68cc495 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -24,7 +24,7 @@ public class SingleJointedArmSim extends LinearSystemSim { private final double m_gearing; // The length of the arm. - private final double m_armLenMeters; + private final double m_armLength; // The minimum angle that the arm is capable of. private final double m_minAngle; @@ -43,7 +43,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * double, double)}. * @param gearbox The type of and number of motors in the arm gearbox. * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). - * @param armLengthMeters The length of the arm. + * @param armLength The length of the arm in meters. * @param minAngleRads The minimum angle that the arm is capable of. * @param maxAngleRads The maximum angle that the arm is capable of. * @param simulateGravity Whether gravity should be simulated or not. @@ -56,7 +56,7 @@ public SingleJointedArmSim( LinearSystem plant, DCMotor gearbox, double gearing, - double armLengthMeters, + double armLength, double minAngleRads, double maxAngleRads, boolean simulateGravity, @@ -65,7 +65,7 @@ public SingleJointedArmSim( super(plant, measurementStdDevs); m_gearbox = gearbox; m_gearing = gearing; - m_armLenMeters = armLengthMeters; + m_armLength = armLength; m_minAngle = minAngleRads; m_maxAngle = maxAngleRads; m_simulateGravity = simulateGravity; @@ -78,8 +78,8 @@ public SingleJointedArmSim( * * @param gearbox The type of and number of motors in the arm gearbox. * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). - * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software. - * @param armLengthMeters The length of the arm. + * @param j The moment of inertia of the arm in kg-m²; can be calculated from CAD software. + * @param armLength The length of the arm in meters. * @param minAngleRads The minimum angle that the arm is capable of. * @param maxAngleRads The maximum angle that the arm is capable of. * @param simulateGravity Whether gravity should be simulated or not. @@ -90,18 +90,18 @@ public SingleJointedArmSim( public SingleJointedArmSim( DCMotor gearbox, double gearing, - double jKgMetersSquared, - double armLengthMeters, + double j, + double armLength, double minAngleRads, double maxAngleRads, boolean simulateGravity, double startingAngleRads, double... measurementStdDevs) { this( - LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing), + LinearSystemId.createSingleJointedArmSystem(gearbox, j, gearing), gearbox, gearing, - armLengthMeters, + armLength, minAngleRads, maxAngleRads, simulateGravity, @@ -147,7 +147,7 @@ public boolean wouldHitUpperLimit(double currentAngleRads) { * @return Whether the arm has hit the lower limit. */ public boolean hasHitLowerLimit() { - return wouldHitLowerLimit(getAngleRads()); + return wouldHitLowerLimit(getAngle()); } /** @@ -156,33 +156,33 @@ public boolean hasHitLowerLimit() { * @return Whether the arm has hit the upper limit. */ public boolean hasHitUpperLimit() { - return wouldHitUpperLimit(getAngleRads()); + return wouldHitUpperLimit(getAngle()); } /** * Returns the current arm angle. * - * @return The current arm angle. + * @return The current arm angle in radians. */ - public double getAngleRads() { + public double getAngle() { return getOutput(0); } /** * Returns the current arm velocity. * - * @return The current arm velocity. + * @return The current arm velocity in radians per second. */ - public double getVelocityRadPerSec() { + public double getVelocity() { return getOutput(1); } /** * Returns the arm current draw. * - * @return The arm current draw. + * @return The arm current draw in amps. */ - public double getCurrentDrawAmps() { + public double getCurrentDraw() { // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is // spinning 10x faster than the output var motorVelocity = m_x.get(1, 0) * m_gearing; @@ -202,12 +202,12 @@ public void setInputVoltage(double volts) { /** * Calculates a rough estimate of the moment of inertia of an arm given its length and mass. * - * @param lengthMeters The length of the arm. - * @param massKg The mass of the arm. - * @return The calculated moment of inertia. + * @param length The length of the arm in m. + * @param mass The mass of the arm in kg. + * @return The calculated moment of inertia in kg-m². */ - public static double estimateMOI(double lengthMeters, double massKg) { - return 1.0 / 3.0 * massKg * lengthMeters * lengthMeters; + public static double estimateMOI(double length, double mass) { + return 1.0 / 3.0 * mass * length * length; } /** @@ -215,10 +215,10 @@ public static double estimateMOI(double lengthMeters, double massKg) { * * @param currentXhat The current state estimate. * @param u The system inputs (voltage). - * @param dtSeconds The time difference between controller updates. + * @param dt The time difference between controller updates in seconds. */ @Override - protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { + protected Matrix updateX(Matrix currentXhat, Matrix u, double dt) { // The torque on the arm is given by τ = F⋅r, where F is the force applied by // gravity and r the distance from pivot to center of mass. Recall from // dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is @@ -248,14 +248,14 @@ protected Matrix updateX(Matrix currentXhat, Matrix u, d (Matrix x, Matrix _u) -> { Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); if (m_simulateGravity) { - double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLenMeters; + double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLength; xdot = xdot.plus(VecBuilder.fill(0, alphaGrav)); } return xdot; }, currentXhat, u, - dtSeconds); + dt); // We check for collision after updating xhat if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java index 7ce6af8c3e8..749e2f843e6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java @@ -58,12 +58,12 @@ public synchronized void setRobotPose(Pose2d pose) { /** * Set the robot pose from x, y, and rotation. * - * @param xMeters X location, in meters - * @param yMeters Y location, in meters + * @param x X location, in meters + * @param y Y location, in meters * @param rotation rotation */ - public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) { - m_objects.get(0).setPose(xMeters, yMeters, rotation); + public synchronized void setRobotPose(double x, double y, Rotation2d rotation) { + m_objects.get(0).setPose(x, y, rotation); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java index f3627a6bef1..c943a1d317f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java @@ -43,12 +43,12 @@ public synchronized void setPose(Pose2d pose) { /** * Set the pose from x, y, and rotation. * - * @param xMeters X location, in meters - * @param yMeters Y location, in meters + * @param x X location, in meters + * @param y Y location, in meters * @param rotation rotation */ - public synchronized void setPose(double xMeters, double yMeters, Rotation2d rotation) { - setPose(new Pose2d(xMeters, yMeters, rotation)); + public synchronized void setPose(double x, double y, Rotation2d rotation) { + setPose(new Pose2d(x, y, rotation)); } /** @@ -94,7 +94,7 @@ public synchronized void setPoses(Pose2d... poses) { public synchronized void setTrajectory(Trajectory trajectory) { m_poses.clear(); for (Trajectory.State state : trajectory.getStates()) { - m_poses.add(state.poseMeters); + m_poses.add(state.pose); } updateEntry(); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SharpIRTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SharpIRTest.java index 1ae35ae781d..4049dc805ab 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SharpIRTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SharpIRTest.java @@ -15,13 +15,13 @@ void testSharpIR() { try (SharpIR s = SharpIR.GP2Y0A02YK0F(1)) { SharpIRSim sim = new SharpIRSim(s); - assertEquals(20, s.getRangeCM()); + assertEquals(0.2, s.getRange()); - sim.setRangeCm(30); - assertEquals(30, s.getRangeCM()); + sim.setRange(0.3); + assertEquals(0.3, s.getRange()); - sim.setRangeCm(300); - assertEquals(150, s.getRangeCM()); + sim.setRange(3); + assertEquals(1.5, s.getRange()); } } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java index b7a4a6dd875..d7f141eb56e 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java @@ -36,23 +36,23 @@ void testVoltageSteadyState() { // ------ SimulationPeriodic() happens after user code ------- RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw())); sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage()); sim.update(0.020); - encoderSim.setRate(sim.getAngularVelocityRadPerSec()); + encoderSim.setRate(sim.getAngularVelocity()); } - assertEquals(gearbox.KvRadPerSecPerVolt * 12, encoder.getRate(), 0.1); + assertEquals(gearbox.Kv * 12, encoder.getRate(), 0.1); for (int i = 0; i < 100; i++) { motor.setVoltage(0); // ------ SimulationPeriodic() happens after user code ------- RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw())); sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage()); sim.update(0.020); - encoderSim.setRate(sim.getAngularVelocityRadPerSec()); + encoderSim.setRate(sim.getAngularVelocity()); } assertEquals(0, encoder.getRate(), 0.1); @@ -78,11 +78,11 @@ void testPositionFeedbackControl() { // ------ SimulationPeriodic() happens after user code ------- RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw())); sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage()); sim.update(0.020); - encoderSim.setDistance(sim.getAngularPositionRad()); - encoderSim.setRate(sim.getAngularVelocityRadPerSec()); + encoderSim.setDistance(sim.getAngularPosition()); + encoderSim.setRate(sim.getAngularVelocity()); } assertEquals(750, encoder.getDistance(), 1.0); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java index 25bbdb59834..9d767dee210 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java @@ -42,7 +42,7 @@ void testConvergence() { plant, motor, 1, - kinematics.trackWidthMeters, + kinematics.trackwidth, Units.inchesToMeters(2), VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005)); @@ -61,15 +61,13 @@ void testConvergence() { new TrajectoryConfig(1, 1) .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1))); - for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) { + for (double t = 0; t < traj.getTotalTime(); t += 0.020) { var state = traj.sample(t); var feedbackOut = feedback.calculate(sim.getPose(), state); var wheelSpeeds = kinematics.toWheelSpeeds(feedbackOut); - var voltages = - feedforward.calculate( - VecBuilder.fill(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond)); + var voltages = feedforward.calculate(VecBuilder.fill(wheelSpeeds.left, wheelSpeeds.right)); // Sim periodic code sim.setInputs(voltages.get(0, 0), voltages.get(1, 0)); @@ -104,25 +102,25 @@ void testCurrent() { var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); var sim = new DifferentialDrivetrainSim( - plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2), null); + plant, motor, 1, kinematics.trackwidth, Units.inchesToMeters(2), null); sim.setInputs(-12, -12); for (int i = 0; i < 10; i++) { sim.update(0.020); } - assertTrue(sim.getCurrentDrawAmps() > 0); + assertTrue(sim.getCurrentDraw() > 0); sim.setInputs(12, 12); for (int i = 0; i < 20; i++) { sim.update(0.020); } - assertTrue(sim.getCurrentDrawAmps() > 0); + assertTrue(sim.getCurrentDraw() > 0); sim.setInputs(-12, 12); for (int i = 0; i < 30; i++) { sim.update(0.020); } - assertTrue(sim.getCurrentDrawAmps() > 0); + assertTrue(sim.getCurrentDraw() > 0); } @Test @@ -138,7 +136,7 @@ void testModelStability() { plant, motor, 5, - kinematics.trackWidthMeters, + kinematics.trackwidth, Units.inchesToMeters(2), VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005)); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java index deade245fa1..260bc2772aa 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java @@ -59,7 +59,7 @@ void testStateSpaceSimWithElevator() { encoderSim.setDistance(y.get(0, 0)); } - assertEquals(controller.getSetpoint(), sim.getPositionMeters(), 0.2); + assertEquals(controller.getSetpoint(), sim.getPosition(), 0.2); } } @@ -81,14 +81,14 @@ void testMinMax() { for (int i = 0; i < 100; i++) { sim.setInput(VecBuilder.fill(0)); sim.update(0.020); - var height = sim.getPositionMeters(); + var height = sim.getPosition(); assertTrue(height >= -0.05); } for (int i = 0; i < 100; i++) { sim.setInput(VecBuilder.fill(12.0)); sim.update(0.020); - var height = sim.getPositionMeters(); + var height = sim.getPosition(); assertTrue(height <= 1.05); } } @@ -110,7 +110,7 @@ void testStability() { DCMotor.getVex775Pro(4), 4, Units.inchesToMeters(0.5), 100); assertEquals( system.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0), - sim.getPositionMeters(), + sim.getPosition(), 0.01); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java index 928fd3edc89..4cbd7db4fb7 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java @@ -34,6 +34,6 @@ void testArmDisabled() { } // the arm should swing down - assertEquals(-Math.PI / 2.0, m_sim.getAngleRads(), 0.1); + assertEquals(-Math.PI / 2.0, m_sim.getAngle(), 0.1); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java index 3c92370019e..33613a995bd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java @@ -65,7 +65,7 @@ public class Arm implements AutoCloseable { new MechanismLigament2d( "Arm", 30, - Units.radiansToDegrees(m_armSim.getAngleRads()), + Units.radiansToDegrees(m_armSim.getAngle()), 6, new Color8Bit(Color.kYellow))); @@ -92,13 +92,13 @@ public void simulationPeriodic() { m_armSim.update(0.020); // Finally, we set our simulated encoder's readings and simulated battery voltage - m_encoderSim.setDistance(m_armSim.getAngleRads()); + m_encoderSim.setDistance(m_armSim.getAngle()); // SimBattery estimates loaded battery voltages RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps())); + BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDraw())); // Update the Mechanism Arm angle based on the simulated arm angle - m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads())); + m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngle())); } /** Load setpoint and kP from preferences. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java index adc52e2ef67..cce08317330 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java @@ -19,7 +19,7 @@ public class Drivetrain { public static final double kMaxSpeed = 3.0; // meters per second public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second - private static final double kTrackWidth = 0.381 * 2; // meters + private static final double kTrackwidth = 0.381 * 2; // meters private static final double kWheelRadius = 0.0508; // meters private static final int kEncoderResolution = 4096; @@ -37,7 +37,7 @@ public class Drivetrain { private final PIDController m_rightPIDController = new PIDController(1, 0, 0); private final DifferentialDriveKinematics m_kinematics = - new DifferentialDriveKinematics(kTrackWidth); + new DifferentialDriveKinematics(kTrackwidth); private final DifferentialDriveOdometry m_odometry; @@ -79,13 +79,12 @@ public Drivetrain() { * @param speeds The desired wheel speeds. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); - final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); + final double leftFeedforward = m_feedforward.calculate(speeds.left); + final double rightFeedforward = m_feedforward.calculate(speeds.right); - final double leftOutput = - m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); + final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left); final double rightOutput = - m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond); + m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right); m_leftLeader.setVoltage(leftOutput + leftFeedforward); m_rightLeader.setVoltage(rightOutput + rightFeedforward); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index e1e0607100f..e23bc65e427 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -42,7 +42,7 @@ public class Drivetrain { public static final double kMaxSpeed = 3.0; // meters per second public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second - private static final double kTrackWidth = 0.381 * 2; // meters + private static final double kTrackwidth = 0.381 * 2; // meters private static final double kWheelRadius = 0.0508; // meters private static final int kEncoderResolution = 4096; @@ -60,7 +60,7 @@ public class Drivetrain { private final PIDController m_rightPIDController = new PIDController(1, 0, 0); private final DifferentialDriveKinematics m_kinematics = - new DifferentialDriveKinematics(kTrackWidth); + new DifferentialDriveKinematics(kTrackwidth); private final Pose3d m_objectInField; @@ -96,7 +96,7 @@ public class Drivetrain { LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); private final DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim( - m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null); + m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null); /** * Constructs a differential drive object. Sets the encoder distance per pulse and resets the @@ -137,13 +137,12 @@ public Drivetrain(DoubleArrayTopic cameraToObjectTopic) { * @param speeds The desired wheel speeds. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); - final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); + final double leftFeedforward = m_feedforward.calculate(speeds.left); + final double rightFeedforward = m_feedforward.calculate(speeds.right); - final double leftOutput = - m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); + final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left); final double rightOutput = - m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond); + m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right); m_leftLeader.setVoltage(leftOutput + leftFeedforward); m_rightLeader.setVoltage(rightOutput + rightFeedforward); } @@ -251,10 +250,10 @@ public void simulationPeriodic() { m_rightLeader.get() * RobotController.getInputVoltage()); m_drivetrainSimulator.update(0.02); - m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); - m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); - m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); - m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); + m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPosition()); + m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocity()); + m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPosition()); + m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocity()); // m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java index 1748045c49c..c19622510ea 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java @@ -24,14 +24,14 @@ public static final class DriveConstants { // These characterization values MUST be determined either experimentally or theoretically // for *your* robot's drive. // The SysId tool provides a convenient method for obtaining these values for your robot. - public static final double ksVolts = 1; - public static final double kvVoltSecondsPerMeter = 0.8; - public static final double kaVoltSecondsSquaredPerMeter = 0.15; + public static final double ks = 1; // V + public static final double kv = 0.8; // V/(m/s) + public static final double ka = 0.15; // V/(m/s²) public static final double kp = 1; - public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxSpeed = 3; // m/s + public static final double kMaxAcceleration = 3; // m/s² } public static final class OIConstants { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java index 3c3179dcc03..19959a61366 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java @@ -33,10 +33,7 @@ public class DriveSubsystem extends SubsystemBase { // The feedforward controller. private final SimpleMotorFeedforward m_feedforward = - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerMeter, - DriveConstants.kaVoltSecondsSquaredPerMeter); + new SimpleMotorFeedforward(DriveConstants.ks, DriveConstants.kv, DriveConstants.ka); // The robot's drive private final DifferentialDrive m_drive = @@ -46,8 +43,7 @@ public class DriveSubsystem extends SubsystemBase { private final TrapezoidProfile m_profile = new TrapezoidProfile( new TrapezoidProfile.Constraints( - DriveConstants.kMaxSpeedMetersPerSecond, - DriveConstants.kMaxAccelerationMetersPerSecondSquared)); + DriveConstants.kMaxSpeed, DriveConstants.kMaxAcceleration)); // The timer private final Timer m_timer = new Timer(); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java index 7a6a937fee4..d0ec6bf51cd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java @@ -26,11 +26,11 @@ public class Constants { public static final double kElevatorDrumRadius = Units.inchesToMeters(1.0); public static final double kCarriageMass = Units.lbsToKilograms(12); // kg - public static final double kSetpointMeters = Units.inchesToMeters(42.875); - public static final double kLowerkSetpointMeters = Units.inchesToMeters(15); + public static final double kSetpoint = Units.inchesToMeters(42.875); + public static final double kLowerkSetpoint = Units.inchesToMeters(15); // Encoder is reset to measure 0 at the bottom, so minimum height is 0. - public static final double kMinElevatorHeightMeters = 0.0; - public static final double kMaxElevatorHeightMeters = Units.inchesToMeters(50); + public static final double kMinElevatorHeight = 0.0; // m + public static final double kMaxElevatorHeight = Units.inchesToMeters(50); // distance per pulse = (distance per revolution) / (pulses per revolution) // = (Pi * D) / ppr diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java index 889fe8e9b32..ef72be7f902 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java @@ -38,10 +38,10 @@ public void teleopInit() { public void teleopPeriodic() { if (m_joystick.getTrigger()) { // Here, we set the constant setpoint of 10 meters. - m_elevator.reachGoal(Constants.kSetpointMeters); + m_elevator.reachGoal(Constants.kSetpoint); } else { // Otherwise, we update the setpoint to 1 meter. - m_elevator.reachGoal(Constants.kLowerkSetpointMeters); + m_elevator.reachGoal(Constants.kLowerkSetpoint); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java index f4fb80ed81d..7f304519a4e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java @@ -55,8 +55,8 @@ public class Elevator implements AutoCloseable { Constants.kElevatorGearing, Constants.kCarriageMass, Constants.kElevatorDrumRadius, - Constants.kMinElevatorHeightMeters, - Constants.kMaxElevatorHeightMeters, + Constants.kMinElevatorHeight, + Constants.kMaxElevatorHeight, true, 0, 0.005, @@ -70,8 +70,7 @@ public class Elevator implements AutoCloseable { private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", Units.inchesToMeters(5), Units.inchesToMeters(0.5)); private final MechanismLigament2d m_elevatorMech2d = - m_mech2dRoot.append( - new MechanismLigament2d("Elevator", m_elevatorSim.getPositionMeters(), 90)); + m_mech2dRoot.append(new MechanismLigament2d("Elevator", m_elevatorSim.getPosition(), 90)); /** Subsystem constructor. */ public Elevator() { @@ -92,10 +91,10 @@ public void simulationPeriodic() { m_elevatorSim.update(0.020); // Finally, we set our simulated encoder's readings and simulated battery voltage - m_encoderSim.setDistance(m_elevatorSim.getPositionMeters()); + m_encoderSim.setDistance(m_elevatorSim.getPosition()); // SimBattery estimates loaded battery voltages RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps())); + BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDraw())); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java index f5a7567b377..7fd6332bed2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java @@ -25,10 +25,10 @@ public class Constants { public static final double kElevatorDrumRadius = Units.inchesToMeters(2.0); public static final double kCarriageMass = 4.0; // kg - public static final double kSetpointMeters = 0.75; + public static final double kSetpoint = 0.75; // m // Encoder is reset to measure 0 at the bottom, so minimum height is 0. - public static final double kMinElevatorHeightMeters = 0.0; - public static final double kMaxElevatorHeightMeters = 1.25; + public static final double kMinElevatorHeight = 0.0; // m + public static final double kMaxElevatorHeight = 1.25; // m // distance per pulse = (distance per revolution) / (pulses per revolution) // = (Pi * D) / ppr diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java index e45ba060918..96f36b58df7 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java @@ -31,7 +31,7 @@ public void simulationPeriodic() { public void teleopPeriodic() { if (m_joystick.getTrigger()) { // Here, we set the constant setpoint of 0.75 meters. - m_elevator.reachGoal(Constants.kSetpointMeters); + m_elevator.reachGoal(Constants.kSetpoint); } else { // Otherwise, we update the setpoint to 0. m_elevator.reachGoal(0.0); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java index d4f04fed26d..675326172f0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java @@ -50,8 +50,8 @@ public class Elevator implements AutoCloseable { Constants.kElevatorGearing, Constants.kCarriageMass, Constants.kElevatorDrumRadius, - Constants.kMinElevatorHeightMeters, - Constants.kMaxElevatorHeightMeters, + Constants.kMinElevatorHeight, + Constants.kMaxElevatorHeight, true, 0, 0.01, @@ -63,8 +63,7 @@ public class Elevator implements AutoCloseable { private final Mechanism2d m_mech2d = new Mechanism2d(20, 50); private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0); private final MechanismLigament2d m_elevatorMech2d = - m_mech2dRoot.append( - new MechanismLigament2d("Elevator", m_elevatorSim.getPositionMeters(), 90)); + m_mech2dRoot.append(new MechanismLigament2d("Elevator", m_elevatorSim.getPosition(), 90)); /** Subsystem constructor. */ public Elevator() { @@ -85,10 +84,10 @@ public void simulationPeriodic() { m_elevatorSim.update(0.020); // Finally, we set our simulated encoder's readings and simulated battery voltage - m_encoderSim.setDistance(m_elevatorSim.getPositionMeters()); + m_encoderSim.setDistance(m_elevatorSim.getPosition()); // SimBattery estimates loaded battery voltages RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps())); + BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDraw())); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java index 7a4ed2d0830..63eceb73704 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java @@ -14,8 +14,8 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; public class Robot extends TimedRobot { - public static final int SHOT_VELOCITY = 200; // rpm - public static final int TOLERANCE = 8; // rpm + public static final double SHOT_VELOCITY = 200; // rpm + public static final double TOLERANCE = 8; // rpm private final PWMSparkMax m_shooter = new PWMSparkMax(0); private final Encoder m_shooterEncoder = new Encoder(0, 1); @@ -34,7 +34,7 @@ public Robot() { m_controller.setTolerance(TOLERANCE); BooleanEvent isBallAtKicker = - new BooleanEvent(m_loop, () -> false); // m_kickerSensor.getRangeMM() < KICKER_THRESHOLD); + new BooleanEvent(m_loop, () -> false); // m_kickerSensor.getRange() < KICKER_THRESHOLD); BooleanEvent intakeButton = new BooleanEvent(m_loop, () -> m_joystick.getRawButton(2)); // if the thumb button is held diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java index 76fc4317594..9dae80f329a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java @@ -96,6 +96,6 @@ public void simulationPeriodic() { // simulation, and write the simulated velocities to our simulated encoder m_flywheelSim.setInputVoltage(m_flywheelMotor.get() * RobotController.getInputVoltage()); m_flywheelSim.update(0.02); - m_encoderSim.setRate(m_flywheelSim.getAngularVelocityRadPerSec()); + m_encoderSim.setRate(m_flywheelSim.getAngularVelocity()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index 8f69237db62..9aa5c049a3c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -95,23 +95,19 @@ public MecanumDriveWheelPositions getCurrentDistances() { * @param speeds The desired wheel speeds. */ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { - final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond); - final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond); - final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond); - final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond); + final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeft); + final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRight); + final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeft); + final double backRightFeedforward = m_feedforward.calculate(speeds.rearRight); final double frontLeftOutput = - m_frontLeftPIDController.calculate( - m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond); + m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), speeds.frontLeft); final double frontRightOutput = - m_frontRightPIDController.calculate( - m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond); + m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), speeds.frontRight); final double backLeftOutput = - m_backLeftPIDController.calculate( - m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond); + m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), speeds.rearLeft); final double backRightOutput = - m_backRightPIDController.calculate( - m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond); + m_backRightPIDController.calculate(m_backRightEncoder.getRate(), speeds.rearRight); m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward); m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward); @@ -128,13 +124,12 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive( - double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { + double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) { var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); if (fieldRelative) { chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d()); } - setSpeeds( - m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(periodSeconds)).desaturate(kMaxSpeed)); + setSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(kMaxSpeed)); } /** Updates the field relative position of the robot. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java index 19164dc29e8..80762194595 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java @@ -34,23 +34,23 @@ public static final class DriveConstants { public static final boolean kFrontRightEncoderReversed = false; public static final boolean kRearRightEncoderReversed = true; - public static final double kTrackWidth = 0.5; + public static final double kTrackwidth = 0.5; // Distance between centers of right and left wheels on robot public static final double kWheelBase = 0.7; // Distance between centers of front and back wheels on robot public static final MecanumDriveKinematics kDriveKinematics = new MecanumDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + new Translation2d(kWheelBase / 2, kTrackwidth / 2), + new Translation2d(kWheelBase / 2, -kTrackwidth / 2), + new Translation2d(-kWheelBase / 2, kTrackwidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackwidth / 2)); public static final int kEncoderCPR = 1024; - public static final double kWheelDiameterMeters = 0.15; + public static final double kWheelDiameter = 0.15; // m public static final double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterMeters * Math.PI) / kEncoderCPR; + (kWheelDiameter * Math.PI) / kEncoderCPR; // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These characterization values MUST be determined either experimentally or theoretically @@ -71,10 +71,10 @@ public static final class OIConstants { } public static final class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final double kMaxSpeed = 3; // m/s + public static final double kMaxAcceleration = 3; // m/s² + public static final double kMaxAngularSpeed = Math.PI; // rad/s + public static final double kMaxAngularAcceleration = Math.PI; // rad/s² public static final double kPXController = 0.5; public static final double kPYController = 0.5; @@ -82,7 +82,6 @@ public static final class AutoConstants { // Constraint for the motion profilied robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = - new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + new TrapezoidProfile.Constraints(kMaxAngularSpeed, kMaxAngularAcceleration); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java index 5ceb363806e..cf8e088ddb3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java @@ -80,9 +80,7 @@ private void configureButtonBindings() { public Command getAutonomousCommand() { // Create config for trajectory TrajectoryConfig config = - new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) + new TrajectoryConfig(AutoConstants.kMaxSpeed, AutoConstants.kMaxAcceleration) // Add kinematics to ensure max speed is actually obeyed .setKinematics(DriveConstants.kDriveKinematics); @@ -111,7 +109,7 @@ public Command getAutonomousCommand() { AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints), // Needed for normalizing wheel speeds - AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxSpeed, // Velocity PID's new PIDController(DriveConstants.kPFrontLeftVel, 0, 0), diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java index ba497e2069e..0803adb206e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java @@ -94,7 +94,7 @@ public void periodic() { * @return The pose. */ public Pose2d getPose() { - return m_odometry.getPoseMeters(); + return m_odometry.getPose(); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index f1b0e5003c3..6ce731d5ca4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -107,23 +107,19 @@ public MecanumDriveWheelPositions getCurrentDistances() { * @param speeds The desired wheel speeds. */ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { - final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond); - final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond); - final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond); - final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond); + final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeft); + final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRight); + final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeft); + final double backRightFeedforward = m_feedforward.calculate(speeds.rearRight); final double frontLeftOutput = - m_frontLeftPIDController.calculate( - m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond); + m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), speeds.frontLeft); final double frontRightOutput = - m_frontRightPIDController.calculate( - m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond); + m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), speeds.frontRight); final double backLeftOutput = - m_backLeftPIDController.calculate( - m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond); + m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), speeds.rearLeft); final double backRightOutput = - m_backRightPIDController.calculate( - m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond); + m_backRightPIDController.calculate(m_backRightEncoder.getRate(), speeds.rearRight); m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward); m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward); @@ -140,14 +136,13 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive( - double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { + double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) { var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); if (fieldRelative) { chassisSpeeds = chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation()); } - setSpeeds( - m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(periodSeconds)).desaturate(kMaxSpeed)); + setSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(kMaxSpeed)); } /** Updates the field relative position of the robot. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java index 65549bb1067..560e18f36b1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java @@ -20,10 +20,10 @@ public class Robot extends TimedRobot { static final int kJoystickChannel = 3; // The elevator can move 1.5 meters from top to bottom - static final double kFullHeightMeters = 1.5; + static final double kFullHeight = 1.5; - // Bottom, middle, and top elevator setpoints - static final double[] kSetpointsMeters = {0.2, 0.8, 1.4}; + // Bottom, middle, and top elevator setpoints in meters + static final double[] kSetpoints = {0.2, 0.8, 1.4}; // proportional, integral, and derivative speed constants // DANGER: when tuning PID constants, high/inappropriate values for kP, kI, @@ -35,7 +35,7 @@ public class Robot extends TimedRobot { private final PIDController m_pidController = new PIDController(kP, kI, kD); // Scaling is handled internally private final AnalogPotentiometer m_potentiometer = - new AnalogPotentiometer(kPotChannel, kFullHeightMeters); + new AnalogPotentiometer(kPotChannel, kFullHeight); private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(kMotorChannel); private final Joystick m_joystick = new Joystick(kJoystickChannel); @@ -45,7 +45,7 @@ public class Robot extends TimedRobot { public void teleopInit() { // Move to the bottom setpoint when teleop starts m_index = 0; - m_pidController.setSetpoint(kSetpointsMeters[m_index]); + m_pidController.setSetpoint(kSetpoints[m_index]); } @Override @@ -62,9 +62,9 @@ public void teleopPeriodic() { // when the button is pressed once, the selected elevator setpoint is incremented if (m_joystick.getTriggerPressed()) { // index of the elevator setpoint wraps around. - m_index = (m_index + 1) % kSetpointsMeters.length; + m_index = (m_index + 1) % kSetpoints.length; System.out.println("m_index = " + m_index); - m_pidController.setSetpoint(kSetpointsMeters[m_index]); + m_pidController.setSetpoint(kSetpoints[m_index]); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java index e0bdb757ba0..40dc0f92896 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java @@ -27,10 +27,10 @@ public static final class DriveConstants { public static final boolean kRightEncoderReversed = true; public static final int kEncoderCPR = 1024; - public static final double kWheelDiameterMeters = Units.inchesToMeters(6); + public static final double kWheelDiameter = Units.inchesToMeters(6); public static final double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterMeters * Math.PI) / kEncoderCPR; + (kWheelDiameter * Math.PI) / kEncoderCPR; // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These values MUST be determined either experimentally or theoretically for *your* robot's @@ -46,9 +46,9 @@ public static final class DriveConstants { public static final double kMaxTurnRateDegPerS = 100; public static final double kMaxTurnAccelerationDegPerSSquared = 300; - public static final double ksVolts = 1; - public static final double kvVoltSecondsPerDegree = 0.8; - public static final double kaVoltSecondsSquaredPerDegree = 0.15; + public static final double ks = 1; // V + public static final double kv = 0.8; // V/(deg/s) + public static final double ka = 0.15; // V/(deg/s²) } public static final class ShooterConstants { @@ -70,9 +70,9 @@ public static final class ShooterConstants { // On a real robot the feedforward constants should be empirically determined; these are // reasonable guesses. - public static final double kSVolts = 0.05; + public static final double kS = 0.05; // V // Should have value 12V at free speed - public static final double kVVoltSecondsPerRotation = 12.0 / kShooterFreeRPS; + public static final double kV = 12.0 / kShooterFreeRPS; // V/(rot/s) public static final double kFeederSpeed = 0.5; } @@ -88,8 +88,8 @@ public static final class StorageConstants { } public static final class AutoConstants { - public static final double kTimeoutSeconds = 3; - public static final double kDriveDistanceMeters = 2; + public static final double kTimeout = 3; + public static final double kDriveDistance = 2; // m public static final double kDriveSpeed = 0.5; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java index a4273a481ae..750279b0d2f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java @@ -87,7 +87,7 @@ public void configureBindings() { public Command getAutonomousCommand() { // Drive forward for 2 meters at half speed with a 3 second timeout return m_drive - .driveDistanceCommand(AutoConstants.kDriveDistanceMeters, AutoConstants.kDriveSpeed) - .withTimeout(AutoConstants.kTimeoutSeconds); + .driveDistanceCommand(AutoConstants.kDriveDistance, AutoConstants.kDriveSpeed) + .withTimeout(AutoConstants.kTimeout); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java index b0d2670380c..5b2af06dfdb 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java @@ -59,10 +59,7 @@ public class Drive extends SubsystemBase { DriveConstants.kMaxTurnRateDegPerS, DriveConstants.kMaxTurnAccelerationDegPerSSquared)); private final SimpleMotorFeedforward m_feedforward = - new SimpleMotorFeedforward( - DriveConstants.ksVolts, - DriveConstants.kvVoltSecondsPerDegree, - DriveConstants.kaVoltSecondsSquaredPerDegree); + new SimpleMotorFeedforward(DriveConstants.ks, DriveConstants.kv, DriveConstants.ka); /** Creates a new Drive subsystem. */ public Drive() { @@ -105,10 +102,10 @@ public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) { /** * Returns a command that drives the robot forward a specified distance at a specified speed. * - * @param distanceMeters The distance to drive forward in meters + * @param distance The distance to drive forward in meters * @param speed The fraction of max speed at which to drive */ - public Command driveDistanceCommand(double distanceMeters, double speed) { + public Command driveDistanceCommand(double distance, double speed) { return runOnce( () -> { // Reset encoders at the start of the command @@ -119,9 +116,7 @@ public Command driveDistanceCommand(double distanceMeters, double speed) { .andThen(run(() -> m_drive.arcadeDrive(speed, 0))) // End command when we've traveled the specified distance .until( - () -> - Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance()) - >= distanceMeters) + () -> Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance()) >= distance) // Stop the drive when the command ends .finallyDo(interrupted -> m_drive.stopMotor()); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java index 39023c2e8ef..5689341b37d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java @@ -26,8 +26,7 @@ public class Shooter extends SubsystemBase { ShooterConstants.kEncoderPorts[1], ShooterConstants.kEncoderReversed); private final SimpleMotorFeedforward m_shooterFeedforward = - new SimpleMotorFeedforward( - ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation); + new SimpleMotorFeedforward(ShooterConstants.kS, ShooterConstants.kV); private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0.0, 0.0); /** The shooter subsystem for the robot. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index 5045aac3e64..b5e86b8bccd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -30,7 +30,7 @@ public class Drivetrain { // 1/2 rotation per second. public static final double kMaxAngularSpeed = Math.PI; - private static final double kTrackWidth = 0.381 * 2; + private static final double kTrackwidth = 0.381 * 2; private static final double kWheelRadius = 0.0508; private static final int kEncoderResolution = -4096; @@ -48,7 +48,7 @@ public class Drivetrain { private final AnalogGyro m_gyro = new AnalogGyro(0); private final DifferentialDriveKinematics m_kinematics = - new DifferentialDriveKinematics(kTrackWidth); + new DifferentialDriveKinematics(kTrackwidth); private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry( m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); @@ -65,7 +65,7 @@ public class Drivetrain { LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); private final DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim( - m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null); + m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null); /** Subsystem constructor. */ public Drivetrain() { @@ -92,12 +92,10 @@ public Drivetrain() { /** Sets speeds to the drivetrain motors. */ public void setSpeeds(DifferentialDriveWheelSpeeds speeds) { - final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond); - final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond); - double leftOutput = - m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond); - double rightOutput = - m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond); + final double leftFeedforward = m_feedforward.calculate(speeds.left); + final double rightFeedforward = m_feedforward.calculate(speeds.right); + double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left); + double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right); m_leftLeader.setVoltage(leftOutput + leftFeedforward); m_rightLeader.setVoltage(rightOutput + rightFeedforward); @@ -128,7 +126,7 @@ public void resetOdometry(Pose2d pose) { /** Check the current robot pose. */ public Pose2d getPose() { - return m_odometry.getPoseMeters(); + return m_odometry.getPose(); } /** Update our simulation. This should be run every robot loop in simulation. */ @@ -142,16 +140,16 @@ public void simulationPeriodic() { m_rightLeader.get() * RobotController.getInputVoltage()); m_drivetrainSimulator.update(0.02); - m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); - m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); - m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); - m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); + m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPosition()); + m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocity()); + m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPosition()); + m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocity()); // m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); } /** Update odometry - this should be run every robot loop. */ public void periodic() { updateOdometry(); - m_fieldSim.setRobotPose(m_odometry.getPoseMeters()); + m_fieldSim.setRobotPose(m_odometry.getPose()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java index 85953c63579..d8471b2c49a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java @@ -56,7 +56,7 @@ public void autonomousPeriodic() { double elapsed = m_timer.get(); Trajectory.State reference = m_trajectory.sample(elapsed); ChassisSpeeds speeds = m_feedback.calculate(m_drive.getPose(), reference); - m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); + m_drive.drive(speeds.vx, speeds.omega); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index 0784b623b0d..2cfbd938df1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -56,12 +56,12 @@ public Drivetrain() { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive( - double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { + double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) { var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); if (fieldRelative) { chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d()); } - chassisSpeeds = chassisSpeeds.discretize(periodSeconds); + chassisSpeeds = chassisSpeeds.discretize(period); var states = m_kinematics.toWheelSpeeds(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java index b57ba6e5eae..1590dfaa805 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -121,9 +121,9 @@ public void setDesiredState(SwerveModuleState desiredState) { // Calculate the drive output from the drive PID controller. final double driveOutput = - m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond); + m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed); - final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond); + final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed); // Calculate the turning motor output from the turning PID controller. final double turnOutput = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java index acdaa388594..3a13b717792 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java @@ -52,16 +52,16 @@ public static final class DriveConstants { // If you call DriveSubsystem.drive() with a different period make sure to update this. public static final double kDrivePeriod = TimedRobot.kDefaultPeriod; - public static final double kTrackWidth = 0.5; + public static final double kTrackwidth = 0.5; // Distance between centers of right and left wheels on robot public static final double kWheelBase = 0.7; // Distance between front and back wheels on robot public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2, kTrackWidth / 2), - new Translation2d(kWheelBase / 2, -kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, kTrackWidth / 2), - new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); + new Translation2d(kWheelBase / 2, kTrackwidth / 2), + new Translation2d(kWheelBase / 2, -kTrackwidth / 2), + new Translation2d(-kWheelBase / 2, kTrackwidth / 2), + new Translation2d(-kWheelBase / 2, -kTrackwidth / 2)); public static final boolean kGyroReversed = false; @@ -69,22 +69,22 @@ public static final class DriveConstants { // These characterization values MUST be determined either experimentally or theoretically // for *your* robot's drive. // The SysId tool provides a convenient method for obtaining these values for your robot. - public static final double ksVolts = 1; - public static final double kvVoltSecondsPerMeter = 0.8; - public static final double kaVoltSecondsSquaredPerMeter = 0.15; + public static final double ks = 1; // V + public static final double kv = 0.8; // V/(m/s) + public static final double ka = 0.15; // V/(m/s²) - public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxSpeed = 3; // m/s } public static final class ModuleConstants { - public static final double kMaxModuleAngularSpeedRadiansPerSecond = 2 * Math.PI; - public static final double kMaxModuleAngularAccelerationRadiansPerSecondSquared = 2 * Math.PI; + public static final double kMaxModuleAngularSpeed = 2 * Math.PI; // rad/s + public static final double kMaxModuleAngularAcceleration = 2 * Math.PI; // rad/s² public static final int kEncoderCPR = 1024; - public static final double kWheelDiameterMeters = 0.15; + public static final double kWheelDiameter = 0.15; // m public static final double kDriveEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterMeters * Math.PI) / kEncoderCPR; + (kWheelDiameter * Math.PI) / kEncoderCPR; public static final double kTurningEncoderDistancePerPulse = // Assumes the encoders are on a 1:1 reduction with the module shaft. @@ -100,10 +100,10 @@ public static final class OIConstants { } public static final class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = 3; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final double kMaxSpeed = 3; // m/s + public static final double kMaxAcceleration = 3; // m/s² + public static final double kMaxAngularSpeed = Math.PI; // rad/s + public static final double kMaxAngularAcceleration = Math.PI; // rad/s² public static final double kPXController = 1; public static final double kPYController = 1; @@ -111,7 +111,6 @@ public static final class AutoConstants { // Constraint for the motion profiled robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = - new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + new TrapezoidProfile.Constraints(kMaxAngularSpeed, kMaxAngularAcceleration); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java index f2641890a6d..b729de42bb8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java @@ -54,10 +54,9 @@ public RobotContainer() { // Multiply by max speed to map the joystick unitless inputs to actual units. // This will map the [-1, 1] to [max speed backwards, max speed forwards], // converting them to actual units. - m_driverController.getLeftY() * DriveConstants.kMaxSpeedMetersPerSecond, - m_driverController.getLeftX() * DriveConstants.kMaxSpeedMetersPerSecond, - m_driverController.getRightX() - * ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond, + m_driverController.getLeftY() * DriveConstants.kMaxSpeed, + m_driverController.getLeftX() * DriveConstants.kMaxSpeed, + m_driverController.getRightX() * ModuleConstants.kMaxModuleAngularSpeed, false), m_robotDrive)); } @@ -78,9 +77,7 @@ private void configureButtonBindings() {} public Command getAutonomousCommand() { // Create config for trajectory TrajectoryConfig config = - new TrajectoryConfig( - AutoConstants.kMaxSpeedMetersPerSecond, - AutoConstants.kMaxAccelerationMetersPerSecondSquared) + new TrajectoryConfig(AutoConstants.kMaxSpeed, AutoConstants.kMaxAcceleration) // Add kinematics to ensure max speed is actually obeyed .setKinematics(DriveConstants.kDriveKinematics); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index 20bf69aaee7..865c88904e6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -89,7 +89,7 @@ public void periodic() { * @return The pose. */ public Pose2d getPose() { - return m_odometry.getPoseMeters(); + return m_odometry.getPose(); } /** @@ -125,7 +125,7 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ chassisSpeeds = chassisSpeeds.discretize(DriveConstants.kDrivePeriod); var states = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeedMetersPerSecond); + SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeed); m_frontLeft.setDesiredState(states[0]); m_frontRight.setDesiredState(states[1]); @@ -139,8 +139,7 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ * @param desiredStates The desired SwerveModule states. */ public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds( - desiredStates, DriveConstants.kMaxSpeedMetersPerSecond); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kMaxSpeed); m_frontLeft.setDesiredState(desiredStates[0]); m_frontRight.setDesiredState(desiredStates[1]); m_rearLeft.setDesiredState(desiredStates[2]); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java index 1a032d273fa..0ee79e7d029 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java @@ -31,8 +31,8 @@ public class SwerveModule { 0, 0, new TrapezoidProfile.Constraints( - ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond, - ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared)); + ModuleConstants.kMaxModuleAngularSpeed, + ModuleConstants.kMaxModuleAngularAcceleration)); /** * Constructs a SwerveModule. @@ -117,7 +117,7 @@ public void setDesiredState(SwerveModuleState desiredState) { // Calculate the drive output from the drive PID controller. final double driveOutput = - m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond); + m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed); // Calculate the turning motor output from the turning PID controller. final double turnOutput = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java index f754f08bed4..0f155e6eede 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java @@ -65,14 +65,14 @@ public Drivetrain() { * @param fieldRelative Whether the provided x and y speeds are relative to the field. */ public void drive( - double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) { + double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) { var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot); if (fieldRelative) { chassisSpeeds = chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation()); } - chassisSpeeds = chassisSpeeds.discretize(periodSeconds); + chassisSpeeds = chassisSpeeds.discretize(period); var states = m_kinematics.toWheelSpeeds(chassisSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java index e8af157d37b..45111f1275a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java @@ -120,9 +120,9 @@ public void setDesiredState(SwerveModuleState desiredState) { // Calculate the drive output from the drive PID controller. final double driveOutput = - m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond); + m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed); - final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond); + final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed); // Calculate the turning motor output from the turning PID controller. final double turnOutput = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/Constants.java index 7e45d022479..62ae403b9e6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/Constants.java @@ -27,10 +27,10 @@ public static final class DriveConstants { public static final boolean kRightEncoderReversed = true; public static final int kEncoderCPR = 1024; - public static final double kWheelDiameterMeters = Units.inchesToMeters(6); + public static final double kWheelDiameter = Units.inchesToMeters(6); public static final double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterMeters * Math.PI) / kEncoderCPR; + (kWheelDiameter * Math.PI) / kEncoderCPR; } public static final class ShooterConstants { @@ -53,11 +53,10 @@ public static final class ShooterConstants { // On a real robot the feedforward constants should be empirically determined; these are // reasonable guesses. - public static final double kSVolts = 0.05; - public static final double kVVoltSecondsPerRotation = - // Should have value 12V at free speed... - 12.0 / kShooterFreeRPS; - public static final double kAVoltSecondsSquaredPerRotation = 0; + public static final double kS = 0.05; // V + // Should have value 12V at free speed + public static final double kV = 12.0 / kShooterFreeRPS; // V/(rot/s) + public static final double kA = 0; // V/(rot/s²) public static final double kFeederSpeed = 0.5; } @@ -73,8 +72,8 @@ public static final class StorageConstants { } public static final class AutoConstants { - public static final double kTimeoutSeconds = 3; - public static final double kDriveDistanceMeters = 2; + public static final double kTimeout = 3; + public static final double kDriveDistance = 2; // m public static final double kDriveSpeed = 0.5; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java index affceda1443..a8dd04e7f0e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java @@ -74,10 +74,7 @@ public class Shooter extends SubsystemBase { // Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to // those calculated by SysId private final SimpleMotorFeedforward m_shooterFeedforward = - new SimpleMotorFeedforward( - ShooterConstants.kSVolts, - ShooterConstants.kVVoltSecondsPerRotation, - ShooterConstants.kAVoltSecondsSquaredPerRotation); + new SimpleMotorFeedforward(ShooterConstants.kS, ShooterConstants.kV, ShooterConstants.kA); /** Creates a new Shooter subsystem. */ public Shooter() { diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java index 7faeec68f33..d7074a0fd99 100644 --- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java @@ -88,12 +88,12 @@ void teleopTest() { // advance 75 timesteps SimHooks.stepTiming(1.5); - assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05); + assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05); // advance 25 timesteps to see setpoint is held. SimHooks.stepTiming(0.5); - assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05); + assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05); } { @@ -115,12 +115,12 @@ void teleopTest() { // advance 75 timesteps SimHooks.stepTiming(1.5); - assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05); + assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05); // advance 25 timesteps to see setpoint is held. SimHooks.stepTiming(0.5); - assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05); + assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05); } { diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java index 8a3d92b8e66..6fe3cc413e6 100644 --- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java @@ -53,7 +53,7 @@ void startThread() { kCarriageMassKg, kElevatorDrumRadius, 0.0, - Robot.kFullHeightMeters, + Robot.kFullHeight, true, 0); m_analogSim = new AnalogInputSim(Robot.kPotChannel); @@ -74,7 +74,7 @@ void startThread() { */ m_analogSim.setVoltage( RobotController.getVoltage5V() - * (m_elevatorSim.getPositionMeters() / Robot.kFullHeightMeters)); + * (m_elevatorSim.getPosition() / Robot.kFullHeight)); }); m_thread.start(); @@ -113,7 +113,7 @@ void teleopTest() { // advance 50 timesteps SimHooks.stepTiming(1); - assertEquals(Robot.kSetpointsMeters[0], m_elevatorSim.getPositionMeters(), 0.1); + assertEquals(Robot.kSetpoints[0], m_elevatorSim.getPosition(), 0.1); } // second setpoint @@ -125,7 +125,7 @@ void teleopTest() { // advance 50 timesteps SimHooks.stepTiming(1); - assertEquals(Robot.kSetpointsMeters[1], m_elevatorSim.getPositionMeters(), 0.1); + assertEquals(Robot.kSetpoints[1], m_elevatorSim.getPosition(), 0.1); } // we need to unpress the button @@ -146,7 +146,7 @@ void teleopTest() { // advance 50 timesteps SimHooks.stepTiming(1); - assertEquals(Robot.kSetpointsMeters[2], m_elevatorSim.getPositionMeters(), 0.1); + assertEquals(Robot.kSetpoints[2], m_elevatorSim.getPosition(), 0.1); } // we need to unpress the button @@ -167,7 +167,7 @@ void teleopTest() { // advance 60 timesteps SimHooks.stepTiming(1.2); - assertEquals(Robot.kSetpointsMeters[0], m_elevatorSim.getPositionMeters(), 0.1); + assertEquals(Robot.kSetpoints[0], m_elevatorSim.getPosition(), 0.1); } } } diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java index 8db878799f9..76b95da39ce 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java @@ -19,80 +19,80 @@ import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3021, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3020, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + - "BW9tZWdhIkYKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh8KC3RyYWNrX3dpZHRo" + - "GAEgASgBUgp0cmFja1dpZHRoIlAKJFByb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVXaGVlbFNwZWVkcxIS" + - "CgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCJTCidQcm90b2J1ZkRpZmZlcmVu" + - "dGlhbERyaXZlV2hlZWxQb3NpdGlvbnMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIF" + - "cmlnaHQipAIKHlByb3RvYnVmTWVjYW51bURyaXZlS2luZW1hdGljcxI/Cgpmcm9udF9sZWZ0GAEgASgL" + - "MiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJZnJvbnRMZWZ0EkEKC2Zyb250X3JpZ2h0" + - "GAIgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIKZnJvbnRSaWdodBI9CglyZWFy" + - "X2xlZnQYAyABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUghyZWFyTGVmdBI/Cgpy" + - "ZWFyX3JpZ2h0GAQgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJcmVhclJpZ2h0" + - "IqABCiJQcm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFS" + - "CWZyb250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyAB" + - "KAFSCHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNh" + - "bnVtRHJpdmVXaGVlbFNwZWVkcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRf" + - "cmlnaHQYAiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFy" + - "X3JpZ2h0GAQgASgBUglyZWFyUmlnaHQiWwodUHJvdG9idWZTd2VydmVEcml2ZUtpbmVtYXRpY3MSOgoH" + - "bW9kdWxlcxgBIAMoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSB21vZHVsZXMibwoc" + - "UHJvdG9idWZTd2VydmVNb2R1bGVQb3NpdGlvbhIaCghkaXN0YW5jZRgBIAEoAVIIZGlzdGFuY2USMwoF" + - "YW5nbGUYAiABKAsyHS53cGkucHJvdG8uUHJvdG9idWZSb3RhdGlvbjJkUgVhbmdsZSJmChlQcm90b2J1", - "ZlN3ZXJ2ZU1vZHVsZVN0YXRlEhQKBXNwZWVkGAEgASgBUgVzcGVlZBIzCgVhbmdsZRgCIAEoCzIdLndw" + - "aS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90" + - "b0qZDQoGEgQAAD0BCggKAQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGADEKCQoCCAES" + - "AwYAMQoKCgIEABIECAAMAQoKCgMEAAESAwgIHQoLCgQEAAIAEgMJAhAKDAoFBAACAAUSAwkCCAoMCgUE" + - "AAIAARIDCQkLCgwKBQQAAgADEgMJDg8KCwoEBAACARIDCgIQCgwKBQQAAgEFEgMKAggKDAoFBAACAQES" + - "AwoJCwoMCgUEAAIBAxIDCg4PCgsKBAQAAgISAwsCEwoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCQ4K" + - "DAoFBAACAgMSAwsREgoKCgIEARIEDgAQAQoKCgMEAQESAw4IKwoLCgQEAQIAEgMPAhkKDAoFBAECAAUS" + - "Aw8CCAoMCgUEAQIAARIDDwkUCgwKBQQBAgADEgMPFxgKCgoCBAISBBIAFQEKCgoDBAIBEgMSCCwKCwoE" + - "BAICABIDEwISCgwKBQQCAgAFEgMTAggKDAoFBAICAAESAxMJDQoMCgUEAgIAAxIDExARCgsKBAQCAgES" + - "AxQCEwoMCgUEAgIBBRIDFAIICgwKBQQCAgEBEgMUCQ4KDAoFBAICAQMSAxQREgoKCgIEAxIEFwAaAQoK" + - "CgMEAwESAxcILwoLCgQEAwIAEgMYAhIKDAoFBAMCAAUSAxgCCAoMCgUEAwIAARIDGAkNCgwKBQQDAgAD" + - "EgMYEBEKCwoEBAMCARIDGQITCgwKBQQDAgEFEgMZAggKDAoFBAMCAQESAxkJDgoMCgUEAwIBAxIDGRES" + - "CgoKAgQEEgQcACEBCgoKAwQEARIDHAgmCgsKBAQEAgASAx0CJwoMCgUEBAIABhIDHQIXCgwKBQQEAgAB" + - "EgMdGCIKDAoFBAQCAAMSAx0lJgoLCgQEBAIBEgMeAigKDAoFBAQCAQYSAx4CFwoMCgUEBAIBARIDHhgj" + - "CgwKBQQEAgEDEgMeJicKCwoEBAQCAhIDHwImCgwKBQQEAgIGEgMfAhcKDAoFBAQCAgESAx8YIQoMCgUE" + - "BAICAxIDHyQlCgsKBAQEAgMSAyACJwoMCgUEBAIDBhIDIAIXCgwKBQQEAgMBEgMgGCIKDAoFBAQCAwMS" + - "AyAlJgoKCgIEBRIEIwAoAQoKCgMEBQESAyMIKgoLCgQEBQIAEgMkAhgKDAoFBAUCAAUSAyQCCAoMCgUE" + - "BQIAARIDJAkTCgwKBQQFAgADEgMkFhcKCwoEBAUCARIDJQIZCgwKBQQFAgEFEgMlAggKDAoFBAUCAQES" + - "AyUJFAoMCgUEBQIBAxIDJRcYCgsKBAQFAgISAyYCFwoMCgUEBQICBRIDJgIICgwKBQQFAgIBEgMmCRIK" + - "DAoFBAUCAgMSAyYVFgoLCgQEBQIDEgMnAhgKDAoFBAUCAwUSAycCCAoMCgUEBQIDARIDJwkTCgwKBQQF", - "AgMDEgMnFhcKCgoCBAYSBCoALwEKCgoDBAYBEgMqCCcKCwoEBAYCABIDKwIYCgwKBQQGAgAFEgMrAggK" + - "DAoFBAYCAAESAysJEwoMCgUEBgIAAxIDKxYXCgsKBAQGAgESAywCGQoMCgUEBgIBBRIDLAIICgwKBQQG" + - "AgEBEgMsCRQKDAoFBAYCAQMSAywXGAoLCgQEBgICEgMtAhcKDAoFBAYCAgUSAy0CCAoMCgUEBgICARID" + - "LQkSCgwKBQQGAgIDEgMtFRYKCwoEBAYCAxIDLgIYCgwKBQQGAgMFEgMuAggKDAoFBAYCAwESAy4JEwoM" + - "CgUEBgIDAxIDLhYXCgoKAgQHEgQxADMBCgoKAwQHARIDMQglCgsKBAQHAgASAzICLQoMCgUEBwIABBID" + - "MgIKCgwKBQQHAgAGEgMyCyAKDAoFBAcCAAESAzIhKAoMCgUEBwIAAxIDMissCgoKAgQIEgQ1ADgBCgoK" + - "AwQIARIDNQgkCgsKBAQIAgASAzYCFgoMCgUECAIABRIDNgIICgwKBQQIAgABEgM2CREKDAoFBAgCAAMS" + - "AzYUFQoLCgQECAIBEgM3Ah8KDAoFBAgCAQYSAzcCFAoMCgUECAIBARIDNxUaCgwKBQQIAgEDEgM3HR4K" + - "CgoCBAkSBDoAPQEKCgoDBAkBEgM6CCEKCwoEBAkCABIDOwITCgwKBQQJAgAFEgM7AggKDAoFBAkCAAES" + - "AzsJDgoMCgUECQIAAxIDOxESCgsKBAQJAgESAzwCHwoMCgUECQIBBhIDPAIUCgwKBQQJAgEBEgM8FRoK" + - "DAoFBAkCAQMSAzwdHmIGcHJvdG8z"); + "BW9tZWdhIkUKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh4KCnRyYWNrd2lkdGgY" + + "ASABKAFSCnRyYWNrd2lkdGgiUAokUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsU3BlZWRzEhIK" + + "BGxlZnQYASABKAFSBGxlZnQSFAoFcmlnaHQYAiABKAFSBXJpZ2h0IlMKJ1Byb3RvYnVmRGlmZmVyZW50" + + "aWFsRHJpdmVXaGVlbFBvc2l0aW9ucxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVy" + + "aWdodCKkAgoeUHJvdG9idWZNZWNhbnVtRHJpdmVLaW5lbWF0aWNzEj8KCmZyb250X2xlZnQYASABKAsy" + + "IC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglmcm9udExlZnQSQQoLZnJvbnRfcmlnaHQY" + + "AiABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgpmcm9udFJpZ2h0Ej0KCXJlYXJf" + + "bGVmdBgDIAEoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSCHJlYXJMZWZ0Ej8KCnJl" + + "YXJfcmlnaHQYBCABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglyZWFyUmlnaHQi" + + "oAEKIlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxQb3NpdGlvbnMSHQoKZnJvbnRfbGVmdBgBIAEoAVIJ" + + "ZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVmdBgDIAEo" + + "AVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0Ip0BCh9Qcm90b2J1Zk1lY2Fu" + + "dW1Ecml2ZVdoZWVsU3BlZWRzEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9udF9y" + + "aWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJlYXJf" + + "cmlnaHQYBCABKAFSCXJlYXJSaWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURyaXZlS2luZW1hdGljcxI6Cgdt" + + "b2R1bGVzGAEgAygLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIHbW9kdWxlcyJvChxQ" + + "cm90b2J1ZlN3ZXJ2ZU1vZHVsZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVh" + + "bmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVm", + "U3dlcnZlTW9kdWxlU3RhdGUSFAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3Bp" + + "LnByb3RvLlByb3RvYnVmUm90YXRpb24yZFIFYW5nbGVCGgoYZWR1LndwaS5maXJzdC5tYXRoLnByb3Rv" + + "SpkNCgYSBAAAPQEKCAoBDBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAaCggKAQgSAwYAMQoJCgIIARID" + + "BgAxCgoKAgQAEgQIAAwBCgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoMCgUEAAIABRIDCQIICgwKBQQA" + + "AgABEgMJCQsKDAoFBAACAAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAACAQUSAwoCCAoMCgUEAAIBARID" + + "CgkLCgwKBQQAAgEDEgMKDg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgMLAggKDAoFBAACAgESAwsJDgoM" + + "CgUEAAICAxIDCxESCgoKAgQBEgQOABABCgoKAwQBARIDDggrCgsKBAQBAgASAw8CGAoMCgUEAQIABRID" + + "DwIICgwKBQQBAgABEgMPCRMKDAoFBAECAAMSAw8WFwoKCgIEAhIEEgAVAQoKCgMEAgESAxIILAoLCgQE" + + "AgIAEgMTAhIKDAoFBAICAAUSAxMCCAoMCgUEAgIAARIDEwkNCgwKBQQCAgADEgMTEBEKCwoEBAICARID" + + "FAITCgwKBQQCAgEFEgMUAggKDAoFBAICAQESAxQJDgoMCgUEAgIBAxIDFBESCgoKAgQDEgQXABoBCgoK" + + "AwQDARIDFwgvCgsKBAQDAgASAxgCEgoMCgUEAwIABRIDGAIICgwKBQQDAgABEgMYCQ0KDAoFBAMCAAMS" + + "AxgQEQoLCgQEAwIBEgMZAhMKDAoFBAMCAQUSAxkCCAoMCgUEAwIBARIDGQkOCgwKBQQDAgEDEgMZERIK" + + "CgoCBAQSBBwAIQEKCgoDBAQBEgMcCCYKCwoEBAQCABIDHQInCgwKBQQEAgAGEgMdAhcKDAoFBAQCAAES" + + "Ax0YIgoMCgUEBAIAAxIDHSUmCgsKBAQEAgESAx4CKAoMCgUEBAIBBhIDHgIXCgwKBQQEAgEBEgMeGCMK" + + "DAoFBAQCAQMSAx4mJwoLCgQEBAICEgMfAiYKDAoFBAQCAgYSAx8CFwoMCgUEBAICARIDHxghCgwKBQQE" + + "AgIDEgMfJCUKCwoEBAQCAxIDIAInCgwKBQQEAgMGEgMgAhcKDAoFBAQCAwESAyAYIgoMCgUEBAIDAxID" + + "ICUmCgoKAgQFEgQjACgBCgoKAwQFARIDIwgqCgsKBAQFAgASAyQCGAoMCgUEBQIABRIDJAIICgwKBQQF" + + "AgABEgMkCRMKDAoFBAUCAAMSAyQWFwoLCgQEBQIBEgMlAhkKDAoFBAUCAQUSAyUCCAoMCgUEBQIBARID" + + "JQkUCgwKBQQFAgEDEgMlFxgKCwoEBAUCAhIDJgIXCgwKBQQFAgIFEgMmAggKDAoFBAUCAgESAyYJEgoM" + + "CgUEBQICAxIDJhUWCgsKBAQFAgMSAycCGAoMCgUEBQIDBRIDJwIICgwKBQQFAgMBEgMnCRMKDAoFBAUC", + "AwMSAycWFwoKCgIEBhIEKgAvAQoKCgMEBgESAyoIJwoLCgQEBgIAEgMrAhgKDAoFBAYCAAUSAysCCAoM" + + "CgUEBgIAARIDKwkTCgwKBQQGAgADEgMrFhcKCwoEBAYCARIDLAIZCgwKBQQGAgEFEgMsAggKDAoFBAYC" + + "AQESAywJFAoMCgUEBgIBAxIDLBcYCgsKBAQGAgISAy0CFwoMCgUEBgICBRIDLQIICgwKBQQGAgIBEgMt" + + "CRIKDAoFBAYCAgMSAy0VFgoLCgQEBgIDEgMuAhgKDAoFBAYCAwUSAy4CCAoMCgUEBgIDARIDLgkTCgwK" + + "BQQGAgMDEgMuFhcKCgoCBAcSBDEAMwEKCgoDBAcBEgMxCCUKCwoEBAcCABIDMgItCgwKBQQHAgAEEgMy" + + "AgoKDAoFBAcCAAYSAzILIAoMCgUEBwIAARIDMiEoCgwKBQQHAgADEgMyKywKCgoCBAgSBDUAOAEKCgoD" + + "BAgBEgM1CCQKCwoEBAgCABIDNgIWCgwKBQQIAgAFEgM2AggKDAoFBAgCAAESAzYJEQoMCgUECAIAAxID" + + "NhQVCgsKBAQIAgESAzcCHwoMCgUECAIBBhIDNwIUCgwKBQQIAgEBEgM3FRoKDAoFBAgCAQMSAzcdHgoK" + + "CgIECRIEOgA9AQoKCgMECQESAzoIIQoLCgQECQIAEgM7AhMKDAoFBAkCAAUSAzsCCAoMCgUECQIAARID" + + "OwkOCgwKBQQJAgADEgM7ERIKCwoEBAkCARIDPAIfCgwKBQQJAgEGEgM8AhQKDAoFBAkCAQESAzwVGgoM" + + "CgUECQIBAxIDPB0eYgZwcm90bzM="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); static final Descriptors.Descriptor wpi_proto_ProtobufChassisSpeeds_descriptor = descriptor.internalContainedType(49, 77, "ProtobufChassisSpeeds", "wpi.proto.ProtobufChassisSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveKinematics_descriptor = descriptor.internalContainedType(128, 70, "ProtobufDifferentialDriveKinematics", "wpi.proto.ProtobufDifferentialDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveKinematics_descriptor = descriptor.internalContainedType(128, 69, "ProtobufDifferentialDriveKinematics", "wpi.proto.ProtobufDifferentialDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(200, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(199, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(282, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(281, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(368, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(367, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(663, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(662, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(826, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(825, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(985, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(984, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1078, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1077, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1191, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1190, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); /** * @return this proto file's descriptor. @@ -520,9 +520,9 @@ public static final class ProtobufDifferentialDriveKinematics extends ProtoMessa private static final long serialVersionUID = 0L; /** - * optional double track_width = 1; + * optional double trackwidth = 1; */ - private double trackWidth; + private double trackwidth; private ProtobufDifferentialDriveKinematics() { } @@ -535,39 +535,39 @@ public static ProtobufDifferentialDriveKinematics newInstance() { } /** - * optional double track_width = 1; - * @return whether the trackWidth field is set + * optional double trackwidth = 1; + * @return whether the trackwidth field is set */ - public boolean hasTrackWidth() { + public boolean hasTrackwidth() { return (bitField0_ & 0x00000001) != 0; } /** - * optional double track_width = 1; + * optional double trackwidth = 1; * @return this */ - public ProtobufDifferentialDriveKinematics clearTrackWidth() { + public ProtobufDifferentialDriveKinematics clearTrackwidth() { bitField0_ &= ~0x00000001; - trackWidth = 0D; + trackwidth = 0D; return this; } /** - * optional double track_width = 1; - * @return the trackWidth + * optional double trackwidth = 1; + * @return the trackwidth */ - public double getTrackWidth() { - return trackWidth; + public double getTrackwidth() { + return trackwidth; } /** - * optional double track_width = 1; - * @param value the trackWidth to set + * optional double trackwidth = 1; + * @param value the trackwidth to set * @return this */ - public ProtobufDifferentialDriveKinematics setTrackWidth(final double value) { + public ProtobufDifferentialDriveKinematics setTrackwidth(final double value) { bitField0_ |= 0x00000001; - trackWidth = value; + trackwidth = value; return this; } @@ -577,7 +577,7 @@ public ProtobufDifferentialDriveKinematics copyFrom( cachedSize = other.cachedSize; if ((bitField0_ | other.bitField0_) != 0) { bitField0_ = other.bitField0_; - trackWidth = other.trackWidth; + trackwidth = other.trackwidth; } return this; } @@ -589,8 +589,8 @@ public ProtobufDifferentialDriveKinematics mergeFrom( return this; } cachedSize = -1; - if (other.hasTrackWidth()) { - setTrackWidth(other.trackWidth); + if (other.hasTrackwidth()) { + setTrackwidth(other.trackwidth); } return this; } @@ -602,7 +602,7 @@ public ProtobufDifferentialDriveKinematics clear() { } cachedSize = -1; bitField0_ = 0; - trackWidth = 0D; + trackwidth = 0D; return this; } @@ -626,14 +626,14 @@ public boolean equals(Object o) { } ProtobufDifferentialDriveKinematics other = (ProtobufDifferentialDriveKinematics) o; return bitField0_ == other.bitField0_ - && (!hasTrackWidth() || ProtoUtil.isEqual(trackWidth, other.trackWidth)); + && (!hasTrackwidth() || ProtoUtil.isEqual(trackwidth, other.trackwidth)); } @Override public void writeTo(final ProtoSink output) throws IOException { if ((bitField0_ & 0x00000001) != 0) { output.writeRawByte((byte) 9); - output.writeDoubleNoTag(trackWidth); + output.writeDoubleNoTag(trackwidth); } } @@ -655,8 +655,8 @@ public ProtobufDifferentialDriveKinematics mergeFrom(final ProtoSource input) th while (true) { switch (tag) { case 9: { - // trackWidth - trackWidth = input.readDouble(); + // trackwidth + trackwidth = input.readDouble(); bitField0_ |= 0x00000001; tag = input.readTag(); if (tag != 0) { @@ -681,7 +681,7 @@ public ProtobufDifferentialDriveKinematics mergeFrom(final ProtoSource input) th public void writeTo(final JsonSink output) throws IOException { output.beginObject(); if ((bitField0_ & 0x00000001) != 0) { - output.writeDouble(FieldNames.trackWidth, trackWidth); + output.writeDouble(FieldNames.trackwidth, trackwidth); } output.endObject(); } @@ -694,11 +694,10 @@ public ProtobufDifferentialDriveKinematics mergeFrom(final JsonSource input) thr } while (!input.isAtEnd()) { switch (input.readFieldHash()) { - case 1152213819: - case 1600986578: { - if (input.isAtField(FieldNames.trackWidth)) { + case 1181766491: { + if (input.isAtField(FieldNames.trackwidth)) { if (!input.trySkipNullValue()) { - trackWidth = input.readDouble(); + trackwidth = input.readDouble(); bitField0_ |= 0x00000001; } } else { @@ -768,7 +767,7 @@ public ProtobufDifferentialDriveKinematics create() { * Contains name constants used for serializing JSON */ static class FieldNames { - static final FieldName trackWidth = FieldName.forField("trackWidth", "track_width"); + static final FieldName trackwidth = FieldName.forField("trackwidth"); } } diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp index ad404212b9c..5010d135e76 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.cpp @@ -24,297 +24,297 @@ static const uint8_t file_descriptor[] { 0x0a,0x02,0x76,0x79,0x18,0x02,0x20,0x01,0x28,0x01, 0x52,0x02,0x76,0x79,0x12,0x14,0x0a,0x05,0x6f,0x6d, 0x65,0x67,0x61,0x18,0x03,0x20,0x01,0x28,0x01,0x52, -0x05,0x6f,0x6d,0x65,0x67,0x61,0x22,0x46,0x0a,0x23, +0x05,0x6f,0x6d,0x65,0x67,0x61,0x22,0x45,0x0a,0x23, 0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,0x69, 0x66,0x66,0x65,0x72,0x65,0x6e,0x74,0x69,0x61,0x6c, 0x44,0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,0x65,0x6d, -0x61,0x74,0x69,0x63,0x73,0x12,0x1f,0x0a,0x0b,0x74, -0x72,0x61,0x63,0x6b,0x5f,0x77,0x69,0x64,0x74,0x68, -0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x0a,0x74,0x72, -0x61,0x63,0x6b,0x57,0x69,0x64,0x74,0x68,0x22,0x50, -0x0a,0x24,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, -0x44,0x69,0x66,0x66,0x65,0x72,0x65,0x6e,0x74,0x69, -0x61,0x6c,0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65, -0x65,0x6c,0x53,0x70,0x65,0x65,0x64,0x73,0x12,0x12, -0x0a,0x04,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01, -0x28,0x01,0x52,0x04,0x6c,0x65,0x66,0x74,0x12,0x14, -0x0a,0x05,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20, -0x01,0x28,0x01,0x52,0x05,0x72,0x69,0x67,0x68,0x74, -0x22,0x53,0x0a,0x27,0x50,0x72,0x6f,0x74,0x6f,0x62, -0x75,0x66,0x44,0x69,0x66,0x66,0x65,0x72,0x65,0x6e, -0x74,0x69,0x61,0x6c,0x44,0x72,0x69,0x76,0x65,0x57, -0x68,0x65,0x65,0x6c,0x50,0x6f,0x73,0x69,0x74,0x69, -0x6f,0x6e,0x73,0x12,0x12,0x0a,0x04,0x6c,0x65,0x66, -0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x04,0x6c, -0x65,0x66,0x74,0x12,0x14,0x0a,0x05,0x72,0x69,0x67, -0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x05, -0x72,0x69,0x67,0x68,0x74,0x22,0xa4,0x02,0x0a,0x1e, -0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x4d,0x65, -0x63,0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,0x76,0x65, -0x4b,0x69,0x6e,0x65,0x6d,0x61,0x74,0x69,0x63,0x73, -0x12,0x3f,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f, -0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x0b, +0x61,0x74,0x69,0x63,0x73,0x12,0x1e,0x0a,0x0a,0x74, +0x72,0x61,0x63,0x6b,0x77,0x69,0x64,0x74,0x68,0x18, +0x01,0x20,0x01,0x28,0x01,0x52,0x0a,0x74,0x72,0x61, +0x63,0x6b,0x77,0x69,0x64,0x74,0x68,0x22,0x50,0x0a, +0x24,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44, +0x69,0x66,0x66,0x65,0x72,0x65,0x6e,0x74,0x69,0x61, +0x6c,0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65, +0x6c,0x53,0x70,0x65,0x65,0x64,0x73,0x12,0x12,0x0a, +0x04,0x6c,0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28, +0x01,0x52,0x04,0x6c,0x65,0x66,0x74,0x12,0x14,0x0a, +0x05,0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01, +0x28,0x01,0x52,0x05,0x72,0x69,0x67,0x68,0x74,0x22, +0x53,0x0a,0x27,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, +0x66,0x44,0x69,0x66,0x66,0x65,0x72,0x65,0x6e,0x74, +0x69,0x61,0x6c,0x44,0x72,0x69,0x76,0x65,0x57,0x68, +0x65,0x65,0x6c,0x50,0x6f,0x73,0x69,0x74,0x69,0x6f, +0x6e,0x73,0x12,0x12,0x0a,0x04,0x6c,0x65,0x66,0x74, +0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x04,0x6c,0x65, +0x66,0x74,0x12,0x14,0x0a,0x05,0x72,0x69,0x67,0x68, +0x74,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x05,0x72, +0x69,0x67,0x68,0x74,0x22,0xa4,0x02,0x0a,0x1e,0x50, +0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63, +0x61,0x6e,0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x4b, +0x69,0x6e,0x65,0x6d,0x61,0x74,0x69,0x63,0x73,0x12, +0x3f,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c, +0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x0b,0x32, +0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,0x74, +0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, +0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,0x74,0x69,0x6f, +0x6e,0x32,0x64,0x52,0x09,0x66,0x72,0x6f,0x6e,0x74, +0x4c,0x65,0x66,0x74,0x12,0x41,0x0a,0x0b,0x66,0x72, +0x6f,0x6e,0x74,0x5f,0x72,0x69,0x67,0x68,0x74,0x18, +0x02,0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70, +0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72, +0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e, +0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52, +0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69,0x67,0x68, +0x74,0x12,0x3d,0x0a,0x09,0x72,0x65,0x61,0x72,0x5f, +0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01,0x28,0x0b, 0x32,0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f, 0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, 0x66,0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,0x74,0x69, -0x6f,0x6e,0x32,0x64,0x52,0x09,0x66,0x72,0x6f,0x6e, -0x74,0x4c,0x65,0x66,0x74,0x12,0x41,0x0a,0x0b,0x66, -0x72,0x6f,0x6e,0x74,0x5f,0x72,0x69,0x67,0x68,0x74, -0x18,0x02,0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,0x77, -0x70,0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50, -0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61, -0x6e,0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64, +0x6f,0x6e,0x32,0x64,0x52,0x08,0x72,0x65,0x61,0x72, +0x4c,0x65,0x66,0x74,0x12,0x3f,0x0a,0x0a,0x72,0x65, +0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04, +0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69, +0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f, +0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73, +0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x09, +0x72,0x65,0x61,0x72,0x52,0x69,0x67,0x68,0x74,0x22, +0xa0,0x01,0x0a,0x22,0x50,0x72,0x6f,0x74,0x6f,0x62, +0x75,0x66,0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d,0x44, +0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65,0x6c,0x50, +0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x73,0x12,0x1d, +0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c,0x65, +0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x09, +0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74,0x12, +0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x72, +0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x01, 0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69,0x67, -0x68,0x74,0x12,0x3d,0x0a,0x09,0x72,0x65,0x61,0x72, +0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,0x61,0x72, 0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01,0x28, -0x0b,0x32,0x20,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72, -0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62, -0x75,0x66,0x54,0x72,0x61,0x6e,0x73,0x6c,0x61,0x74, -0x69,0x6f,0x6e,0x32,0x64,0x52,0x08,0x72,0x65,0x61, -0x72,0x4c,0x65,0x66,0x74,0x12,0x3f,0x0a,0x0a,0x72, -0x65,0x61,0x72,0x5f,0x72,0x69,0x67,0x68,0x74,0x18, -0x04,0x20,0x01,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70, -0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72, -0x6f,0x74,0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e, -0x73,0x6c,0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52, -0x09,0x72,0x65,0x61,0x72,0x52,0x69,0x67,0x68,0x74, -0x22,0xa0,0x01,0x0a,0x22,0x50,0x72,0x6f,0x74,0x6f, -0x62,0x75,0x66,0x4d,0x65,0x63,0x61,0x6e,0x75,0x6d, -0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65,0x65,0x6c, -0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x73,0x12, -0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c, -0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52, -0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74, -0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f, -0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28, -0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69, -0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,0x61, -0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01, -0x28,0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,0x65, -0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,0x72, -0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,0x01, -0x28,0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69, -0x67,0x68,0x74,0x22,0x9d,0x01,0x0a,0x1f,0x50,0x72, -0x6f,0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63,0x61, -0x6e,0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x57,0x68, -0x65,0x65,0x6c,0x53,0x70,0x65,0x65,0x64,0x73,0x12, -0x1d,0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c, -0x65,0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52, -0x09,0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74, -0x12,0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f, -0x72,0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28, -0x01,0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69, -0x67,0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,0x61, -0x72,0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01, -0x28,0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,0x65, -0x66,0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,0x72, -0x5f,0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,0x01, -0x28,0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69, -0x67,0x68,0x74,0x22,0x5b,0x0a,0x1d,0x50,0x72,0x6f, -0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72,0x76, -0x65,0x44,0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,0x65, -0x6d,0x61,0x74,0x69,0x63,0x73,0x12,0x3a,0x0a,0x07, -0x6d,0x6f,0x64,0x75,0x6c,0x65,0x73,0x18,0x01,0x20, -0x03,0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69,0x2e, -0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74, -0x6f,0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73,0x6c, -0x61,0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x07,0x6d, -0x6f,0x64,0x75,0x6c,0x65,0x73,0x22,0x6f,0x0a,0x1c, -0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77, -0x65,0x72,0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65, -0x50,0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x12,0x1a, -0x0a,0x08,0x64,0x69,0x73,0x74,0x61,0x6e,0x63,0x65, -0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x08,0x64,0x69, -0x73,0x74,0x61,0x6e,0x63,0x65,0x12,0x33,0x0a,0x05, -0x61,0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01,0x28, -0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72, -0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62, -0x75,0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f,0x6e, -0x32,0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65,0x22, -0x66,0x0a,0x19,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, -0x66,0x53,0x77,0x65,0x72,0x76,0x65,0x4d,0x6f,0x64, -0x75,0x6c,0x65,0x53,0x74,0x61,0x74,0x65,0x12,0x14, -0x0a,0x05,0x73,0x70,0x65,0x65,0x64,0x18,0x01,0x20, -0x01,0x28,0x01,0x52,0x05,0x73,0x70,0x65,0x65,0x64, -0x12,0x33,0x0a,0x05,0x61,0x6e,0x67,0x6c,0x65,0x18, -0x02,0x20,0x01,0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70, -0x69,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72, -0x6f,0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74,0x61, -0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61,0x6e, -0x67,0x6c,0x65,0x42,0x1a,0x0a,0x18,0x65,0x64,0x75, -0x2e,0x77,0x70,0x69,0x2e,0x66,0x69,0x72,0x73,0x74, -0x2e,0x6d,0x61,0x74,0x68,0x2e,0x70,0x72,0x6f,0x74, -0x6f,0x4a,0x99,0x0d,0x0a,0x06,0x12,0x04,0x00,0x00, -0x3d,0x01,0x0a,0x08,0x0a,0x01,0x0c,0x12,0x03,0x00, -0x00,0x12,0x0a,0x08,0x0a,0x01,0x02,0x12,0x03,0x02, -0x00,0x12,0x0a,0x09,0x0a,0x02,0x03,0x00,0x12,0x03, -0x04,0x00,0x1a,0x0a,0x08,0x0a,0x01,0x08,0x12,0x03, -0x06,0x00,0x31,0x0a,0x09,0x0a,0x02,0x08,0x01,0x12, -0x03,0x06,0x00,0x31,0x0a,0x0a,0x0a,0x02,0x04,0x00, -0x12,0x04,0x08,0x00,0x0c,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x00,0x01,0x12,0x03,0x08,0x08,0x1d,0x0a,0x0b, -0x0a,0x04,0x04,0x00,0x02,0x00,0x12,0x03,0x09,0x02, -0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x05, -0x12,0x03,0x09,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x00,0x01,0x12,0x03,0x09,0x09,0x0b,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x03,0x12,0x03, -0x09,0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02, -0x01,0x12,0x03,0x0a,0x02,0x10,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x01,0x05,0x12,0x03,0x0a,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x01,0x12, -0x03,0x0a,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x01,0x03,0x12,0x03,0x0a,0x0e,0x0f,0x0a,0x0b, -0x0a,0x04,0x04,0x00,0x02,0x02,0x12,0x03,0x0b,0x02, -0x13,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x05, -0x12,0x03,0x0b,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x02,0x01,0x12,0x03,0x0b,0x09,0x0e,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x03,0x12,0x03, -0x0b,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12, -0x04,0x0e,0x00,0x10,0x01,0x0a,0x0a,0x0a,0x03,0x04, -0x01,0x01,0x12,0x03,0x0e,0x08,0x2b,0x0a,0x0b,0x0a, -0x04,0x04,0x01,0x02,0x00,0x12,0x03,0x0f,0x02,0x19, -0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x05,0x12, -0x03,0x0f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01, -0x02,0x00,0x01,0x12,0x03,0x0f,0x09,0x14,0x0a,0x0c, -0x0a,0x05,0x04,0x01,0x02,0x00,0x03,0x12,0x03,0x0f, -0x17,0x18,0x0a,0x0a,0x0a,0x02,0x04,0x02,0x12,0x04, -0x12,0x00,0x15,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02, -0x01,0x12,0x03,0x12,0x08,0x2c,0x0a,0x0b,0x0a,0x04, -0x04,0x02,0x02,0x00,0x12,0x03,0x13,0x02,0x12,0x0a, -0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x05,0x12,0x03, -0x13,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, -0x00,0x01,0x12,0x03,0x13,0x09,0x0d,0x0a,0x0c,0x0a, -0x05,0x04,0x02,0x02,0x00,0x03,0x12,0x03,0x13,0x10, -0x11,0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x01,0x12, -0x03,0x14,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02, -0x02,0x01,0x05,0x12,0x03,0x14,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x02,0x02,0x01,0x01,0x12,0x03,0x14, -0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x01, -0x03,0x12,0x03,0x14,0x11,0x12,0x0a,0x0a,0x0a,0x02, -0x04,0x03,0x12,0x04,0x17,0x00,0x1a,0x01,0x0a,0x0a, -0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x17,0x08,0x2f, -0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12,0x03, -0x18,0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, -0x00,0x05,0x12,0x03,0x18,0x02,0x08,0x0a,0x0c,0x0a, -0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x18,0x09, -0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x03, -0x12,0x03,0x18,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04, -0x03,0x02,0x01,0x12,0x03,0x19,0x02,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03,0x19, -0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01, -0x01,0x12,0x03,0x19,0x09,0x0e,0x0a,0x0c,0x0a,0x05, -0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x19,0x11,0x12, -0x0a,0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1c,0x00, -0x21,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01,0x12, -0x03,0x1c,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x04, -0x02,0x00,0x12,0x03,0x1d,0x02,0x27,0x0a,0x0c,0x0a, -0x05,0x04,0x04,0x02,0x00,0x06,0x12,0x03,0x1d,0x02, -0x17,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x01, -0x12,0x03,0x1d,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04, -0x04,0x02,0x00,0x03,0x12,0x03,0x1d,0x25,0x26,0x0a, -0x0b,0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03,0x1e, -0x02,0x28,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01, -0x06,0x12,0x03,0x1e,0x02,0x17,0x0a,0x0c,0x0a,0x05, -0x04,0x04,0x02,0x01,0x01,0x12,0x03,0x1e,0x18,0x23, -0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03,0x12, -0x03,0x1e,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x04, -0x02,0x02,0x12,0x03,0x1f,0x02,0x26,0x0a,0x0c,0x0a, -0x05,0x04,0x04,0x02,0x02,0x06,0x12,0x03,0x1f,0x02, -0x17,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x02,0x01, -0x12,0x03,0x1f,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04, -0x04,0x02,0x02,0x03,0x12,0x03,0x1f,0x24,0x25,0x0a, -0x0b,0x0a,0x04,0x04,0x04,0x02,0x03,0x12,0x03,0x20, -0x02,0x27,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x03, -0x06,0x12,0x03,0x20,0x02,0x17,0x0a,0x0c,0x0a,0x05, -0x04,0x04,0x02,0x03,0x01,0x12,0x03,0x20,0x18,0x22, -0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x03,0x03,0x12, -0x03,0x20,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x05, -0x12,0x04,0x23,0x00,0x28,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x05,0x01,0x12,0x03,0x23,0x08,0x2a,0x0a,0x0b, -0x0a,0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x24,0x02, -0x18,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x05, -0x12,0x03,0x24,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x05,0x02,0x00,0x01,0x12,0x03,0x24,0x09,0x13,0x0a, -0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03, -0x24,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02, -0x01,0x12,0x03,0x25,0x02,0x19,0x0a,0x0c,0x0a,0x05, -0x04,0x05,0x02,0x01,0x05,0x12,0x03,0x25,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01,0x12, -0x03,0x25,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x05, -0x02,0x01,0x03,0x12,0x03,0x25,0x17,0x18,0x0a,0x0b, -0x0a,0x04,0x04,0x05,0x02,0x02,0x12,0x03,0x26,0x02, -0x17,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x02,0x05, -0x12,0x03,0x26,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x05,0x02,0x02,0x01,0x12,0x03,0x26,0x09,0x12,0x0a, -0x0c,0x0a,0x05,0x04,0x05,0x02,0x02,0x03,0x12,0x03, -0x26,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02, -0x03,0x12,0x03,0x27,0x02,0x18,0x0a,0x0c,0x0a,0x05, -0x04,0x05,0x02,0x03,0x05,0x12,0x03,0x27,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x03,0x01,0x12, -0x03,0x27,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x05, -0x02,0x03,0x03,0x12,0x03,0x27,0x16,0x17,0x0a,0x0a, -0x0a,0x02,0x04,0x06,0x12,0x04,0x2a,0x00,0x2f,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03,0x2a, -0x08,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x00, -0x12,0x03,0x2b,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x00,0x05,0x12,0x03,0x2b,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12,0x03, -0x2b,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, -0x00,0x03,0x12,0x03,0x2b,0x16,0x17,0x0a,0x0b,0x0a, -0x04,0x04,0x06,0x02,0x01,0x12,0x03,0x2c,0x02,0x19, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x05,0x12, -0x03,0x2c,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x01,0x01,0x12,0x03,0x2c,0x09,0x14,0x0a,0x0c, -0x0a,0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x2c, -0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x02, -0x12,0x03,0x2d,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x02,0x05,0x12,0x03,0x2d,0x02,0x08,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03, -0x2d,0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, -0x02,0x03,0x12,0x03,0x2d,0x15,0x16,0x0a,0x0b,0x0a, -0x04,0x04,0x06,0x02,0x03,0x12,0x03,0x2e,0x02,0x18, -0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x05,0x12, -0x03,0x2e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06, -0x02,0x03,0x01,0x12,0x03,0x2e,0x09,0x13,0x0a,0x0c, -0x0a,0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03,0x2e, -0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12,0x04, -0x31,0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x07, -0x01,0x12,0x03,0x31,0x08,0x25,0x0a,0x0b,0x0a,0x04, -0x04,0x07,0x02,0x00,0x12,0x03,0x32,0x02,0x2d,0x0a, -0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x04,0x12,0x03, -0x32,0x02,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, -0x00,0x06,0x12,0x03,0x32,0x0b,0x20,0x0a,0x0c,0x0a, -0x05,0x04,0x07,0x02,0x00,0x01,0x12,0x03,0x32,0x21, -0x28,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x03, -0x12,0x03,0x32,0x2b,0x2c,0x0a,0x0a,0x0a,0x02,0x04, -0x08,0x12,0x04,0x35,0x00,0x38,0x01,0x0a,0x0a,0x0a, -0x03,0x04,0x08,0x01,0x12,0x03,0x35,0x08,0x24,0x0a, -0x0b,0x0a,0x04,0x04,0x08,0x02,0x00,0x12,0x03,0x36, -0x02,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00, -0x05,0x12,0x03,0x36,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x08,0x02,0x00,0x01,0x12,0x03,0x36,0x09,0x11, -0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x03,0x12, -0x03,0x36,0x14,0x15,0x0a,0x0b,0x0a,0x04,0x04,0x08, -0x02,0x01,0x12,0x03,0x37,0x02,0x1f,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x01,0x06,0x12,0x03,0x37,0x02, -0x14,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x01, -0x12,0x03,0x37,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04, -0x08,0x02,0x01,0x03,0x12,0x03,0x37,0x1d,0x1e,0x0a, -0x0a,0x0a,0x02,0x04,0x09,0x12,0x04,0x3a,0x00,0x3d, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01,0x12,0x03, -0x3a,0x08,0x21,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02, -0x00,0x12,0x03,0x3b,0x02,0x13,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x00,0x05,0x12,0x03,0x3b,0x02,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x01,0x12, -0x03,0x3b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x09, -0x02,0x00,0x03,0x12,0x03,0x3b,0x11,0x12,0x0a,0x0b, -0x0a,0x04,0x04,0x09,0x02,0x01,0x12,0x03,0x3c,0x02, -0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x06, -0x12,0x03,0x3c,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04, -0x09,0x02,0x01,0x01,0x12,0x03,0x3c,0x15,0x1a,0x0a, -0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x03,0x12,0x03, -0x3c,0x1d,0x1e,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f, -0x33, +0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,0x65,0x66, +0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,0x72,0x5f, +0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,0x01,0x28, +0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69,0x67, +0x68,0x74,0x22,0x9d,0x01,0x0a,0x1f,0x50,0x72,0x6f, +0x74,0x6f,0x62,0x75,0x66,0x4d,0x65,0x63,0x61,0x6e, +0x75,0x6d,0x44,0x72,0x69,0x76,0x65,0x57,0x68,0x65, +0x65,0x6c,0x53,0x70,0x65,0x65,0x64,0x73,0x12,0x1d, +0x0a,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x6c,0x65, +0x66,0x74,0x18,0x01,0x20,0x01,0x28,0x01,0x52,0x09, +0x66,0x72,0x6f,0x6e,0x74,0x4c,0x65,0x66,0x74,0x12, +0x1f,0x0a,0x0b,0x66,0x72,0x6f,0x6e,0x74,0x5f,0x72, +0x69,0x67,0x68,0x74,0x18,0x02,0x20,0x01,0x28,0x01, +0x52,0x0a,0x66,0x72,0x6f,0x6e,0x74,0x52,0x69,0x67, +0x68,0x74,0x12,0x1b,0x0a,0x09,0x72,0x65,0x61,0x72, +0x5f,0x6c,0x65,0x66,0x74,0x18,0x03,0x20,0x01,0x28, +0x01,0x52,0x08,0x72,0x65,0x61,0x72,0x4c,0x65,0x66, +0x74,0x12,0x1d,0x0a,0x0a,0x72,0x65,0x61,0x72,0x5f, +0x72,0x69,0x67,0x68,0x74,0x18,0x04,0x20,0x01,0x28, +0x01,0x52,0x09,0x72,0x65,0x61,0x72,0x52,0x69,0x67, +0x68,0x74,0x22,0x5b,0x0a,0x1d,0x50,0x72,0x6f,0x74, +0x6f,0x62,0x75,0x66,0x53,0x77,0x65,0x72,0x76,0x65, +0x44,0x72,0x69,0x76,0x65,0x4b,0x69,0x6e,0x65,0x6d, +0x61,0x74,0x69,0x63,0x73,0x12,0x3a,0x0a,0x07,0x6d, +0x6f,0x64,0x75,0x6c,0x65,0x73,0x18,0x01,0x20,0x03, +0x28,0x0b,0x32,0x20,0x2e,0x77,0x70,0x69,0x2e,0x70, +0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f, +0x62,0x75,0x66,0x54,0x72,0x61,0x6e,0x73,0x6c,0x61, +0x74,0x69,0x6f,0x6e,0x32,0x64,0x52,0x07,0x6d,0x6f, +0x64,0x75,0x6c,0x65,0x73,0x22,0x6f,0x0a,0x1c,0x50, +0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x53,0x77,0x65, +0x72,0x76,0x65,0x4d,0x6f,0x64,0x75,0x6c,0x65,0x50, +0x6f,0x73,0x69,0x74,0x69,0x6f,0x6e,0x12,0x1a,0x0a, +0x08,0x64,0x69,0x73,0x74,0x61,0x6e,0x63,0x65,0x18, +0x01,0x20,0x01,0x28,0x01,0x52,0x08,0x64,0x69,0x73, +0x74,0x61,0x6e,0x63,0x65,0x12,0x33,0x0a,0x05,0x61, +0x6e,0x67,0x6c,0x65,0x18,0x02,0x20,0x01,0x28,0x0b, +0x32,0x1d,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f, +0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, +0x66,0x52,0x6f,0x74,0x61,0x74,0x69,0x6f,0x6e,0x32, +0x64,0x52,0x05,0x61,0x6e,0x67,0x6c,0x65,0x22,0x66, +0x0a,0x19,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, +0x53,0x77,0x65,0x72,0x76,0x65,0x4d,0x6f,0x64,0x75, +0x6c,0x65,0x53,0x74,0x61,0x74,0x65,0x12,0x14,0x0a, +0x05,0x73,0x70,0x65,0x65,0x64,0x18,0x01,0x20,0x01, +0x28,0x01,0x52,0x05,0x73,0x70,0x65,0x65,0x64,0x12, +0x33,0x0a,0x05,0x61,0x6e,0x67,0x6c,0x65,0x18,0x02, +0x20,0x01,0x28,0x0b,0x32,0x1d,0x2e,0x77,0x70,0x69, +0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f, +0x74,0x6f,0x62,0x75,0x66,0x52,0x6f,0x74,0x61,0x74, +0x69,0x6f,0x6e,0x32,0x64,0x52,0x05,0x61,0x6e,0x67, +0x6c,0x65,0x42,0x1a,0x0a,0x18,0x65,0x64,0x75,0x2e, +0x77,0x70,0x69,0x2e,0x66,0x69,0x72,0x73,0x74,0x2e, +0x6d,0x61,0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f, +0x4a,0x99,0x0d,0x0a,0x06,0x12,0x04,0x00,0x00,0x3d, +0x01,0x0a,0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00, +0x12,0x0a,0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00, +0x12,0x0a,0x09,0x0a,0x02,0x03,0x00,0x12,0x03,0x04, +0x00,0x1a,0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x06, +0x00,0x31,0x0a,0x09,0x0a,0x02,0x08,0x01,0x12,0x03, +0x06,0x00,0x31,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12, +0x04,0x08,0x00,0x0c,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x00,0x01,0x12,0x03,0x08,0x08,0x1d,0x0a,0x0b,0x0a, +0x04,0x04,0x00,0x02,0x00,0x12,0x03,0x09,0x02,0x10, +0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12, +0x03,0x09,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00, +0x02,0x00,0x01,0x12,0x03,0x09,0x09,0x0b,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x09, +0x0e,0x0f,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01, +0x12,0x03,0x0a,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x01,0x05,0x12,0x03,0x0a,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03, +0x0a,0x09,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, +0x01,0x03,0x12,0x03,0x0a,0x0e,0x0f,0x0a,0x0b,0x0a, +0x04,0x04,0x00,0x02,0x02,0x12,0x03,0x0b,0x02,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12, +0x03,0x0b,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00, +0x02,0x02,0x01,0x12,0x03,0x0b,0x09,0x0e,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0b, +0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12,0x04, +0x0e,0x00,0x10,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01, +0x01,0x12,0x03,0x0e,0x08,0x2b,0x0a,0x0b,0x0a,0x04, +0x04,0x01,0x02,0x00,0x12,0x03,0x0f,0x02,0x18,0x0a, +0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x05,0x12,0x03, +0x0f,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, +0x00,0x01,0x12,0x03,0x0f,0x09,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x00,0x03,0x12,0x03,0x0f,0x16, +0x17,0x0a,0x0a,0x0a,0x02,0x04,0x02,0x12,0x04,0x12, +0x00,0x15,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01, +0x12,0x03,0x12,0x08,0x2c,0x0a,0x0b,0x0a,0x04,0x04, +0x02,0x02,0x00,0x12,0x03,0x13,0x02,0x12,0x0a,0x0c, +0x0a,0x05,0x04,0x02,0x02,0x00,0x05,0x12,0x03,0x13, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00, +0x01,0x12,0x03,0x13,0x09,0x0d,0x0a,0x0c,0x0a,0x05, +0x04,0x02,0x02,0x00,0x03,0x12,0x03,0x13,0x10,0x11, +0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x01,0x12,0x03, +0x14,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, +0x01,0x05,0x12,0x03,0x14,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x02,0x02,0x01,0x01,0x12,0x03,0x14,0x09, +0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x01,0x03, +0x12,0x03,0x14,0x11,0x12,0x0a,0x0a,0x0a,0x02,0x04, +0x03,0x12,0x04,0x17,0x00,0x1a,0x01,0x0a,0x0a,0x0a, +0x03,0x04,0x03,0x01,0x12,0x03,0x17,0x08,0x2f,0x0a, +0x0b,0x0a,0x04,0x04,0x03,0x02,0x00,0x12,0x03,0x18, +0x02,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00, +0x05,0x12,0x03,0x18,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x03,0x02,0x00,0x01,0x12,0x03,0x18,0x09,0x0d, +0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x03,0x12, +0x03,0x18,0x10,0x11,0x0a,0x0b,0x0a,0x04,0x04,0x03, +0x02,0x01,0x12,0x03,0x19,0x02,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x03,0x02,0x01,0x05,0x12,0x03,0x19,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x01, +0x12,0x03,0x19,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04, +0x03,0x02,0x01,0x03,0x12,0x03,0x19,0x11,0x12,0x0a, +0x0a,0x0a,0x02,0x04,0x04,0x12,0x04,0x1c,0x00,0x21, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x04,0x01,0x12,0x03, +0x1c,0x08,0x26,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02, +0x00,0x12,0x03,0x1d,0x02,0x27,0x0a,0x0c,0x0a,0x05, +0x04,0x04,0x02,0x00,0x06,0x12,0x03,0x1d,0x02,0x17, +0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x01,0x12, +0x03,0x1d,0x18,0x22,0x0a,0x0c,0x0a,0x05,0x04,0x04, +0x02,0x00,0x03,0x12,0x03,0x1d,0x25,0x26,0x0a,0x0b, +0x0a,0x04,0x04,0x04,0x02,0x01,0x12,0x03,0x1e,0x02, +0x28,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x06, +0x12,0x03,0x1e,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, +0x04,0x02,0x01,0x01,0x12,0x03,0x1e,0x18,0x23,0x0a, +0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x03,0x12,0x03, +0x1e,0x26,0x27,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02, +0x02,0x12,0x03,0x1f,0x02,0x26,0x0a,0x0c,0x0a,0x05, +0x04,0x04,0x02,0x02,0x06,0x12,0x03,0x1f,0x02,0x17, +0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x02,0x01,0x12, +0x03,0x1f,0x18,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x04, +0x02,0x02,0x03,0x12,0x03,0x1f,0x24,0x25,0x0a,0x0b, +0x0a,0x04,0x04,0x04,0x02,0x03,0x12,0x03,0x20,0x02, +0x27,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x03,0x06, +0x12,0x03,0x20,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04, +0x04,0x02,0x03,0x01,0x12,0x03,0x20,0x18,0x22,0x0a, +0x0c,0x0a,0x05,0x04,0x04,0x02,0x03,0x03,0x12,0x03, +0x20,0x25,0x26,0x0a,0x0a,0x0a,0x02,0x04,0x05,0x12, +0x04,0x23,0x00,0x28,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x05,0x01,0x12,0x03,0x23,0x08,0x2a,0x0a,0x0b,0x0a, +0x04,0x04,0x05,0x02,0x00,0x12,0x03,0x24,0x02,0x18, +0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x05,0x12, +0x03,0x24,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05, +0x02,0x00,0x01,0x12,0x03,0x24,0x09,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03,0x24, +0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x01, +0x12,0x03,0x25,0x02,0x19,0x0a,0x0c,0x0a,0x05,0x04, +0x05,0x02,0x01,0x05,0x12,0x03,0x25,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x05,0x02,0x01,0x01,0x12,0x03, +0x25,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, +0x01,0x03,0x12,0x03,0x25,0x17,0x18,0x0a,0x0b,0x0a, +0x04,0x04,0x05,0x02,0x02,0x12,0x03,0x26,0x02,0x17, +0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x02,0x05,0x12, +0x03,0x26,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x05, +0x02,0x02,0x01,0x12,0x03,0x26,0x09,0x12,0x0a,0x0c, +0x0a,0x05,0x04,0x05,0x02,0x02,0x03,0x12,0x03,0x26, +0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x05,0x02,0x03, +0x12,0x03,0x27,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04, +0x05,0x02,0x03,0x05,0x12,0x03,0x27,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x05,0x02,0x03,0x01,0x12,0x03, +0x27,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02, +0x03,0x03,0x12,0x03,0x27,0x16,0x17,0x0a,0x0a,0x0a, +0x02,0x04,0x06,0x12,0x04,0x2a,0x00,0x2f,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x06,0x01,0x12,0x03,0x2a,0x08, +0x27,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x00,0x12, +0x03,0x2b,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x00,0x05,0x12,0x03,0x2b,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x00,0x01,0x12,0x03,0x2b, +0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00, +0x03,0x12,0x03,0x2b,0x16,0x17,0x0a,0x0b,0x0a,0x04, +0x04,0x06,0x02,0x01,0x12,0x03,0x2c,0x02,0x19,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x05,0x12,0x03, +0x2c,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x01,0x01,0x12,0x03,0x2c,0x09,0x14,0x0a,0x0c,0x0a, +0x05,0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x2c,0x17, +0x18,0x0a,0x0b,0x0a,0x04,0x04,0x06,0x02,0x02,0x12, +0x03,0x2d,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x02,0x05,0x12,0x03,0x2d,0x02,0x08,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03,0x2d, +0x09,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02, +0x03,0x12,0x03,0x2d,0x15,0x16,0x0a,0x0b,0x0a,0x04, +0x04,0x06,0x02,0x03,0x12,0x03,0x2e,0x02,0x18,0x0a, +0x0c,0x0a,0x05,0x04,0x06,0x02,0x03,0x05,0x12,0x03, +0x2e,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, +0x03,0x01,0x12,0x03,0x2e,0x09,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x06,0x02,0x03,0x03,0x12,0x03,0x2e,0x16, +0x17,0x0a,0x0a,0x0a,0x02,0x04,0x07,0x12,0x04,0x31, +0x00,0x33,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x07,0x01, +0x12,0x03,0x31,0x08,0x25,0x0a,0x0b,0x0a,0x04,0x04, +0x07,0x02,0x00,0x12,0x03,0x32,0x02,0x2d,0x0a,0x0c, +0x0a,0x05,0x04,0x07,0x02,0x00,0x04,0x12,0x03,0x32, +0x02,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00, +0x06,0x12,0x03,0x32,0x0b,0x20,0x0a,0x0c,0x0a,0x05, +0x04,0x07,0x02,0x00,0x01,0x12,0x03,0x32,0x21,0x28, +0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x03,0x12, +0x03,0x32,0x2b,0x2c,0x0a,0x0a,0x0a,0x02,0x04,0x08, +0x12,0x04,0x35,0x00,0x38,0x01,0x0a,0x0a,0x0a,0x03, +0x04,0x08,0x01,0x12,0x03,0x35,0x08,0x24,0x0a,0x0b, +0x0a,0x04,0x04,0x08,0x02,0x00,0x12,0x03,0x36,0x02, +0x16,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x05, +0x12,0x03,0x36,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x00,0x01,0x12,0x03,0x36,0x09,0x11,0x0a, +0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x03,0x12,0x03, +0x36,0x14,0x15,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02, +0x01,0x12,0x03,0x37,0x02,0x1f,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x01,0x06,0x12,0x03,0x37,0x02,0x14, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x01,0x12, +0x03,0x37,0x15,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x08, +0x02,0x01,0x03,0x12,0x03,0x37,0x1d,0x1e,0x0a,0x0a, +0x0a,0x02,0x04,0x09,0x12,0x04,0x3a,0x00,0x3d,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x09,0x01,0x12,0x03,0x3a, +0x08,0x21,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x00, +0x12,0x03,0x3b,0x02,0x13,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x00,0x05,0x12,0x03,0x3b,0x02,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x01,0x12,0x03, +0x3b,0x09,0x0e,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, +0x00,0x03,0x12,0x03,0x3b,0x11,0x12,0x0a,0x0b,0x0a, +0x04,0x04,0x09,0x02,0x01,0x12,0x03,0x3c,0x02,0x1f, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x06,0x12, +0x03,0x3c,0x02,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x01,0x01,0x12,0x03,0x3c,0x15,0x1a,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x01,0x03,0x12,0x03,0x3c, +0x1d,0x1e,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, + }; static const char file_name[] = "kinematics.proto"; static const char wpi_proto_ProtobufChassisSpeeds_name[] = "wpi.proto.ProtobufChassisSpeeds"; diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h index cbb27afc08c..0f8900c8cbc 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/kinematics.npb.h @@ -31,7 +31,7 @@ typedef struct _wpi_proto_ProtobufDifferentialDriveKinematics { static std::string_view msg_name(void) noexcept; static pb_filedesc_t file_descriptor(void) noexcept; - double track_width; + double trackwidth; } wpi_proto_ProtobufDifferentialDriveKinematics; typedef struct _wpi_proto_ProtobufDifferentialDriveWheelSpeeds { @@ -138,7 +138,7 @@ typedef struct _wpi_proto_ProtobufSwerveModuleState { #define wpi_proto_ProtobufChassisSpeeds_vx_tag 1 #define wpi_proto_ProtobufChassisSpeeds_vy_tag 2 #define wpi_proto_ProtobufChassisSpeeds_omega_tag 3 -#define wpi_proto_ProtobufDifferentialDriveKinematics_track_width_tag 1 +#define wpi_proto_ProtobufDifferentialDriveKinematics_trackwidth_tag 1 #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_left_tag 1 #define wpi_proto_ProtobufDifferentialDriveWheelSpeeds_right_tag 2 #define wpi_proto_ProtobufDifferentialDriveWheelPositions_left_tag 1 @@ -170,7 +170,7 @@ X(a, STATIC, SINGULAR, DOUBLE, omega, 3) #define wpi_proto_ProtobufChassisSpeeds_DEFAULT NULL #define wpi_proto_ProtobufDifferentialDriveKinematics_FIELDLIST(X, a) \ -X(a, STATIC, SINGULAR, DOUBLE, track_width, 1) +X(a, STATIC, SINGULAR, DOUBLE, trackwidth, 1) #define wpi_proto_ProtobufDifferentialDriveKinematics_CALLBACK NULL #define wpi_proto_ProtobufDifferentialDriveKinematics_DEFAULT NULL diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index fb26ae13319..d406819a997 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -37,12 +37,12 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable * @param kg The gravity gain in volts. * @param kv The velocity gain in V/(rad/s). * @param ka The acceleration gain in V/(rad/s²). - * @param dtSeconds The period in seconds. + * @param dt The period in seconds. * @throws IllegalArgumentException for kv < zero. * @throws IllegalArgumentException for ka < zero. * @throws IllegalArgumentException for period ≤ zero. */ - public ArmFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) { + public ArmFeedforward(double ks, double kg, double kv, double ka, double dt) { this.ks = ks; this.kg = kg; this.kv = kv; @@ -53,11 +53,10 @@ public ArmFeedforward(double ks, double kg, double kv, double ka, double dtSecon if (ka < 0.0) { throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); } - if (dtSeconds <= 0.0) { - throw new IllegalArgumentException( - "period must be a positive number, got " + dtSeconds + "!"); + if (dt <= 0.0) { + throw new IllegalArgumentException("period must be a positive number, got " + dt + "!"); } - m_dt = dtSeconds; + m_dt = dt; } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java index 9b14542207f..ca541b103a3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java @@ -54,14 +54,14 @@ public class ControlAffinePlantInversionFeedforward states, Nat inputs, BiFunction, Matrix, Matrix> f, - double dtSeconds) { - this.m_dt = dtSeconds; + double dt) { + this.m_dt = dt; this.m_f = f; this.m_inputs = inputs; @@ -84,15 +84,15 @@ public ControlAffinePlantInversionFeedforward( * @param f A vector-valued function of x, the state, that returns the derivative of the state * vector. * @param B Continuous input matrix of the plant being controlled. - * @param dtSeconds The timestep between calls of calculate(). + * @param dt The timestep between calls of calculate() in seconds. */ public ControlAffinePlantInversionFeedforward( Nat states, Nat inputs, Function, Matrix> f, Matrix B, - double dtSeconds) { - this.m_dt = dtSeconds; + double dt) { + this.m_dt = dt; this.m_inputs = inputs; this.m_f = (x, u) -> f.apply(x); diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java index 15be9f62f9b..549adbd4a24 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java @@ -71,7 +71,7 @@ public DifferentialDriveFeedforward( * @param currentRightVelocity The current right velocity of the differential drive in * meters/second. * @param nextRightVelocity The next right velocity of the differential drive in meters/second. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. * @return A DifferentialDriveWheelVoltages object containing the computed feedforward voltages. */ public DifferentialDriveWheelVoltages calculate( @@ -79,8 +79,8 @@ public DifferentialDriveWheelVoltages calculate( double nextLeftVelocity, double currentRightVelocity, double nextRightVelocity, - double dtSeconds) { - var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dtSeconds); + double dt) { + var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dt); var r = VecBuilder.fill(currentLeftVelocity, currentRightVelocity); var nextR = VecBuilder.fill(nextLeftVelocity, nextRightVelocity); var u = feedforward.calculate(r, nextR); diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 71601612ea1..0702af3f631 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -36,12 +36,12 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ * @param kg The gravity gain in volts. * @param kv The velocity gain in V/(m/s). * @param ka The acceleration gain in V/(m/s²). - * @param dtSeconds The period in seconds. + * @param dt The period in seconds. * @throws IllegalArgumentException for kv < zero. * @throws IllegalArgumentException for ka < zero. * @throws IllegalArgumentException for period ≤ zero. */ - public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) { + public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dt) { this.ks = ks; this.kg = kg; this.kv = kv; @@ -52,11 +52,10 @@ public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dt if (ka < 0.0) { throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); } - if (dtSeconds <= 0.0) { - throw new IllegalArgumentException( - "period must be a positive number, got " + dtSeconds + "!"); + if (dt <= 0.0) { + throw new IllegalArgumentException("period must be a positive number, got " + dt + "!"); } - m_dt = dtSeconds; + m_dt = dt; } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java index cc5b1438c5a..9d3aaf3b9fd 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java @@ -77,14 +77,14 @@ public void setTolerance(Pose2d tolerance) { * * @param currentPose The current pose, as measured by odometry or pose estimator. * @param trajectoryPose The desired trajectory pose, as sampled for the current timestep. - * @param desiredLinearVelocityMetersPerSecond The desired linear velocity. + * @param desiredLinearVelocity The desired linear velocity in m/s. * @param desiredHeading The desired heading. * @return The next output of the holonomic drive controller. */ public ChassisSpeeds calculate( Pose2d currentPose, Pose2d trajectoryPose, - double desiredLinearVelocityMetersPerSecond, + double desiredLinearVelocity, Rotation2d desiredHeading) { // If this is the first run, then we need to reset the theta controller to the current pose's // heading. @@ -94,8 +94,8 @@ public ChassisSpeeds calculate( } // Calculate feedforward velocities (field-relative). - double xFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getCos(); - double yFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getSin(); + double xFF = desiredLinearVelocity * trajectoryPose.getRotation().getCos(); + double yFF = desiredLinearVelocity * trajectoryPose.getRotation().getSin(); double thetaFF = m_thetaController.calculate( currentPose.getRotation().getRadians(), desiredHeading.getRadians()); @@ -126,8 +126,7 @@ public ChassisSpeeds calculate( */ public ChassisSpeeds calculate( Pose2d currentPose, Trajectory.State desiredState, Rotation2d desiredHeading) { - return calculate( - currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, desiredHeading); + return calculate(currentPose, desiredState.pose, desiredState.velocity, desiredHeading); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index 1f6ca64bb96..6e212780d2c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -242,10 +242,8 @@ public DifferentialDriveWheelVoltages calculate( currentPose, leftVelocity, rightVelocity, - desiredState.poseMeters, - desiredState.velocityMetersPerSecond - * (1 - (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)), - desiredState.velocityMetersPerSecond - * (1 + (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0))); + desiredState.pose, + desiredState.velocity * (1 - (desiredState.curvature * m_trackwidth / 2.0)), + desiredState.velocity * (1 + (desiredState.curvature * m_trackwidth / 2.0))); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java index d95496bd103..d51c8602a7f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -205,9 +205,9 @@ public ChassisSpeeds calculate( public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) { return calculate( currentPose, - desiredState.poseMeters, - desiredState.velocityMetersPerSecond, - desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter); + desiredState.pose, + desiredState.velocity, + desiredState.velocity * desiredState.curvature); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java index f71465a82a2..cba235d5717 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java @@ -40,11 +40,10 @@ public class LinearPlantInversionFeedforward< * Constructs a feedforward with the given plant. * * @param plant The plant being controlled. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. */ - public LinearPlantInversionFeedforward( - LinearSystem plant, double dtSeconds) { - this(plant.getA(), plant.getB(), dtSeconds); + public LinearPlantInversionFeedforward(LinearSystem plant, double dt) { + this(plant.getA(), plant.getB(), dt); } /** @@ -52,11 +51,11 @@ public LinearPlantInversionFeedforward( * * @param A Continuous system matrix of the plant being controlled. * @param B Continuous input matrix of the plant being controlled. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. */ public LinearPlantInversionFeedforward( - Matrix A, Matrix B, double dtSeconds) { - var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + Matrix A, Matrix B, double dt) { + var discABPair = Discretization.discretizeAB(A, B, dt); this.m_A = discABPair.getFirst(); this.m_B = discABPair.getSecond(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java index 7b06a503401..8092e9e2ce2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java @@ -45,20 +45,20 @@ public class LinearQuadraticRegulator plant, Vector qelms, Vector relms, - double dtSeconds) { + double dt) { this( plant.getA(), plant.getB(), StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms), - dtSeconds); + dt); } /** @@ -72,7 +72,7 @@ public LinearQuadraticRegulator( * @param B Continuous input matrix of the plant being controlled. * @param qelms The maximum desired error tolerance for each state. * @param relms The maximum desired control effort for each input. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. * @throws IllegalArgumentException If the system is unstabilizable. */ public LinearQuadraticRegulator( @@ -80,13 +80,8 @@ public LinearQuadraticRegulator( Matrix B, Vector qelms, Vector relms, - double dtSeconds) { - this( - A, - B, - StateSpaceUtil.makeCostMatrix(qelms), - StateSpaceUtil.makeCostMatrix(relms), - dtSeconds); + double dt) { + this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms), dt); } /** @@ -96,7 +91,7 @@ public LinearQuadraticRegulator( * @param B Continuous input matrix of the plant being controlled. * @param Q The state cost matrix. * @param R The input cost matrix. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. * @throws IllegalArgumentException If the system is unstabilizable. */ public LinearQuadraticRegulator( @@ -104,8 +99,8 @@ public LinearQuadraticRegulator( Matrix B, Matrix Q, Matrix R, - double dtSeconds) { - var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + double dt) { + var discABPair = Discretization.discretizeAB(A, B, dt); var discA = discABPair.getFirst(); var discB = discABPair.getSecond(); @@ -134,7 +129,7 @@ public LinearQuadraticRegulator( * @param Q The state cost matrix. * @param R The input cost matrix. * @param N The state-input cross-term cost matrix. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. * @throws IllegalArgumentException If the system is unstabilizable. */ public LinearQuadraticRegulator( @@ -143,8 +138,8 @@ public LinearQuadraticRegulator( Matrix Q, Matrix R, Matrix N, - double dtSeconds) { - var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + double dt) { + var discABPair = Discretization.discretizeAB(A, B, dt); var discA = discABPair.getFirst(); var discB = discABPair.getSecond(); @@ -253,15 +248,15 @@ public Matrix calculate(Matrix x, Matrix nex * appendix C.4 for a derivation. * * @param plant The plant being controlled. - * @param dtSeconds Discretization timestep in seconds. - * @param inputDelaySeconds Input time delay in seconds. + * @param dt Discretization timestep in seconds. + * @param inputDelay Input time delay in seconds. */ public void latencyCompensate( - LinearSystem plant, double dtSeconds, double inputDelaySeconds) { - var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dtSeconds); + LinearSystem plant, double dt, double inputDelay) { + var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt); var discA = discABPair.getFirst(); var discB = discABPair.getSecond(); - m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelaySeconds / dtSeconds)); + m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelay / dt)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index e9e6a3f22d9..d4b211602b2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -31,12 +31,12 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria * @param ks The static gain in volts. * @param kv The velocity gain in V/(units/s). * @param ka The acceleration gain in V/(units/s²). - * @param dtSeconds The period in seconds. + * @param dt The period in seconds. * @throws IllegalArgumentException for kv < zero. * @throws IllegalArgumentException for ka < zero. * @throws IllegalArgumentException for period ≤ zero. */ - public SimpleMotorFeedforward(double ks, double kv, double ka, double dtSeconds) { + public SimpleMotorFeedforward(double ks, double kv, double ka, double dt) { this.ks = ks; this.kv = kv; this.ka = ka; @@ -46,11 +46,10 @@ public SimpleMotorFeedforward(double ks, double kv, double ka, double dtSeconds) if (ka < 0.0) { throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); } - if (dtSeconds <= 0.0) { - throw new IllegalArgumentException( - "period must be a positive number, got " + dtSeconds + "!"); + if (dt <= 0.0) { + throw new IllegalArgumentException("period must be a positive number, got " + dt + "!"); } - m_dt = dtSeconds; + m_dt = dt; } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java index 7918f9fbb8e..0d0ccccee16 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java @@ -38,22 +38,22 @@ public class DifferentialDrivePoseEstimator extends PoseEstimator stateStdDevs, Matrix visionMeasurementStdDevs) { super( kinematics, - new DifferentialDriveOdometry( - gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters), + new DifferentialDriveOdometry(gyroAngle, leftDistance, rightDistance, initialPose), stateStdDevs, visionMeasurementStdDevs); } @@ -96,19 +95,14 @@ public DifferentialDrivePoseEstimator( * automatically takes care of offsetting the gyro angle. * * @param gyroAngle The angle reported by the gyroscope. - * @param leftPositionMeters The distance traveled by the left encoder. - * @param rightPositionMeters The distance traveled by the right encoder. - * @param poseMeters The position on the field that your robot is at. + * @param leftPosition The distance traveled by the left encoder in meters. + * @param rightPosition The distance traveled by the right encoder in meters. + * @param pose The position on the field that your robot is at. */ public void resetPosition( - Rotation2d gyroAngle, - double leftPositionMeters, - double rightPositionMeters, - Pose2d poseMeters) { + Rotation2d gyroAngle, double leftPosition, double rightPosition, Pose2d pose) { resetPosition( - gyroAngle, - new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters), - poseMeters); + gyroAngle, new DifferentialDriveWheelPositions(leftPosition, rightPosition), pose); } /** @@ -116,34 +110,27 @@ public void resetPosition( * loop. * * @param gyroAngle The current gyro angle. - * @param distanceLeftMeters The total distance travelled by the left wheel in meters. - * @param distanceRightMeters The total distance travelled by the right wheel in meters. + * @param distanceLeft The total distance travelled by the left wheel in meters. + * @param distanceRight The total distance travelled by the right wheel in meters. * @return The estimated pose of the robot in meters. */ - public Pose2d update( - Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) { - return update( - gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); + public Pose2d update(Rotation2d gyroAngle, double distanceLeft, double distanceRight) { + return update(gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight)); } /** * Updates the pose estimator with wheel encoder and gyro information. This should be called every * loop. * - * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param currentTime Time at which this method was called, in seconds. * @param gyroAngle The current gyro angle. - * @param distanceLeftMeters The total distance travelled by the left wheel in meters. - * @param distanceRightMeters The total distance travelled by the right wheel in meters. + * @param distanceLeft The total distance travelled by the left wheel in meters. + * @param distanceRight The total distance travelled by the right wheel in meters. * @return The estimated pose of the robot in meters. */ public Pose2d updateWithTime( - double currentTimeSeconds, - Rotation2d gyroAngle, - double distanceLeftMeters, - double distanceRightMeters) { + double currentTime, Rotation2d gyroAngle, double distanceLeft, double distanceRight) { return updateWithTime( - currentTimeSeconds, - gyroAngle, - new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); + currentTime, gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java index 470a05aa373..0c437296cbf 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java @@ -48,22 +48,22 @@ public class DifferentialDrivePoseEstimator3d * * @param kinematics A correctly-configured kinematics object for your drivetrain. * @param gyroAngle The current gyro angle. - * @param leftDistanceMeters The distance traveled by the left encoder. - * @param rightDistanceMeters The distance traveled by the right encoder. - * @param initialPoseMeters The starting pose estimate. + * @param leftDistance The distance traveled by the left encoder in meters. + * @param rightDistance The distance traveled by the right encoder in meters. + * @param initialPose The starting pose estimate. */ public DifferentialDrivePoseEstimator3d( DifferentialDriveKinematics kinematics, Rotation3d gyroAngle, - double leftDistanceMeters, - double rightDistanceMeters, - Pose3d initialPoseMeters) { + double leftDistance, + double rightDistance, + Pose3d initialPose) { this( kinematics, gyroAngle, - leftDistanceMeters, - rightDistanceMeters, - initialPoseMeters, + leftDistance, + rightDistance, + initialPose, VecBuilder.fill(0.02, 0.02, 0.02, 0.01), VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); } @@ -73,9 +73,9 @@ public DifferentialDrivePoseEstimator3d( * * @param kinematics A correctly-configured kinematics object for your drivetrain. * @param gyroAngle The gyro angle of the robot. - * @param leftDistanceMeters The distance traveled by the left encoder. - * @param rightDistanceMeters The distance traveled by the right encoder. - * @param initialPoseMeters The estimated initial pose. + * @param leftDistance The distance traveled by the left encoder in meters. + * @param rightDistance The distance traveled by the right encoder in meters. + * @param initialPose The estimated initial pose. * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position * in meters, and heading in radians). Increase these numbers to trust your state estimate * less. @@ -86,15 +86,14 @@ public DifferentialDrivePoseEstimator3d( public DifferentialDrivePoseEstimator3d( DifferentialDriveKinematics kinematics, Rotation3d gyroAngle, - double leftDistanceMeters, - double rightDistanceMeters, - Pose3d initialPoseMeters, + double leftDistance, + double rightDistance, + Pose3d initialPose, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { super( kinematics, - new DifferentialDriveOdometry3d( - gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters), + new DifferentialDriveOdometry3d(gyroAngle, leftDistance, rightDistance, initialPose), stateStdDevs, visionMeasurementStdDevs); } @@ -106,19 +105,14 @@ public DifferentialDrivePoseEstimator3d( * automatically takes care of offsetting the gyro angle. * * @param gyroAngle The angle reported by the gyroscope. - * @param leftPositionMeters The distance traveled by the left encoder. - * @param rightPositionMeters The distance traveled by the right encoder. - * @param poseMeters The position on the field that your robot is at. + * @param leftPosition The distance traveled by the left encoder in meters. + * @param rightPosition The distance traveled by the right encoder in meters. + * @param pose The position on the field that your robot is at. */ public void resetPosition( - Rotation3d gyroAngle, - double leftPositionMeters, - double rightPositionMeters, - Pose3d poseMeters) { + Rotation3d gyroAngle, double leftPosition, double rightPosition, Pose3d pose) { resetPosition( - gyroAngle, - new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters), - poseMeters); + gyroAngle, new DifferentialDriveWheelPositions(leftPosition, rightPosition), pose); } /** @@ -126,34 +120,27 @@ public void resetPosition( * loop. * * @param gyroAngle The current gyro angle. - * @param distanceLeftMeters The total distance travelled by the left wheel in meters. - * @param distanceRightMeters The total distance travelled by the right wheel in meters. + * @param distanceLeft The total distance travelled by the left wheel in meters. + * @param distanceRight The total distance travelled by the right wheel in meters. * @return The estimated pose of the robot in meters. */ - public Pose3d update( - Rotation3d gyroAngle, double distanceLeftMeters, double distanceRightMeters) { - return update( - gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); + public Pose3d update(Rotation3d gyroAngle, double distanceLeft, double distanceRight) { + return update(gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight)); } /** * Updates the pose estimator with wheel encoder and gyro information. This should be called every * loop. * - * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param currentTime Time at which this method was called, in seconds. * @param gyroAngle The current gyro angle. - * @param distanceLeftMeters The total distance travelled by the left wheel in meters. - * @param distanceRightMeters The total distance travelled by the right wheel in meters. + * @param distanceLeft The total distance travelled by the left wheel in meters. + * @param distanceRight The total distance travelled by the right wheel in meters. * @return The estimated pose of the robot in meters. */ public Pose3d updateWithTime( - double currentTimeSeconds, - Rotation3d gyroAngle, - double distanceLeftMeters, - double distanceRightMeters) { + double currentTime, Rotation3d gyroAngle, double distanceLeft, double distanceRight) { return updateWithTime( - currentTimeSeconds, - gyroAngle, - new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); + currentTime, gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java index 055bf9939af..78bc3d13358 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java @@ -58,7 +58,7 @@ public class ExtendedKalmanFilter m_P; - private double m_dtSeconds; + private double m_dt; /** * Constructs an extended Kalman filter. @@ -74,7 +74,7 @@ public class ExtendedKalmanFilter states, @@ -84,7 +84,7 @@ public ExtendedKalmanFilter( BiFunction, Matrix, Matrix> h, Matrix stateStdDevs, Matrix measurementStdDevs, - double dtSeconds) { + double dt) { this( states, inputs, @@ -95,7 +95,7 @@ public ExtendedKalmanFilter( measurementStdDevs, Matrix::minus, Matrix::plus, - dtSeconds); + dt); } /** @@ -115,7 +115,7 @@ public ExtendedKalmanFilter( * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it * subtracts them.) * @param addFuncX A function that adds two state vectors. - * @param dtSeconds Nominal discretization timestep. + * @param dt Nominal discretization timestep in seconds. */ public ExtendedKalmanFilter( Nat states, @@ -127,7 +127,7 @@ public ExtendedKalmanFilter( Matrix measurementStdDevs, BiFunction, Matrix, Matrix> residualFuncY, BiFunction, Matrix, Matrix> addFuncX, - double dtSeconds) { + double dt) { m_states = states; m_outputs = outputs; @@ -139,7 +139,7 @@ public ExtendedKalmanFilter( m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); - m_dtSeconds = dtSeconds; + m_dt = dt; reset(); @@ -150,11 +150,11 @@ public ExtendedKalmanFilter( NumericalJacobian.numericalJacobianX( outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1())); - final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds); + final var discPair = Discretization.discretizeAQ(contA, m_contQ, dt); final var discA = discPair.getFirst(); final var discQ = discPair.getSecond(); - final var discR = Discretization.discretizeR(m_contR, dtSeconds); + final var discR = Discretization.discretizeR(m_contR, dt); if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) { m_initP = DARE.dare(discA.transpose(), C.transpose(), discQ, discR); @@ -249,11 +249,11 @@ public final 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. + * @param dt Timestep for prediction in seconds. */ @Override - public void predict(Matrix u, double dtSeconds) { - predict(u, m_f, dtSeconds); + public void predict(Matrix u, double dt) { + predict(u, m_f, dt); } /** @@ -261,26 +261,26 @@ public void predict(Matrix u, double dtSeconds) { * * @param u New control input from controller. * @param f The function used to linearize the model. - * @param dtSeconds Timestep for prediction. + * @param dt Timestep for prediction in seconds. */ public void predict( Matrix u, BiFunction, Matrix, Matrix> f, - double dtSeconds) { + double dt) { // Find continuous A final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u); // Find discrete A and Q - final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds); + final var discPair = Discretization.discretizeAQ(contA, m_contQ, dt); final var discA = discPair.getFirst(); final var discQ = discPair.getSecond(); - m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dtSeconds); + m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dt); // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q m_P = discA.times(m_P).times(discA.transpose()).plus(discQ); - m_dtSeconds = dtSeconds; + m_dt = dt; } /** @@ -356,7 +356,7 @@ public void correct( BiFunction, Matrix, Matrix> residualFuncY, BiFunction, Matrix, Matrix> addFuncX) { final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u); - final var discR = Discretization.discretizeR(R, m_dtSeconds); + final var discR = Discretization.discretizeR(R, m_dt); final var S = C.times(m_P).times(C.transpose()).plus(discR); diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java index 40e6c96c7a7..8302cd554b2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java @@ -41,7 +41,7 @@ public class KalmanFilter m_P; private final Matrix m_contQ; private final Matrix m_contR; - private double m_dtSeconds; + private double m_dt; private final Matrix m_initP; @@ -57,7 +57,7 @@ public class KalmanFilter plant, Matrix stateStdDevs, Matrix measurementStdDevs, - double dtSeconds) { + double dt) { this.m_states = states; this.m_plant = plant; m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); - m_dtSeconds = dtSeconds; + m_dt = dt; // Find discrete A and Q - var pair = Discretization.discretizeAQ(plant.getA(), m_contQ, dtSeconds); + var pair = Discretization.discretizeAQ(plant.getA(), m_contQ, dt); var discA = pair.getFirst(); var discQ = pair.getSecond(); - var discR = Discretization.discretizeR(m_contR, dtSeconds); + var discR = Discretization.discretizeR(m_contR, dt); var C = plant.getC(); @@ -173,21 +173,21 @@ public final 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. + * @param dt Timestep for prediction in seconds. */ @Override - public void predict(Matrix u, double dtSeconds) { + public void predict(Matrix u, double dt) { // Find discrete A and Q - final var discPair = Discretization.discretizeAQ(m_plant.getA(), m_contQ, dtSeconds); + final var discPair = Discretization.discretizeAQ(m_plant.getA(), m_contQ, dt); final var discA = discPair.getFirst(); final var discQ = discPair.getSecond(); - m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds); + m_xHat = m_plant.calculateX(m_xHat, u, dt); // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q m_P = discA.times(m_P).times(discA.transpose()).plus(discQ); - m_dtSeconds = dtSeconds; + m_dt = dt; } /** @@ -214,7 +214,7 @@ public void correct(Matrix u, Matrix y, Matrix observer, - Matrix u, - Matrix localY, - double timestampSeconds) { - m_pastObserverSnapshots.add( - Map.entry(timestampSeconds, new ObserverSnapshot(observer, u, localY))); + KalmanTypeFilter observer, Matrix u, Matrix localY, double timestamp) { + m_pastObserverSnapshots.add(Map.entry(timestamp, new ObserverSnapshot(observer, u, localY))); if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) { m_pastObserverSnapshots.remove(0); @@ -62,27 +58,24 @@ public void addObserverState( * @param The rows in the global measurement vector. * @param rows The rows in the global measurement vector. * @param observer The observer to apply the past global measurement. - * @param nominalDtSeconds The nominal timestep. + * @param nominalDt The nominal timestep in seconds. * @param y The measurement. * @param globalMeasurementCorrect The function take calls correct() on the observer. - * @param timestampSeconds The timestamp of the measurement. + * @param timestamp The timestamp of the measurement in seconds. */ public void applyPastGlobalMeasurement( Nat rows, KalmanTypeFilter observer, - double nominalDtSeconds, + double nominalDt, Matrix y, BiConsumer, Matrix> globalMeasurementCorrect, - double timestampSeconds) { + double timestamp) { if (m_pastObserverSnapshots.isEmpty()) { // State map was empty, which means that we got a past measurement right at startup. The only // thing we can really do is ignore the measurement. return; } - // Use a less verbose name for timestamp - double timestamp = timestampSeconds; - int maxIdx = m_pastObserverSnapshots.size() - 1; int low = 0; int high = maxIdx; @@ -134,8 +127,7 @@ public void applyPastGlobalMeasurement( indexOfClosestEntry = prevTimeDiff <= nextTimeDiff ? prevIdx : nextIdx; } - double lastTimestamp = - m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDtSeconds; + double lastTimestamp = m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDt; // We will now go back in time to the state of the system at the time when // the measurement was captured. We will reset the observer to that state, diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java index ff9856c32df..e1dcf0dd2f8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java @@ -76,9 +76,9 @@ public interface KalmanTypeFilter u, double dtSeconds); + void predict(Matrix u, double dt); /** * Correct the state estimate x-hat using the measurements in y. diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index e33b30d09ca..cec0960f11b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -37,18 +37,18 @@ public class MecanumDrivePoseEstimator extends PoseEstimator stateStdDevs, Matrix visionMeasurementStdDevs) { super( kinematics, - new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters), + new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPose), stateStdDevs, visionMeasurementStdDevs); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java index e2f7acfdb0c..306d06aa7db 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java @@ -46,18 +46,18 @@ public class MecanumDrivePoseEstimator3d extends PoseEstimator3d stateStdDevs, Matrix visionMeasurementStdDevs) { super( kinematics, - new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters), + new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPose), stateStdDevs, visionMeasurementStdDevs); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 1f00cb7b187..2cfe982ce34 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -73,7 +73,7 @@ public PoseEstimator( Matrix visionMeasurementStdDevs) { m_odometry = odometry; - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); for (int i = 0; i < 3; ++i) { m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); @@ -116,14 +116,14 @@ public final void setVisionMeasurementStdDevs(Matrix visionMeasurementSt * * @param gyroAngle The angle reported by the gyroscope. * @param wheelPositions The current encoder readings. - * @param poseMeters The position on the field that your robot is at. + * @param pose The position on the field that your robot is at. */ - public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) { + public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d pose) { // Reset state estimate and error covariance - m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters); + m_odometry.resetPosition(gyroAngle, wheelPositions, pose); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); } /** @@ -135,7 +135,7 @@ public void resetPose(Pose2d pose) { m_odometry.resetPose(pose); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); } /** @@ -147,7 +147,7 @@ public void resetTranslation(Translation2d translation) { m_odometry.resetTranslation(translation); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); } /** @@ -159,7 +159,7 @@ public void resetRotation(Rotation2d rotation) { m_odometry.resetRotation(rotation); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); } /** @@ -174,10 +174,10 @@ public Pose2d getEstimatedPosition() { /** * Return the pose at a given timestamp, if the buffer is not empty. * - * @param timestampSeconds The pose's timestamp in seconds. + * @param timestamp The pose's timestamp in seconds. * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). */ - public Optional sampleAt(double timestampSeconds) { + public Optional sampleAt(double timestamp) { // Step 0: If there are no odometry updates to sample, skip. if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) { return Optional.empty(); @@ -187,20 +187,19 @@ public Optional sampleAt(double timestampSeconds) { // the buffer will always use a timestamp between the first and last timestamps) double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey(); double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey(); - timestampSeconds = - MathUtil.clamp(timestampSeconds, oldestOdometryTimestamp, newestOdometryTimestamp); + timestamp = MathUtil.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp); // Step 2: If there are no applicable vision updates, use the odometry-only information. - if (m_visionUpdates.isEmpty() || timestampSeconds < m_visionUpdates.firstKey()) { - return m_odometryPoseBuffer.getSample(timestampSeconds); + if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) { + return m_odometryPoseBuffer.getSample(timestamp); } // Step 3: Get the latest vision update from before or at the timestamp to sample at. - double floorTimestamp = m_visionUpdates.floorKey(timestampSeconds); + double floorTimestamp = m_visionUpdates.floorKey(timestamp); var visionUpdate = m_visionUpdates.get(floorTimestamp); // Step 4: Get the pose measured by odometry at the time of the sample. - var odometryEstimate = m_odometryPoseBuffer.getSample(timestampSeconds); + var odometryEstimate = m_odometryPoseBuffer.getSample(timestamp); // Step 5: Apply the vision compensation to the odometry pose. return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose)); @@ -239,19 +238,18 @@ private void cleanUpVisionUpdates() { * recommend only adding vision measurements that are already within one meter or so of the * current pose estimate. * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you - * don't use your own time source by calling {@link + * @param visionRobotPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use + * your own time source by calling {@link * PoseEstimator#updateWithTime(double,Rotation2d,Object)} then you must use a timestamp with * an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link * edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use {@link * edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the epochs. */ - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + public void addVisionMeasurement(Pose2d visionRobotPose, double timestamp) { // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. if (m_odometryPoseBuffer.getInternalBuffer().isEmpty() - || m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration - > timestampSeconds) { + || m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestamp) { return; } @@ -259,7 +257,7 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS cleanUpVisionUpdates(); // Step 2: Get the pose measured by odometry at the moment the vision measurement was made. - var odometrySample = m_odometryPoseBuffer.getSample(timestampSeconds); + var odometrySample = m_odometryPoseBuffer.getSample(timestamp); if (odometrySample.isEmpty()) { return; @@ -267,14 +265,14 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS // Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was // made. - var visionSample = sampleAt(timestampSeconds); + var visionSample = sampleAt(timestamp); if (visionSample.isEmpty()) { return; } // Step 4: Measure the twist between the old pose estimate and the vision pose. - var twist = visionSample.get().log(visionRobotPoseMeters); + var twist = visionSample.get().log(visionRobotPose); // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman // gain matrix representing how much we trust vision measurements compared to our current pose. @@ -286,14 +284,14 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS // Step 7: Calculate and record the vision update. var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); - m_visionUpdates.put(timestampSeconds, visionUpdate); + m_visionUpdates.put(timestamp, visionUpdate); // Step 8: Remove later vision measurements. (Matches previous behavior) - m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear(); + m_visionUpdates.tailMap(timestamp, false).entrySet().clear(); // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update, // it's guaranteed to be the latest vision update. - m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters()); + m_poseEstimate = visionUpdate.compensate(m_odometry.getPose()); } /** @@ -311,23 +309,20 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS * to apply to future measurements until a subsequent call to {@link * PoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method. * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you - * don't use your own time source by calling {@link #updateWithTime}, then you must use a - * timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same - * epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you - * should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in - * this case. + * @param visionRobotPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use + * your own time source by calling {@link #updateWithTime}, then you must use a timestamp with + * an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link + * edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you should use {@link + * edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in this case. * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position * in meters, y position in meters, and heading in radians). Increase these numbers to trust * the vision pose measurement less. */ public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { + Pose2d visionRobotPose, double timestamp, Matrix visionMeasurementStdDevs) { setVisionMeasurementStdDevs(visionMeasurementStdDevs); - addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); + addVisionMeasurement(visionRobotPose, timestamp); } /** @@ -346,15 +341,15 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { * Updates the pose estimator with wheel encoder and gyro information. This should be called every * loop. * - * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param currentTime Time at which this method was called, in seconds. * @param gyroAngle The current gyro angle. * @param wheelPositions The current encoder readings. * @return The estimated pose of the robot in meters. */ - public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) { + public Pose2d updateWithTime(double currentTime, Rotation2d gyroAngle, T wheelPositions) { var odometryEstimate = m_odometry.update(gyroAngle, wheelPositions); - m_odometryPoseBuffer.addSample(currentTimeSeconds, odometryEstimate); + m_odometryPoseBuffer.addSample(currentTime, odometryEstimate); if (m_visionUpdates.isEmpty()) { m_poseEstimate = odometryEstimate; diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java index 9ce96f0243b..31c97549ff5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -81,7 +81,7 @@ public PoseEstimator3d( Matrix visionMeasurementStdDevs) { m_odometry = odometry; - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); for (int i = 0; i < 4; ++i) { m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); @@ -128,14 +128,14 @@ public final void setVisionMeasurementStdDevs(Matrix visionMeasurementSt * * @param gyroAngle The angle reported by the gyroscope. * @param wheelPositions The current encoder readings. - * @param poseMeters The position on the field that your robot is at. + * @param pose The position on the field that your robot is at. */ - public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) { + public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) { // Reset state estimate and error covariance - m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters); + m_odometry.resetPosition(gyroAngle, wheelPositions, pose); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); } /** @@ -147,7 +147,7 @@ public void resetPose(Pose3d pose) { m_odometry.resetPose(pose); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); } /** @@ -159,7 +159,7 @@ public void resetTranslation(Translation3d translation) { m_odometry.resetTranslation(translation); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); } /** @@ -171,7 +171,7 @@ public void resetRotation(Rotation3d rotation) { m_odometry.resetRotation(rotation); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPoseMeters(); + m_poseEstimate = m_odometry.getPose(); } /** @@ -186,10 +186,10 @@ public Pose3d getEstimatedPosition() { /** * Return the pose at a given timestamp, if the buffer is not empty. * - * @param timestampSeconds The pose's timestamp in seconds. + * @param timestamp The pose's timestamp in seconds. * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). */ - public Optional sampleAt(double timestampSeconds) { + public Optional sampleAt(double timestamp) { // Step 0: If there are no odometry updates to sample, skip. if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) { return Optional.empty(); @@ -199,20 +199,19 @@ public Optional sampleAt(double timestampSeconds) { // the buffer will always use a timestamp between the first and last timestamps) double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey(); double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey(); - timestampSeconds = - MathUtil.clamp(timestampSeconds, oldestOdometryTimestamp, newestOdometryTimestamp); + timestamp = MathUtil.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp); // Step 2: If there are no applicable vision updates, use the odometry-only information. - if (m_visionUpdates.isEmpty() || timestampSeconds < m_visionUpdates.firstKey()) { - return m_odometryPoseBuffer.getSample(timestampSeconds); + if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) { + return m_odometryPoseBuffer.getSample(timestamp); } // Step 3: Get the latest vision update from before or at the timestamp to sample at. - double floorTimestamp = m_visionUpdates.floorKey(timestampSeconds); + double floorTimestamp = m_visionUpdates.floorKey(timestamp); var visionUpdate = m_visionUpdates.get(floorTimestamp); // Step 4: Get the pose measured by odometry at the time of the sample. - var odometryEstimate = m_odometryPoseBuffer.getSample(timestampSeconds); + var odometryEstimate = m_odometryPoseBuffer.getSample(timestamp); // Step 5: Apply the vision compensation to the odometry pose. return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose)); @@ -251,20 +250,19 @@ private void cleanUpVisionUpdates() { * recommend only adding vision measurements that are already within one meter or so of the * current pose estimate. * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you - * don't use your own time source by calling {@link + * @param visionRobotPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use + * your own time source by calling {@link * PoseEstimator3d#updateWithTime(double,Rotation3d,Object)} then you must use a timestamp * with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the * epochs. */ - public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampSeconds) { + public void addVisionMeasurement(Pose3d visionRobotPose, double timestamp) { // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. if (m_odometryPoseBuffer.getInternalBuffer().isEmpty() - || m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration - > timestampSeconds) { + || m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestamp) { return; } @@ -272,7 +270,7 @@ public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampS cleanUpVisionUpdates(); // Step 2: Get the pose measured by odometry at the moment the vision measurement was made. - var odometrySample = m_odometryPoseBuffer.getSample(timestampSeconds); + var odometrySample = m_odometryPoseBuffer.getSample(timestamp); if (odometrySample.isEmpty()) { return; @@ -280,14 +278,14 @@ public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampS // Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was // made. - var visionSample = sampleAt(timestampSeconds); + var visionSample = sampleAt(timestamp); if (visionSample.isEmpty()) { return; } // Step 4: Measure the twist between the old pose estimate and the vision pose. - var twist = visionSample.get().log(visionRobotPoseMeters); + var twist = visionSample.get().log(visionRobotPose); // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman // gain matrix representing how much we trust vision measurements compared to our current pose. @@ -307,14 +305,14 @@ public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampS // Step 7: Calculate and record the vision update. var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); - m_visionUpdates.put(timestampSeconds, visionUpdate); + m_visionUpdates.put(timestamp, visionUpdate); // Step 8: Remove later vision measurements. (Matches previous behavior) - m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear(); + m_visionUpdates.tailMap(timestamp, false).entrySet().clear(); // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update, // it's guaranteed to be the latest vision update. - m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters()); + m_poseEstimate = visionUpdate.compensate(m_odometry.getPose()); } /** @@ -332,23 +330,20 @@ public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampS * to apply to future measurements until a subsequent call to {@link * PoseEstimator3d#setVisionMeasurementStdDevs(Matrix)} or this method. * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you - * don't use your own time source by calling {@link #updateWithTime}, then you must use a - * timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same - * epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you - * should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in - * this case. + * @param visionRobotPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use + * your own time source by calling {@link #updateWithTime}, then you must use a timestamp with + * an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link + * edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you should use {@link + * edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in this case. * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position * in meters, y position in meters, z position in meters, and angle in radians). Increase * these numbers to trust the vision pose measurement less. */ public void addVisionMeasurement( - Pose3d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { + Pose3d visionRobotPose, double timestamp, Matrix visionMeasurementStdDevs) { setVisionMeasurementStdDevs(visionMeasurementStdDevs); - addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); + addVisionMeasurement(visionRobotPose, timestamp); } /** @@ -367,15 +362,15 @@ public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { * Updates the pose estimator with wheel encoder and gyro information. This should be called every * loop. * - * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param currentTime Time at which this method was called, in seconds. * @param gyroAngle The current gyro angle. * @param wheelPositions The current encoder readings. * @return The estimated pose of the robot in meters. */ - public Pose3d updateWithTime(double currentTimeSeconds, Rotation3d gyroAngle, T wheelPositions) { + public Pose3d updateWithTime(double currentTime, Rotation3d gyroAngle, T wheelPositions) { var odometryEstimate = m_odometry.update(gyroAngle, wheelPositions); - m_odometryPoseBuffer.addSample(currentTimeSeconds, odometryEstimate); + m_odometryPoseBuffer.addSample(currentTime, odometryEstimate); if (m_visionUpdates.isEmpty()) { m_poseEstimate = odometryEstimate; diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java index 31649728e27..1b85e4385e2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java @@ -58,7 +58,7 @@ public class SteadyStateKalmanFilter plant, Matrix stateStdDevs, Matrix measurementStdDevs, - double dtSeconds) { + double dt) { this.m_states = states; this.m_plant = plant; @@ -75,11 +75,11 @@ public SteadyStateKalmanFilter( var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); - var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds); + var pair = Discretization.discretizeAQ(plant.getA(), contQ, dt); var discA = pair.getFirst(); var discQ = pair.getSecond(); - var discR = Discretization.discretizeR(contR, dtSeconds); + var discR = Discretization.discretizeR(contR, dt); var C = plant.getC(); @@ -174,10 +174,10 @@ public double getXhat(int row) { * Project the model into the future with a new control input u. * * @param u New control input from controller. - * @param dtSeconds Timestep for prediction. + * @param dt Timestep for prediction in seconds. */ - public void predict(Matrix u, double dtSeconds) { - this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds); + public void predict(Matrix u, double dt) { + this.m_xHat = m_plant.calculateX(m_xHat, u, dt); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index 495cd893fc6..6c51f346d2a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -38,18 +38,18 @@ public class SwerveDrivePoseEstimator extends PoseEstimator stateStdDevs, Matrix visionMeasurementStdDevs) { super( kinematics, - new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters), + new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPose), stateStdDevs, visionMeasurementStdDevs); @@ -86,13 +86,13 @@ public SwerveDrivePoseEstimator( @Override public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions) { + double currentTime, Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions) { if (wheelPositions.length != m_numModules) { throw new IllegalArgumentException( "Number of modules is not consistent with number of wheel locations provided in " + "constructor"); } - return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions); + return super.updateWithTime(currentTime, gyroAngle, wheelPositions); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java index 668f45c410e..8016a092cd8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java @@ -47,18 +47,18 @@ public class SwerveDrivePoseEstimator3d extends PoseEstimator3d stateStdDevs, Matrix visionMeasurementStdDevs) { super( kinematics, - new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPoseMeters), + new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPose), stateStdDevs, visionMeasurementStdDevs); @@ -95,13 +95,13 @@ public SwerveDrivePoseEstimator3d( @Override public Pose3d updateWithTime( - double currentTimeSeconds, Rotation3d gyroAngle, SwerveModulePosition[] wheelPositions) { + double currentTime, Rotation3d gyroAngle, SwerveModulePosition[] wheelPositions) { if (wheelPositions.length != m_numModules) { throw new IllegalArgumentException( "Number of modules is not consistent with number of wheel locations provided in " + "constructor"); } - return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions); + return super.updateWithTime(currentTime, gyroAngle, wheelPositions); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java index c64ec85cf9b..03df366089e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java @@ -62,7 +62,7 @@ public class UnscentedKalmanFilter m_contQ; private final Matrix m_contR; private Matrix m_sigmasF; - private double m_dtSeconds; + private double m_dt; private final MerweScaledSigmaPoints m_pts; @@ -79,7 +79,7 @@ public class UnscentedKalmanFilter states, @@ -88,7 +88,7 @@ public UnscentedKalmanFilter( BiFunction, Matrix, Matrix> h, Matrix stateStdDevs, Matrix measurementStdDevs, - double nominalDtSeconds) { + double nominalDt) { this( states, outputs, @@ -101,7 +101,7 @@ public UnscentedKalmanFilter( Matrix::minus, Matrix::minus, Matrix::plus, - nominalDtSeconds); + nominalDt); } /** @@ -128,7 +128,7 @@ public UnscentedKalmanFilter( * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it * subtracts them.) * @param addFuncX A function that adds two state vectors. - * @param nominalDtSeconds Nominal discretization timestep. + * @param nominalDt Nominal discretization timestep in seconds. */ public UnscentedKalmanFilter( Nat states, @@ -142,7 +142,7 @@ public UnscentedKalmanFilter( BiFunction, Matrix, Matrix> residualFuncX, BiFunction, Matrix, Matrix> residualFuncY, BiFunction, Matrix, Matrix> addFuncX, - double nominalDtSeconds) { + double nominalDt) { this.m_states = states; this.m_outputs = outputs; @@ -155,7 +155,7 @@ public UnscentedKalmanFilter( m_residualFuncY = residualFuncY; m_addFuncX = addFuncX; - m_dtSeconds = nominalDtSeconds; + m_dt = nominalDt; m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); @@ -337,14 +337,14 @@ public final 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. + * @param dt Timestep for prediction in seconds. */ @Override - public void predict(Matrix u, double dtSeconds) { + public void predict(Matrix u, double dt) { // Discretize Q before projecting mean and covariance forward Matrix contA = NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u); - var discQ = Discretization.discretizeAQ(contA, m_contQ, dtSeconds).getSecond(); + var discQ = Discretization.discretizeAQ(contA, m_contQ, dt).getSecond(); var squareRootDiscQ = discQ.lltDecompose(true); var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S); @@ -352,7 +352,7 @@ public void predict(Matrix u, double dtSeconds) { for (int i = 0; i < m_pts.getNumSigmas(); ++i) { Matrix x = sigmas.extractColumnVector(i); - m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dtSeconds)); + m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dt)); } var ret = @@ -368,7 +368,7 @@ public void predict(Matrix u, double dtSeconds) { m_xHat = ret.getFirst(); m_S = ret.getSecond(); - m_dtSeconds = dtSeconds; + m_dt = dt; } /** @@ -456,7 +456,7 @@ public void correct( BiFunction, Matrix, Matrix> residualFuncY, BiFunction, Matrix, Matrix> residualFuncX, BiFunction, Matrix, Matrix> addFuncX) { - final var discR = Discretization.discretizeR(R, m_dtSeconds); + final var discR = Discretization.discretizeR(R, m_dt); final var squareRootDiscR = discR.lltDecompose(true); // Transform sigma points into measurement space diff --git a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java index 9a281e83f75..5aa38dafe46 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java +++ b/wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java @@ -21,11 +21,11 @@ public enum DebounceType { kBoth } - private final double m_debounceTimeSeconds; + private final double m_debounceTime; private final DebounceType m_debounceType; private boolean m_baseline; - private double m_prevTimeSeconds; + private double m_prevTime; /** * Creates a new Debouncer. @@ -35,7 +35,7 @@ public enum DebounceType { * @param type Which type of state change the debouncing will be performed on. */ public Debouncer(double debounceTime, DebounceType type) { - m_debounceTimeSeconds = debounceTime; + m_debounceTime = debounceTime; m_debounceType = type; resetTimer(); @@ -58,11 +58,11 @@ public Debouncer(double debounceTime) { } private void resetTimer() { - m_prevTimeSeconds = MathSharedStore.getTimestamp(); + m_prevTime = MathSharedStore.getTimestamp(); } private boolean hasElapsed() { - return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds; + return MathSharedStore.getTimestamp() - m_prevTime >= m_debounceTime; } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java index f9f20c37325..3882583816c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java +++ b/wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java @@ -22,8 +22,14 @@ public final class TimeInterpolatableBuffer { private final Interpolator m_interpolatingFunc; private final NavigableMap m_pastSnapshots = new TreeMap<>(); - private TimeInterpolatableBuffer(Interpolator interpolateFunction, double historySizeSeconds) { - this.m_historySize = historySizeSeconds; + /** + * Constructs a TimeInterpolatableBuffer. + * + * @param interpolateFunction Interpolation function. + * @param historySize The history size of the buffer in seconds. + */ + private TimeInterpolatableBuffer(Interpolator interpolateFunction, double historySize) { + this.m_historySize = historySize; this.m_interpolatingFunc = interpolateFunction; } @@ -31,52 +37,52 @@ private TimeInterpolatableBuffer(Interpolator interpolateFunction, double his * Create a new TimeInterpolatableBuffer. * * @param interpolateFunction The function used to interpolate between values. - * @param historySizeSeconds The history size of the buffer. + * @param historySize The history size of the buffer in seconds. * @param The type of data to store in the buffer. * @return The new TimeInterpolatableBuffer. */ public static TimeInterpolatableBuffer createBuffer( - Interpolator interpolateFunction, double historySizeSeconds) { - return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds); + Interpolator interpolateFunction, double historySize) { + return new TimeInterpolatableBuffer<>(interpolateFunction, historySize); } /** * Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}. * - * @param historySizeSeconds The history size of the buffer. + * @param historySize The history size of the buffer in seconds. * @param The type of {@link Interpolatable} to store in the buffer. * @return The new TimeInterpolatableBuffer. */ public static > TimeInterpolatableBuffer createBuffer( - double historySizeSeconds) { - return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySizeSeconds); + double historySize) { + return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySize); } /** * Create a new TimeInterpolatableBuffer to store Double values. * - * @param historySizeSeconds The history size of the buffer. + * @param historySize The history size of the buffer in seconds. * @return The new TimeInterpolatableBuffer. */ - public static TimeInterpolatableBuffer createDoubleBuffer(double historySizeSeconds) { - return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySizeSeconds); + public static TimeInterpolatableBuffer createDoubleBuffer(double historySize) { + return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySize); } /** * Add a sample to the buffer. * - * @param timeSeconds The timestamp of the sample. + * @param time The timestamp of the sample in seconds. * @param sample The sample object. */ - public void addSample(double timeSeconds, T sample) { - cleanUp(timeSeconds); - m_pastSnapshots.put(timeSeconds, sample); + public void addSample(double time, T sample) { + cleanUp(time); + m_pastSnapshots.put(time, sample); } /** * Removes samples older than our current history size. * - * @param time The current timestamp. + * @param time The current timestamp in seconds. */ private void cleanUp(double time) { while (!m_pastSnapshots.isEmpty()) { @@ -97,22 +103,22 @@ public void clear() { /** * Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned. * - * @param timeSeconds The time at which to sample. + * @param time The time at which to sample in seconds. * @return The interpolated value at that timestamp or an empty Optional. */ - public Optional getSample(double timeSeconds) { + public Optional getSample(double time) { if (m_pastSnapshots.isEmpty()) { return Optional.empty(); } // Special case for when the requested time is the same as a sample - var nowEntry = m_pastSnapshots.get(timeSeconds); + var nowEntry = m_pastSnapshots.get(time); if (nowEntry != null) { return Optional.of(nowEntry); } - var topBound = m_pastSnapshots.ceilingEntry(timeSeconds); - var bottomBound = m_pastSnapshots.floorEntry(timeSeconds); + var topBound = m_pastSnapshots.ceilingEntry(time); + var bottomBound = m_pastSnapshots.floorEntry(time); // Return null if neither sample exists, and the opposite bound if the other is null if (topBound == null && bottomBound == null) { @@ -129,7 +135,7 @@ public Optional getSample(double timeSeconds) { m_interpolatingFunc.interpolate( bottomBound.getValue(), topBound.getValue(), - (timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey()))); + (time - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey()))); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index de6c5fa767b..00fd90c3a19 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -29,14 +29,14 @@ * will often have all three components. */ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable { - /** Velocity along the x-axis. (Fwd is +) */ - public double vxMetersPerSecond; + /** Velocity along the x-axis in meters per second. (Fwd is +) */ + public double vx; - /** Velocity along the y-axis. (Left is +) */ - public double vyMetersPerSecond; + /** Velocity along the y-axis in meters per second. (Left is +) */ + public double vy; - /** Represents the angular velocity of the robot frame. (CCW is +) */ - public double omegaRadiansPerSecond; + /** Angular velocity of the robot frame in radians per second. (CCW is +) */ + public double omega; /** ChassisSpeeds protobuf for serialization. */ public static final ChassisSpeedsProto proto = new ChassisSpeedsProto(); @@ -50,15 +50,14 @@ public ChassisSpeeds() {} /** * Constructs a ChassisSpeeds object. * - * @param vxMetersPerSecond Forward velocity. - * @param vyMetersPerSecond Sideways velocity. - * @param omegaRadiansPerSecond Angular velocity. + * @param vx Forward velocity in meters per second. + * @param vy Sideways velocity in meters per second. + * @param omega Angular velocity in radians per second. */ - public ChassisSpeeds( - double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) { - this.vxMetersPerSecond = vxMetersPerSecond; - this.vyMetersPerSecond = vyMetersPerSecond; - this.omegaRadiansPerSecond = omegaRadiansPerSecond; + public ChassisSpeeds(double vx, double vy, double omega) { + this.vx = vx; + this.vy = vy; + this.omega = omega; } /** @@ -75,14 +74,11 @@ public ChassisSpeeds(LinearVelocity vx, LinearVelocity vy, AngularVelocity omega /** * Creates a Twist2d from ChassisSpeeds. * - * @param dtSeconds The duration of the timestep. + * @param dt The duration of the timestep in seconds. * @return Twist2d. */ - public Twist2d toTwist2d(double dtSeconds) { - return new Twist2d( - vxMetersPerSecond * dtSeconds, - vyMetersPerSecond * dtSeconds, - omegaRadiansPerSecond * dtSeconds); + public Twist2d toTwist2d(double dt) { + return new Twist2d(vx * dt, vy * dt, omega * dt); } /** @@ -96,22 +92,20 @@ public Twist2d toTwist2d(double dtSeconds) { *

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. + * @param dt The duration of the timestep in seconds the speeds should be applied for. * @return Discretized ChassisSpeeds. */ - public ChassisSpeeds discretize(double dtSeconds) { - var desiredDeltaPose = - new Pose2d( - vxMetersPerSecond * dtSeconds, - vyMetersPerSecond * dtSeconds, - new Rotation2d(omegaRadiansPerSecond * dtSeconds)); + public ChassisSpeeds discretize(double dt) { + // Construct the desired pose after a timestep, relative to the current pose. The desired pose + // has decoupled translation and rotation. + var desiredDeltaPose = new Pose2d(vx * dt, vy * dt, new Rotation2d(omega * dt)); // 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 - return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds); + return new ChassisSpeeds(twist.dx / dt, twist.dy / dt, twist.dtheta / dt); } /** @@ -124,9 +118,8 @@ public ChassisSpeeds discretize(double dtSeconds) { */ public ChassisSpeeds toRobotRelative(Rotation2d robotAngle) { // CW rotation into chassis frame - var rotated = - new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus()); - return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond); + var rotated = new Translation2d(vx, vy).rotateBy(robotAngle.unaryMinus()); + return new ChassisSpeeds(rotated.getX(), rotated.getY(), omega); } /** @@ -139,8 +132,8 @@ public ChassisSpeeds toRobotRelative(Rotation2d robotAngle) { */ public ChassisSpeeds toFieldRelative(Rotation2d robotAngle) { // CCW rotation out of chassis frame - var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle); - return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond); + var rotated = new Translation2d(vx, vy).rotateBy(robotAngle); + return new ChassisSpeeds(rotated.getX(), rotated.getY(), omega); } /** @@ -153,10 +146,7 @@ public ChassisSpeeds toFieldRelative(Rotation2d robotAngle) { * @return The sum of the ChassisSpeeds. */ public ChassisSpeeds plus(ChassisSpeeds other) { - return new ChassisSpeeds( - vxMetersPerSecond + other.vxMetersPerSecond, - vyMetersPerSecond + other.vyMetersPerSecond, - omegaRadiansPerSecond + other.omegaRadiansPerSecond); + return new ChassisSpeeds(vx + other.vx, vy + other.vy, omega + other.omega); } /** @@ -169,10 +159,7 @@ public ChassisSpeeds plus(ChassisSpeeds other) { * @return The difference between the two ChassisSpeeds. */ public ChassisSpeeds minus(ChassisSpeeds other) { - return new ChassisSpeeds( - vxMetersPerSecond - other.vxMetersPerSecond, - vyMetersPerSecond - other.vyMetersPerSecond, - omegaRadiansPerSecond - other.omegaRadiansPerSecond); + return new ChassisSpeeds(vx - other.vx, vy - other.vy, omega - other.omega); } /** @@ -182,7 +169,7 @@ public ChassisSpeeds minus(ChassisSpeeds other) { * @return The inverse of the current ChassisSpeeds. */ public ChassisSpeeds unaryMinus() { - return new ChassisSpeeds(-vxMetersPerSecond, -vyMetersPerSecond, -omegaRadiansPerSecond); + return new ChassisSpeeds(-vx, -vy, -omega); } /** @@ -194,8 +181,7 @@ public ChassisSpeeds unaryMinus() { * @return The scaled ChassisSpeeds. */ public ChassisSpeeds times(double scalar) { - return new ChassisSpeeds( - vxMetersPerSecond * scalar, vyMetersPerSecond * scalar, omegaRadiansPerSecond * scalar); + return new ChassisSpeeds(vx * scalar, vy * scalar, omega * scalar); } /** @@ -207,28 +193,23 @@ public ChassisSpeeds times(double scalar) { * @return The scaled ChassisSpeeds. */ public ChassisSpeeds div(double scalar) { - return new ChassisSpeeds( - vxMetersPerSecond / scalar, vyMetersPerSecond / scalar, omegaRadiansPerSecond / scalar); + return new ChassisSpeeds(vx / scalar, vy / scalar, omega / scalar); } @Override public final int hashCode() { - return Objects.hash(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); + return Objects.hash(vx, vy, omega); } @Override public boolean equals(Object o) { return o == this - || o instanceof ChassisSpeeds c - && vxMetersPerSecond == c.vxMetersPerSecond - && vyMetersPerSecond == c.vyMetersPerSecond - && omegaRadiansPerSecond == c.omegaRadiansPerSecond; + || o instanceof ChassisSpeeds c && vx == c.vx && vy == c.vy && omega == c.omega; } @Override public String toString() { return String.format( - "ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)", - vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); + "ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)", vx, vy, omega); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java index ddb66ab4c4a..eced997b242 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java @@ -26,8 +26,8 @@ public class DifferentialDriveKinematics implements Kinematics, ProtobufSerializable, StructSerializable { - /** Differential drive trackwidth. */ - public final double trackWidthMeters; + /** Differential drive trackwidth in meters. */ + public final double trackwidth; /** DifferentialDriveKinematics protobuf for serialization. */ public static final DifferentialDriveKinematicsProto proto = @@ -40,24 +40,24 @@ public class DifferentialDriveKinematics /** * Constructs a differential drive kinematics object. * - * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is the distance - * between the left wheels and right wheels. However, the empirical value may be larger than - * the physical measured value due to scrubbing effects. + * @param trackwidth The trackwidth of the drivetrain in meters. Theoretically, this is the + * distance between the left wheels and right wheels. However, the empirical value may be + * larger than the physical measured value due to scrubbing effects. */ - public DifferentialDriveKinematics(double trackWidthMeters) { - this.trackWidthMeters = trackWidthMeters; + public DifferentialDriveKinematics(double trackwidth) { + this.trackwidth = trackwidth; MathSharedStore.reportUsage("DifferentialDriveKinematics", ""); } /** * Constructs a differential drive kinematics object. * - * @param trackWidth The track width of the drivetrain. Theoretically, this is the distance - * between the left wheels and right wheels. However, the empirical value may be larger than - * the physical measured value due to scrubbing effects. + * @param trackwidth The trackwidth of the drivetrain in meters. Theoretically, this is the + * distance between the left wheels and right wheels. However, the empirical value may be + * larger than the physical measured value due to scrubbing effects. */ - public DifferentialDriveKinematics(Distance trackWidth) { - this(trackWidth.in(Meters)); + public DifferentialDriveKinematics(Distance trackwidth) { + this(trackwidth.in(Meters)); } /** @@ -69,9 +69,9 @@ public DifferentialDriveKinematics(Distance trackWidth) { @Override public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) { return new ChassisSpeeds( - (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, + (wheelSpeeds.left + wheelSpeeds.right) / 2, 0, - (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters); + (wheelSpeeds.right - wheelSpeeds.left) / trackwidth); } /** @@ -84,16 +84,14 @@ public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) { @Override public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { return new DifferentialDriveWheelSpeeds( - chassisSpeeds.vxMetersPerSecond - - trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond, - chassisSpeeds.vxMetersPerSecond - + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond); + chassisSpeeds.vx - trackwidth / 2 * chassisSpeeds.omega, + chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega); } @Override public Twist2d toTwist2d( DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) { - return toTwist2d(end.leftMeters - start.leftMeters, end.rightMeters - start.rightMeters); + return toTwist2d(end.left - start.left, end.right - start.right); } /** @@ -101,27 +99,25 @@ public Twist2d toTwist2d( * distance deltas. This method is often used for odometry -- determining the robot's position on * the field using changes in the distance driven by each wheel on the robot. * - * @param leftDistanceMeters The distance measured by the left side encoder. - * @param rightDistanceMeters The distance measured by the right side encoder. + * @param leftDistance The distance measured by the left side encoder in meters. + * @param rightDistance The distance measured by the right side encoder in meters. * @return The resulting Twist2d. */ - public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) { + public Twist2d toTwist2d(double leftDistance, double rightDistance) { return new Twist2d( - (leftDistanceMeters + rightDistanceMeters) / 2, - 0, - (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters); + (leftDistance + rightDistance) / 2, 0, (rightDistance - leftDistance) / trackwidth); } @Override public DifferentialDriveWheelPositions copy(DifferentialDriveWheelPositions positions) { - return new DifferentialDriveWheelPositions(positions.leftMeters, positions.rightMeters); + return new DifferentialDriveWheelPositions(positions.left, positions.right); } @Override public void copyInto( DifferentialDriveWheelPositions positions, DifferentialDriveWheelPositions output) { - output.leftMeters = positions.leftMeters; - output.rightMeters = positions.rightMeters; + output.left = positions.left; + output.right = positions.right; } @Override diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java index a7c013d19fb..14b77b1eedf 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java @@ -26,20 +26,17 @@ public class DifferentialDriveOdometry extends Odometry { - /** Distance measured by the left side. */ - public double leftMeters; + /** Distance measured by the left side in meters. */ + public double left; - /** Distance measured by the right side. */ - public double rightMeters; + /** Distance measured by the right side in meters. */ + public double right; /** DifferentialDriveWheelPositions struct for serialization. */ public static final DifferentialDriveWheelPositionsStruct struct = @@ -37,19 +37,19 @@ public class DifferentialDriveWheelPositions /** * Constructs a DifferentialDriveWheelPositions. * - * @param leftMeters Distance measured by the left side. - * @param rightMeters Distance measured by the right side. + * @param left Distance measured by the left side in meters. + * @param right Distance measured by the right side in meters. */ - public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) { - this.leftMeters = leftMeters; - this.rightMeters = rightMeters; + public DifferentialDriveWheelPositions(double left, double right) { + this.left = left; + this.right = right; } /** * Constructs a DifferentialDriveWheelPositions. * - * @param left Distance measured by the left side. - * @param right Distance measured by the right side. + * @param left Distance measured by the left side in meters. + * @param right Distance measured by the right side in meters. */ public DifferentialDriveWheelPositions(Distance left, Distance right) { this(left.in(Meters), right.in(Meters)); @@ -58,26 +58,26 @@ public DifferentialDriveWheelPositions(Distance left, Distance right) { @Override public boolean equals(Object obj) { return obj instanceof DifferentialDriveWheelPositions other - && Math.abs(other.leftMeters - leftMeters) < 1E-9 - && Math.abs(other.rightMeters - rightMeters) < 1E-9; + && Math.abs(other.left - left) < 1E-9 + && Math.abs(other.right - right) < 1E-9; } @Override public int hashCode() { - return Objects.hash(leftMeters, rightMeters); + return Objects.hash(left, right); } @Override public String toString() { return String.format( - "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters); + "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", left, right); } @Override public DifferentialDriveWheelPositions interpolate( DifferentialDriveWheelPositions endValue, double t) { return new DifferentialDriveWheelPositions( - MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t), - MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t)); + MathUtil.interpolate(this.left, endValue.left, t), + MathUtil.interpolate(this.right, endValue.right, t)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java index 6796fabb1e2..60b3bca8f35 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java @@ -14,11 +14,11 @@ /** Represents the wheel speeds for a differential drive drivetrain. */ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, StructSerializable { - /** Speed of the left side of the robot. */ - public double leftMetersPerSecond; + /** Speed of the left side of the robot in meters per second. */ + public double left; - /** Speed of the right side of the robot. */ - public double rightMetersPerSecond; + /** Speed of the right side of the robot in meters per second. */ + public double right; /** DifferentialDriveWheelSpeeds protobuf for serialization. */ public static final DifferentialDriveWheelSpeedsProto proto = @@ -34,19 +34,19 @@ public DifferentialDriveWheelSpeeds() {} /** * Constructs a DifferentialDriveWheelSpeeds. * - * @param leftMetersPerSecond The left speed. - * @param rightMetersPerSecond The right speed. + * @param left The left speed in meters per second. + * @param right The right speed in meters per second. */ - public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) { - this.leftMetersPerSecond = leftMetersPerSecond; - this.rightMetersPerSecond = rightMetersPerSecond; + public DifferentialDriveWheelSpeeds(double left, double right) { + this.left = left; + this.right = right; } /** * Constructs a DifferentialDriveWheelSpeeds. * - * @param left The left speed. - * @param right The right speed. + * @param left The left speed in meters per second. + * @param right The right speed in meters per second. */ public DifferentialDriveWheelSpeeds(LinearVelocity left, LinearVelocity right) { this(left.in(MetersPerSecond), right.in(MetersPerSecond)); @@ -60,15 +60,14 @@ public DifferentialDriveWheelSpeeds(LinearVelocity left, LinearVelocity right) { * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the * absolute threshold, while maintaining the ratio of speeds between wheels. * - * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach. + * @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach. */ - public void desaturate(double attainableMaxSpeedMetersPerSecond) { - double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond)); + public void desaturate(double attainableMaxSpeed) { + double realMaxSpeed = Math.max(Math.abs(left), Math.abs(right)); - if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { - leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; - rightMetersPerSecond = - rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; + if (realMaxSpeed > attainableMaxSpeed) { + left = left / realMaxSpeed * attainableMaxSpeed; + right = right / realMaxSpeed * attainableMaxSpeed; } } @@ -80,7 +79,7 @@ public void desaturate(double attainableMaxSpeedMetersPerSecond) { * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the * absolute threshold, while maintaining the ratio of speeds between wheels. * - * @param attainableMaxSpeed The absolute max speed that a wheel can reach. + * @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach. */ public void desaturate(LinearVelocity attainableMaxSpeed) { desaturate(attainableMaxSpeed.in(MetersPerSecond)); @@ -96,9 +95,7 @@ public void desaturate(LinearVelocity attainableMaxSpeed) { * @return The sum of the DifferentialDriveWheelSpeeds. */ public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) { - return new DifferentialDriveWheelSpeeds( - leftMetersPerSecond + other.leftMetersPerSecond, - rightMetersPerSecond + other.rightMetersPerSecond); + return new DifferentialDriveWheelSpeeds(left + other.left, right + other.right); } /** @@ -112,9 +109,7 @@ public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) { * @return The difference between the two DifferentialDriveWheelSpeeds. */ public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) { - return new DifferentialDriveWheelSpeeds( - leftMetersPerSecond - other.leftMetersPerSecond, - rightMetersPerSecond - other.rightMetersPerSecond); + return new DifferentialDriveWheelSpeeds(left - other.left, right - other.right); } /** @@ -124,7 +119,7 @@ public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) { * @return The inverse of the current DifferentialDriveWheelSpeeds. */ public DifferentialDriveWheelSpeeds unaryMinus() { - return new DifferentialDriveWheelSpeeds(-leftMetersPerSecond, -rightMetersPerSecond); + return new DifferentialDriveWheelSpeeds(-left, -right); } /** @@ -138,8 +133,7 @@ public DifferentialDriveWheelSpeeds unaryMinus() { * @return The scaled DifferentialDriveWheelSpeeds. */ public DifferentialDriveWheelSpeeds times(double scalar) { - return new DifferentialDriveWheelSpeeds( - leftMetersPerSecond * scalar, rightMetersPerSecond * scalar); + return new DifferentialDriveWheelSpeeds(left * scalar, right * scalar); } /** @@ -153,14 +147,12 @@ public DifferentialDriveWheelSpeeds times(double scalar) { * @return The scaled DifferentialDriveWheelSpeeds. */ public DifferentialDriveWheelSpeeds div(double scalar) { - return new DifferentialDriveWheelSpeeds( - leftMetersPerSecond / scalar, rightMetersPerSecond / scalar); + return new DifferentialDriveWheelSpeeds(left / scalar, right / scalar); } @Override public String toString() { return String.format( - "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)", - leftMetersPerSecond, rightMetersPerSecond); + "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)", left, right); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index d58efdd7e6b..1217f02dd41 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -40,10 +40,10 @@ public class MecanumDriveKinematics private final SimpleMatrix m_inverseKinematics; private final SimpleMatrix m_forwardKinematics; - private final Translation2d m_frontLeftWheelMeters; - private final Translation2d m_frontRightWheelMeters; - private final Translation2d m_rearLeftWheelMeters; - private final Translation2d m_rearRightWheelMeters; + private final Translation2d m_frontLeftWheel; + private final Translation2d m_frontRightWheel; + private final Translation2d m_rearLeftWheel; + private final Translation2d m_rearRightWheel; private Translation2d m_prevCoR = Translation2d.kZero; @@ -56,29 +56,28 @@ public class MecanumDriveKinematics /** * Constructs a mecanum drive kinematics object. * - * @param frontLeftWheelMeters The location of the front-left wheel relative to the physical - * center of the robot. - * @param frontRightWheelMeters The location of the front-right wheel relative to the physical - * center of the robot. - * @param rearLeftWheelMeters The location of the rear-left wheel relative to the physical center - * of the robot. - * @param rearRightWheelMeters The location of the rear-right wheel relative to the physical - * center of the robot. + * @param frontLeftWheel The location of the front-left wheel relative to the physical center of + * the robot, in meters. + * @param frontRightWheel The location of the front-right wheel relative to the physical center of + * the robot, in meters. + * @param rearLeftWheel The location of the rear-left wheel relative to the physical center of the + * robot, in meters. + * @param rearRightWheel The location of the rear-right wheel relative to the physical center of + * the robot, in meters. */ public MecanumDriveKinematics( - Translation2d frontLeftWheelMeters, - Translation2d frontRightWheelMeters, - Translation2d rearLeftWheelMeters, - Translation2d rearRightWheelMeters) { - m_frontLeftWheelMeters = frontLeftWheelMeters; - m_frontRightWheelMeters = frontRightWheelMeters; - m_rearLeftWheelMeters = rearLeftWheelMeters; - m_rearRightWheelMeters = rearRightWheelMeters; + Translation2d frontLeftWheel, + Translation2d frontRightWheel, + Translation2d rearLeftWheel, + Translation2d rearRightWheel) { + m_frontLeftWheel = frontLeftWheel; + m_frontRightWheel = frontRightWheel; + m_rearLeftWheel = rearLeftWheel; + m_rearRightWheel = rearRightWheel; m_inverseKinematics = new SimpleMatrix(4, 3); - setInverseKinematics( - frontLeftWheelMeters, frontRightWheelMeters, rearLeftWheelMeters, rearRightWheelMeters); + setInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel, rearRightWheel); m_forwardKinematics = m_inverseKinematics.pseudoInverse(); MathSharedStore.reportUsage("MecanumDriveKinematics", ""); @@ -94,33 +93,28 @@ public MecanumDriveKinematics( * for evasive maneuvers, vision alignment, or for any other use case, you can do so. * * @param chassisSpeeds The desired chassis speed. - * @param centerOfRotationMeters The center of rotation. For example, if you set the center of - * rotation at one corner of the robot and provide a chassis speed that only has a dtheta - * component, the robot will rotate around that corner. + * @param centerOfRotation The center of rotation. For example, if you set the center of rotation + * at one corner of the robot and provide a chassis speed that only has a dtheta component, + * the robot will rotate around that corner. * @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user input * may cause one of the wheel speeds to go above the attainable max velocity. Use the {@link * MecanumDriveWheelSpeeds#desaturate(double)} function to rectify this issue. */ public MecanumDriveWheelSpeeds toWheelSpeeds( - ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) { + ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation) { // We have a new center of rotation. We need to compute the matrix again. - if (!centerOfRotationMeters.equals(m_prevCoR)) { - var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters); - var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters); - var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters); - var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters); + if (!centerOfRotation.equals(m_prevCoR)) { + var fl = m_frontLeftWheel.minus(centerOfRotation); + var fr = m_frontRightWheel.minus(centerOfRotation); + var rl = m_rearLeftWheel.minus(centerOfRotation); + var rr = m_rearRightWheel.minus(centerOfRotation); setInverseKinematics(fl, fr, rl, rr); - m_prevCoR = centerOfRotationMeters; + m_prevCoR = centerOfRotation; } var chassisSpeedsVector = new SimpleMatrix(3, 1); - chassisSpeedsVector.setColumn( - 0, - 0, - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond, - chassisSpeeds.omegaRadiansPerSecond); + chassisSpeedsVector.setColumn(0, 0, chassisSpeeds.vx, chassisSpeeds.vy, chassisSpeeds.omega); var wheelsVector = m_inverseKinematics.mult(chassisSpeedsVector); return new MecanumDriveWheelSpeeds( @@ -156,10 +150,10 @@ public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) { wheelSpeedsVector.setColumn( 0, 0, - wheelSpeeds.frontLeftMetersPerSecond, - wheelSpeeds.frontRightMetersPerSecond, - wheelSpeeds.rearLeftMetersPerSecond, - wheelSpeeds.rearRightMetersPerSecond); + wheelSpeeds.frontLeft, + wheelSpeeds.frontRight, + wheelSpeeds.rearLeft, + wheelSpeeds.rearRight); var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsVector); return new ChassisSpeeds( @@ -174,10 +168,10 @@ public Twist2d toTwist2d(MecanumDriveWheelPositions start, MecanumDriveWheelPosi wheelDeltasVector.setColumn( 0, 0, - end.frontLeftMeters - start.frontLeftMeters, - end.frontRightMeters - start.frontRightMeters, - end.rearLeftMeters - start.rearLeftMeters, - end.rearRightMeters - start.rearRightMeters); + end.frontLeft - start.frontLeft, + end.frontRight - start.frontRight, + end.rearLeft - start.rearLeft, + end.rearRight - start.rearRight); var twist = m_forwardKinematics.mult(wheelDeltasVector); return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0)); } @@ -195,10 +189,10 @@ public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) { wheelDeltasVector.setColumn( 0, 0, - wheelDeltas.frontLeftMeters, - wheelDeltas.frontRightMeters, - wheelDeltas.rearLeftMeters, - wheelDeltas.rearRightMeters); + wheelDeltas.frontLeft, + wheelDeltas.frontRight, + wheelDeltas.rearLeft, + wheelDeltas.rearRight); var twist = m_forwardKinematics.mult(wheelDeltasVector); return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0)); } @@ -225,7 +219,7 @@ private void setInverseKinematics( * @return The front-left wheel translation. */ public Translation2d getFrontLeft() { - return m_frontLeftWheelMeters; + return m_frontLeftWheel; } /** @@ -234,7 +228,7 @@ public Translation2d getFrontLeft() { * @return The front-right wheel translation. */ public Translation2d getFrontRight() { - return m_frontRightWheelMeters; + return m_frontRightWheel; } /** @@ -243,7 +237,7 @@ public Translation2d getFrontRight() { * @return The rear-left wheel translation. */ public Translation2d getRearLeft() { - return m_rearLeftWheelMeters; + return m_rearLeftWheel; } /** @@ -252,24 +246,21 @@ public Translation2d getRearLeft() { * @return The rear-right wheel translation. */ public Translation2d getRearRight() { - return m_rearRightWheelMeters; + return m_rearRightWheel; } @Override public MecanumDriveWheelPositions copy(MecanumDriveWheelPositions positions) { return new MecanumDriveWheelPositions( - positions.frontLeftMeters, - positions.frontRightMeters, - positions.rearLeftMeters, - positions.rearRightMeters); + positions.frontLeft, positions.frontRight, positions.rearLeft, positions.rearRight); } @Override public void copyInto(MecanumDriveWheelPositions positions, MecanumDriveWheelPositions output) { - output.frontLeftMeters = positions.frontLeftMeters; - output.frontRightMeters = positions.frontRightMeters; - output.rearLeftMeters = positions.rearLeftMeters; - output.rearRightMeters = positions.rearRightMeters; + output.frontLeft = positions.frontLeft; + output.frontRight = positions.frontRight; + output.rearLeft = positions.rearLeft; + output.rearRight = positions.rearRight; } @Override diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java index 51249d4fc3a..e4c0c7e1a05 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java @@ -8,16 +8,16 @@ @Deprecated(since = "2025", forRemoval = true) public class MecanumDriveMotorVoltages { /** Voltage of the front left motor. */ - public double frontLeftVoltage; + public double frontLeft; /** Voltage of the front right motor. */ - public double frontRightVoltage; + public double frontRight; /** Voltage of the rear left motor. */ - public double rearLeftVoltage; + public double rearLeft; /** Voltage of the rear right motor. */ - public double rearRightVoltage; + public double rearRight; /** Constructs a MecanumDriveMotorVoltages with zeros for all member fields. */ public MecanumDriveMotorVoltages() {} @@ -25,20 +25,17 @@ public MecanumDriveMotorVoltages() {} /** * Constructs a MecanumDriveMotorVoltages. * - * @param frontLeftVoltage Voltage of the front left motor. - * @param frontRightVoltage Voltage of the front right motor. - * @param rearLeftVoltage Voltage of the rear left motor. - * @param rearRightVoltage Voltage of the rear right motor. + * @param frontLeft Voltage of the front left motor. + * @param frontRight Voltage of the front right motor. + * @param rearLeft Voltage of the rear left motor. + * @param rearRight Voltage of the rear right motor. */ public MecanumDriveMotorVoltages( - double frontLeftVoltage, - double frontRightVoltage, - double rearLeftVoltage, - double rearRightVoltage) { - this.frontLeftVoltage = frontLeftVoltage; - this.frontRightVoltage = frontRightVoltage; - this.rearLeftVoltage = rearLeftVoltage; - this.rearRightVoltage = rearRightVoltage; + double frontLeft, double frontRight, double rearLeft, double rearRight) { + this.frontLeft = frontLeft; + this.frontRight = frontRight; + this.rearLeft = rearLeft; + this.rearRight = rearRight; } @Override @@ -46,6 +43,6 @@ public String toString() { return String.format( "MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, " + "Rear Left: %.2f V, Rear Right: %.2f V)", - frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage); + frontLeft, frontRight, rearLeft, rearRight); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java index 4ed5b6b2b8d..b7b0a68d753 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java @@ -22,14 +22,14 @@ public class MecanumDriveOdometry extends Odometry { * @param kinematics The mecanum drive kinematics for your drivetrain. * @param gyroAngle The angle reported by the gyroscope. * @param wheelPositions The distances driven by each wheel. - * @param initialPoseMeters The starting position of the robot on the field. + * @param initialPose The starting position of the robot on the field. */ public MecanumDriveOdometry( MecanumDriveKinematics kinematics, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, - Pose2d initialPoseMeters) { - super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); + Pose2d initialPose) { + super(kinematics, gyroAngle, wheelPositions, initialPose); MathSharedStore.reportUsage("MecanumDriveOdometry", ""); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java index a96cf61b0d9..2fe249117e3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java @@ -31,14 +31,14 @@ public class MecanumDriveOdometry3d extends Odometry3d, ProtobufSerializable, StructSerializable { - /** Distance measured by the front left wheel. */ - public double frontLeftMeters; + /** Distance measured by the front left wheel in meters. */ + public double frontLeft; - /** Distance measured by the front right wheel. */ - public double frontRightMeters; + /** Distance measured by the front right wheel in meters. */ + public double frontRight; - /** Distance measured by the rear left wheel. */ - public double rearLeftMeters; + /** Distance measured by the rear left wheel in meters. */ + public double rearLeft; - /** Distance measured by the rear right wheel. */ - public double rearRightMeters; + /** Distance measured by the rear right wheel in meters. */ + public double rearRight; /** MecanumDriveWheelPositions protobuf for serialization. */ public static final MecanumDriveWheelPositionsProto proto = new MecanumDriveWheelPositionsProto(); @@ -45,29 +45,26 @@ public MecanumDriveWheelPositions() {} /** * Constructs a MecanumDriveWheelPositions. * - * @param frontLeftMeters Distance measured by the front left wheel. - * @param frontRightMeters Distance measured by the front right wheel. - * @param rearLeftMeters Distance measured by the rear left wheel. - * @param rearRightMeters Distance measured by the rear right wheel. + * @param frontLeft Distance measured by the front left wheel in meters. + * @param frontRight Distance measured by the front right wheel in meters. + * @param rearLeft Distance measured by the rear left wheel in meters. + * @param rearRight Distance measured by the rear right wheel in meters. */ public MecanumDriveWheelPositions( - double frontLeftMeters, - double frontRightMeters, - double rearLeftMeters, - double rearRightMeters) { - this.frontLeftMeters = frontLeftMeters; - this.frontRightMeters = frontRightMeters; - this.rearLeftMeters = rearLeftMeters; - this.rearRightMeters = rearRightMeters; + double frontLeft, double frontRight, double rearLeft, double rearRight) { + this.frontLeft = frontLeft; + this.frontRight = frontRight; + this.rearLeft = rearLeft; + this.rearRight = rearRight; } /** * Constructs a MecanumDriveWheelPositions. * - * @param frontLeft Distance measured by the front left wheel. - * @param frontRight Distance measured by the front right wheel. - * @param rearLeft Distance measured by the rear left wheel. - * @param rearRight Distance measured by the rear right wheel. + * @param frontLeft Distance measured by the front left wheel in meters. + * @param frontRight Distance measured by the front right wheel in meters. + * @param rearLeft Distance measured by the rear left wheel in meters. + * @param rearRight Distance measured by the rear right wheel in meters. */ public MecanumDriveWheelPositions( Distance frontLeft, Distance frontRight, Distance rearLeft, Distance rearRight) { @@ -77,15 +74,15 @@ public MecanumDriveWheelPositions( @Override public boolean equals(Object obj) { return obj instanceof MecanumDriveWheelPositions other - && Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9 - && Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9 - && Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9 - && Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9; + && Math.abs(other.frontLeft - frontLeft) < 1E-9 + && Math.abs(other.frontRight - frontRight) < 1E-9 + && Math.abs(other.rearLeft - rearLeft) < 1E-9 + && Math.abs(other.rearRight - rearRight) < 1E-9; } @Override public int hashCode() { - return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters); + return Objects.hash(frontLeft, frontRight, rearLeft, rearRight); } @Override @@ -93,15 +90,15 @@ public String toString() { return String.format( "MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, " + "Rear Left: %.2f m, Rear Right: %.2f m)", - frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters); + frontLeft, frontRight, rearLeft, rearRight); } @Override public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) { return new MecanumDriveWheelPositions( - MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t), - MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t), - MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t), - MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t)); + MathUtil.interpolate(this.frontLeft, endValue.frontLeft, t), + MathUtil.interpolate(this.frontRight, endValue.frontRight, t), + MathUtil.interpolate(this.rearLeft, endValue.rearLeft, t), + MathUtil.interpolate(this.rearRight, endValue.rearRight, t)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java index e85adf8fd6c..11c05c0bd40 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java @@ -14,17 +14,17 @@ /** Represents the wheel speeds for a mecanum drive drivetrain. */ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSerializable { - /** Speed of the front left wheel. */ - public double frontLeftMetersPerSecond; + /** Speed of the front left wheel in meters per second. */ + public double frontLeft; - /** Speed of the front right wheel. */ - public double frontRightMetersPerSecond; + /** Speed of the front right wheel in meters per second. */ + public double frontRight; - /** Speed of the rear left wheel. */ - public double rearLeftMetersPerSecond; + /** Speed of the rear left wheel in meters per second. */ + public double rearLeft; - /** Speed of the rear right wheel. */ - public double rearRightMetersPerSecond; + /** Speed of the rear right wheel in meters per second. */ + public double rearRight; /** MecanumDriveWheelSpeeds protobuf for serialization. */ public static final MecanumDriveWheelSpeedsProto proto = new MecanumDriveWheelSpeedsProto(); @@ -38,29 +38,26 @@ public MecanumDriveWheelSpeeds() {} /** * Constructs a MecanumDriveWheelSpeeds. * - * @param frontLeftMetersPerSecond Speed of the front left wheel. - * @param frontRightMetersPerSecond Speed of the front right wheel. - * @param rearLeftMetersPerSecond Speed of the rear left wheel. - * @param rearRightMetersPerSecond Speed of the rear right wheel. + * @param frontLeft Speed of the front left wheel in meters per second. + * @param frontRight Speed of the front right wheel in meters per second. + * @param rearLeft Speed of the rear left wheel in meters per second. + * @param rearRight Speed of the rear right wheel in meters per second. */ public MecanumDriveWheelSpeeds( - double frontLeftMetersPerSecond, - double frontRightMetersPerSecond, - double rearLeftMetersPerSecond, - double rearRightMetersPerSecond) { - this.frontLeftMetersPerSecond = frontLeftMetersPerSecond; - this.frontRightMetersPerSecond = frontRightMetersPerSecond; - this.rearLeftMetersPerSecond = rearLeftMetersPerSecond; - this.rearRightMetersPerSecond = rearRightMetersPerSecond; + double frontLeft, double frontRight, double rearLeft, double rearRight) { + this.frontLeft = frontLeft; + this.frontRight = frontRight; + this.rearLeft = rearLeft; + this.rearRight = rearRight; } /** * Constructs a MecanumDriveWheelSpeeds. * - * @param frontLeft Speed of the front left wheel. - * @param frontRight Speed of the front right wheel. - * @param rearLeft Speed of the rear left wheel. - * @param rearRight Speed of the rear right wheel. + * @param frontLeft Speed of the front left wheel in meters per second. + * @param frontRight Speed of the front right wheel in meters per second. + * @param rearLeft Speed of the rear left wheel in meters per second. + * @param rearRight Speed of the rear right wheel in meters per second. */ public MecanumDriveWheelSpeeds( LinearVelocity frontLeft, @@ -82,28 +79,23 @@ public MecanumDriveWheelSpeeds( * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the * absolute threshold, while maintaining the ratio of speeds between wheels. * - * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach. + * @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach. * @return Desaturated MecanumDriveWheelSpeeds. */ - public MecanumDriveWheelSpeeds desaturate(double attainableMaxSpeedMetersPerSecond) { - double realMaxSpeed = - Math.max(Math.abs(frontLeftMetersPerSecond), Math.abs(frontRightMetersPerSecond)); - realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeftMetersPerSecond)); - realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRightMetersPerSecond)); + public MecanumDriveWheelSpeeds desaturate(double attainableMaxSpeed) { + double realMaxSpeed = Math.max(Math.abs(frontLeft), Math.abs(frontRight)); + realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeft)); + realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRight)); - if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { + if (realMaxSpeed > attainableMaxSpeed) { return new MecanumDriveWheelSpeeds( - frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond, - frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond, - rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond, - rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond); + frontLeft / realMaxSpeed * attainableMaxSpeed, + frontRight / realMaxSpeed * attainableMaxSpeed, + rearLeft / realMaxSpeed * attainableMaxSpeed, + rearRight / realMaxSpeed * attainableMaxSpeed); } - return new MecanumDriveWheelSpeeds( - frontLeftMetersPerSecond, - frontRightMetersPerSecond, - rearLeftMetersPerSecond, - rearRightMetersPerSecond); + return new MecanumDriveWheelSpeeds(frontLeft, frontRight, rearLeft, rearRight); } /** @@ -132,10 +124,10 @@ public MecanumDriveWheelSpeeds desaturate(LinearVelocity attainableMaxSpeed) { */ public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) { return new MecanumDriveWheelSpeeds( - frontLeftMetersPerSecond + other.frontLeftMetersPerSecond, - frontRightMetersPerSecond + other.frontRightMetersPerSecond, - rearLeftMetersPerSecond + other.rearLeftMetersPerSecond, - rearRightMetersPerSecond + other.rearRightMetersPerSecond); + frontLeft + other.frontLeft, + frontRight + other.frontRight, + rearLeft + other.rearLeft, + rearRight + other.rearRight); } /** @@ -150,10 +142,10 @@ public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) { */ public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) { return new MecanumDriveWheelSpeeds( - frontLeftMetersPerSecond - other.frontLeftMetersPerSecond, - frontRightMetersPerSecond - other.frontRightMetersPerSecond, - rearLeftMetersPerSecond - other.rearLeftMetersPerSecond, - rearRightMetersPerSecond - other.rearRightMetersPerSecond); + frontLeft - other.frontLeft, + frontRight - other.frontRight, + rearLeft - other.rearLeft, + rearRight - other.rearRight); } /** @@ -163,11 +155,7 @@ public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) { * @return The inverse of the current MecanumDriveWheelSpeeds. */ public MecanumDriveWheelSpeeds unaryMinus() { - return new MecanumDriveWheelSpeeds( - -frontLeftMetersPerSecond, - -frontRightMetersPerSecond, - -rearLeftMetersPerSecond, - -rearRightMetersPerSecond); + return new MecanumDriveWheelSpeeds(-frontLeft, -frontRight, -rearLeft, -rearRight); } /** @@ -181,10 +169,7 @@ public MecanumDriveWheelSpeeds unaryMinus() { */ public MecanumDriveWheelSpeeds times(double scalar) { return new MecanumDriveWheelSpeeds( - frontLeftMetersPerSecond * scalar, - frontRightMetersPerSecond * scalar, - rearLeftMetersPerSecond * scalar, - rearRightMetersPerSecond * scalar); + frontLeft * scalar, frontRight * scalar, rearLeft * scalar, rearRight * scalar); } /** @@ -198,10 +183,7 @@ public MecanumDriveWheelSpeeds times(double scalar) { */ public MecanumDriveWheelSpeeds div(double scalar) { return new MecanumDriveWheelSpeeds( - frontLeftMetersPerSecond / scalar, - frontRightMetersPerSecond / scalar, - rearLeftMetersPerSecond / scalar, - rearRightMetersPerSecond / scalar); + frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar); } @Override @@ -209,9 +191,6 @@ public String toString() { return String.format( "MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, " + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", - frontLeftMetersPerSecond, - frontRightMetersPerSecond, - rearLeftMetersPerSecond, - rearRightMetersPerSecond); + frontLeft, frontRight, rearLeft, rearRight); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java index 6a09f92bffd..9c99696590a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java @@ -21,7 +21,7 @@ */ public class Odometry { private final Kinematics m_kinematics; - private Pose2d m_poseMeters; + private Pose2d m_pose; private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; @@ -33,17 +33,14 @@ public class Odometry { * @param kinematics The kinematics of the drivebase. * @param gyroAngle The angle reported by the gyroscope. * @param wheelPositions The current encoder readings. - * @param initialPoseMeters The starting position of the robot on the field. + * @param initialPose The starting position of the robot on the field. */ public Odometry( - Kinematics kinematics, - Rotation2d gyroAngle, - T wheelPositions, - Pose2d initialPoseMeters) { + Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) { m_kinematics = kinematics; - m_poseMeters = initialPoseMeters; - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); - m_previousAngle = m_poseMeters.getRotation(); + m_pose = initialPose; + m_gyroOffset = m_pose.getRotation().minus(gyroAngle); + m_previousAngle = m_pose.getRotation(); m_previousWheelPositions = m_kinematics.copy(wheelPositions); } @@ -55,24 +52,24 @@ public Odometry( * * @param gyroAngle The angle reported by the gyroscope. * @param wheelPositions The current encoder readings. - * @param poseMeters The position on the field that your robot is at. + * @param pose The position on the field that your robot is at. */ - public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) { - m_poseMeters = poseMeters; - m_previousAngle = m_poseMeters.getRotation(); - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d pose) { + m_pose = pose; + m_previousAngle = m_pose.getRotation(); + m_gyroOffset = m_pose.getRotation().minus(gyroAngle); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); } /** * Resets the pose. * - * @param poseMeters The pose to reset to. + * @param pose The pose to reset to. */ - public void resetPose(Pose2d poseMeters) { - m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation())); - m_poseMeters = poseMeters; - m_previousAngle = m_poseMeters.getRotation(); + public void resetPose(Pose2d pose) { + m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation())); + m_pose = pose; + m_previousAngle = m_pose.getRotation(); } /** @@ -81,7 +78,7 @@ public void resetPose(Pose2d poseMeters) { * @param translation The translation to reset to. */ public void resetTranslation(Translation2d translation) { - m_poseMeters = new Pose2d(translation, m_poseMeters.getRotation()); + m_pose = new Pose2d(translation, m_pose.getRotation()); } /** @@ -90,9 +87,9 @@ public void resetTranslation(Translation2d translation) { * @param rotation The rotation to reset to. */ public void resetRotation(Rotation2d rotation) { - m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation())); - m_poseMeters = new Pose2d(m_poseMeters.getTranslation(), rotation); - m_previousAngle = m_poseMeters.getRotation(); + m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation())); + m_pose = new Pose2d(m_pose.getTranslation(), rotation); + m_previousAngle = m_pose.getRotation(); } /** @@ -100,8 +97,8 @@ public void resetRotation(Rotation2d rotation) { * * @return The pose of the robot (x and y are in meters). */ - public Pose2d getPoseMeters() { - return m_poseMeters; + public Pose2d getPose() { + return m_pose; } /** @@ -120,12 +117,12 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions); twist.dtheta = angle.minus(m_previousAngle).getRadians(); - var newPose = m_poseMeters.exp(twist); + var newPose = m_pose.exp(twist); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); m_previousAngle = angle; - m_poseMeters = new Pose2d(newPose.getTranslation(), angle); + m_pose = new Pose2d(newPose.getTranslation(), angle); - return m_poseMeters; + return m_pose; } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java index f79403ef92a..a50a7f1d739 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java @@ -30,7 +30,7 @@ */ public class Odometry3d { private final Kinematics m_kinematics; - private Pose3d m_poseMeters; + private Pose3d m_pose; private Rotation3d m_gyroOffset; private Rotation3d m_previousAngle; @@ -42,17 +42,14 @@ public class Odometry3d { * @param kinematics The kinematics of the drivebase. * @param gyroAngle The angle reported by the gyroscope. * @param wheelPositions The current encoder readings. - * @param initialPoseMeters The starting position of the robot on the field. + * @param initialPose The starting position of the robot on the field. */ public Odometry3d( - Kinematics kinematics, - Rotation3d gyroAngle, - T wheelPositions, - Pose3d initialPoseMeters) { + Kinematics kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) { m_kinematics = kinematics; - m_poseMeters = initialPoseMeters; - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); - m_previousAngle = m_poseMeters.getRotation(); + m_pose = initialPose; + m_gyroOffset = m_pose.getRotation().minus(gyroAngle); + m_previousAngle = m_pose.getRotation(); m_previousWheelPositions = m_kinematics.copy(wheelPositions); } @@ -64,24 +61,24 @@ public Odometry3d( * * @param gyroAngle The angle reported by the gyroscope. * @param wheelPositions The current encoder readings. - * @param poseMeters The position on the field that your robot is at. + * @param pose The position on the field that your robot is at. */ - public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) { - m_poseMeters = poseMeters; - m_previousAngle = m_poseMeters.getRotation(); - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) { + m_pose = pose; + m_previousAngle = m_pose.getRotation(); + m_gyroOffset = m_pose.getRotation().minus(gyroAngle); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); } /** * Resets the pose. * - * @param poseMeters The pose to reset to. + * @param pose The pose to reset to. */ - public void resetPose(Pose3d poseMeters) { - m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation())); - m_poseMeters = poseMeters; - m_previousAngle = m_poseMeters.getRotation(); + public void resetPose(Pose3d pose) { + m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation())); + m_pose = pose; + m_previousAngle = m_pose.getRotation(); } /** @@ -90,7 +87,7 @@ public void resetPose(Pose3d poseMeters) { * @param translation The translation to reset to. */ public void resetTranslation(Translation3d translation) { - m_poseMeters = new Pose3d(translation, m_poseMeters.getRotation()); + m_pose = new Pose3d(translation, m_pose.getRotation()); } /** @@ -99,9 +96,9 @@ public void resetTranslation(Translation3d translation) { * @param rotation The rotation to reset to. */ public void resetRotation(Rotation3d rotation) { - m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation())); - m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation); - m_previousAngle = m_poseMeters.getRotation(); + m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation())); + m_pose = new Pose3d(m_pose.getTranslation(), rotation); + m_previousAngle = m_pose.getRotation(); } /** @@ -109,8 +106,8 @@ public void resetRotation(Rotation3d rotation) { * * @return The pose of the robot (x, y, and z are in meters). */ - public Pose3d getPoseMeters() { - return m_poseMeters; + public Pose3d getPose() { + return m_pose; } /** @@ -137,12 +134,12 @@ public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { angle_difference.get(1), angle_difference.get(2)); - var newPose = m_poseMeters.exp(twist); + var newPose = m_pose.exp(twist); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); m_previousAngle = angle; - m_poseMeters = new Pose3d(newPose.getTranslation(), angle); + m_pose = new Pose3d(newPose.getTranslation(), angle); - return m_poseMeters; + return m_pose; } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index 27834e60d57..915ad68fd3e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -61,15 +61,15 @@ public class SwerveDriveKinematics * also expected that you pass in the module states in the same order when calling the forward * kinematics methods. * - * @param moduleTranslationsMeters The locations of the modules relative to the physical center of - * the robot. + * @param moduleTranslations The locations of the modules relative to the physical center of the + * robot. */ - public SwerveDriveKinematics(Translation2d... moduleTranslationsMeters) { - if (moduleTranslationsMeters.length < 2) { + public SwerveDriveKinematics(Translation2d... moduleTranslations) { + if (moduleTranslations.length < 2) { throw new IllegalArgumentException("A swerve drive requires at least two modules"); } - m_numModules = moduleTranslationsMeters.length; - m_modules = Arrays.copyOf(moduleTranslationsMeters, m_numModules); + m_numModules = moduleTranslations.length; + m_modules = Arrays.copyOf(moduleTranslations, m_numModules); m_moduleHeadings = new Rotation2d[m_numModules]; Arrays.fill(m_moduleHeadings, Rotation2d.kZero); m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3); @@ -111,21 +111,19 @@ public void resetHeadings(Rotation2d... moduleHeadings) { * the previously calculated module angle will be maintained. * * @param chassisSpeeds The desired chassis speed. - * @param centerOfRotationMeters The center of rotation. For example, if you set the center of - * rotation at one corner of the robot and provide a chassis speed that only has a dtheta - * component, the robot will rotate around that corner. + * @param centerOfRotation The center of rotation. For example, if you set the center of rotation + * at one corner of the robot and provide a chassis speed that only has a dtheta component, + * the robot will rotate around that corner. * @return An array containing the module states. Use caution because these module states are not * normalized. Sometimes, a user input may cause one of the module speeds to go above the * attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double) * DesaturateWheelSpeeds} function to rectify this issue. */ public SwerveModuleState[] toSwerveModuleStates( - ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) { + ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation) { var moduleStates = new SwerveModuleState[m_numModules]; - if (chassisSpeeds.vxMetersPerSecond == 0.0 - && chassisSpeeds.vyMetersPerSecond == 0.0 - && chassisSpeeds.omegaRadiansPerSecond == 0.0) { + if (chassisSpeeds.vx == 0.0 && chassisSpeeds.vy == 0.0 && chassisSpeeds.omega == 0.0) { for (int i = 0; i < m_numModules; i++) { moduleStates[i] = new SwerveModuleState(0.0, m_moduleHeadings[i]); } @@ -133,31 +131,18 @@ public SwerveModuleState[] toSwerveModuleStates( return moduleStates; } - if (!centerOfRotationMeters.equals(m_prevCoR)) { + if (!centerOfRotation.equals(m_prevCoR)) { for (int i = 0; i < m_numModules; i++) { m_inverseKinematics.setRow( - i * 2 + 0, - 0, /* Start Data */ - 1, - 0, - -m_modules[i].getY() + centerOfRotationMeters.getY()); + i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotation.getY()); m_inverseKinematics.setRow( - i * 2 + 1, - 0, /* Start Data */ - 0, - 1, - +m_modules[i].getX() - centerOfRotationMeters.getX()); + i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX() - centerOfRotation.getX()); } - m_prevCoR = centerOfRotationMeters; + m_prevCoR = centerOfRotation; } var chassisSpeedsVector = new SimpleMatrix(3, 1); - chassisSpeedsVector.setColumn( - 0, - 0, - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond, - chassisSpeeds.omegaRadiansPerSecond); + chassisSpeedsVector.setColumn(0, 0, chassisSpeeds.vx, chassisSpeeds.vy, chassisSpeeds.omega); var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector); @@ -212,8 +197,8 @@ public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) { for (int i = 0; i < m_numModules; i++) { var module = moduleStates[i]; - moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos()); - moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin()); + moduleStatesMatrix.set(i * 2, 0, module.speed * module.angle.getCos()); + moduleStatesMatrix.set(i * 2 + 1, module.speed * module.angle.getSin()); } var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix); @@ -243,8 +228,8 @@ public Twist2d toTwist2d(SwerveModulePosition... moduleDeltas) { for (int i = 0; i < m_numModules; i++) { var module = moduleDeltas[i]; - moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos()); - moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin()); + moduleDeltaMatrix.set(i * 2, 0, module.distance * module.angle.getCos()); + moduleDeltaMatrix.set(i * 2 + 1, module.distance * module.angle.getSin()); } var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix); @@ -259,8 +244,7 @@ public Twist2d toTwist2d(SwerveModulePosition[] start, SwerveModulePosition[] en } var newPositions = new SwerveModulePosition[start.length]; for (int i = 0; i < start.length; i++) { - newPositions[i] = - new SwerveModulePosition(end[i].distanceMeters - start[i].distanceMeters, end[i].angle); + newPositions[i] = new SwerveModulePosition(end[i].distance - start[i].distance, end[i].angle); } return toTwist2d(newPositions); } @@ -275,18 +259,17 @@ public Twist2d toTwist2d(SwerveModulePosition[] start, SwerveModulePosition[] en * * @param moduleStates Reference to array of module states. The array will be mutated with the * normalized speeds! - * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach. + * @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach. */ public static void desaturateWheelSpeeds( - SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) { + SwerveModuleState[] moduleStates, double attainableMaxSpeed) { double realMaxSpeed = 0; for (SwerveModuleState moduleState : moduleStates) { - realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond)); + realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed)); } - if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { + if (realMaxSpeed > attainableMaxSpeed) { for (SwerveModuleState moduleState : moduleStates) { - moduleState.speedMetersPerSecond = - moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; + moduleState.speed = moduleState.speed / realMaxSpeed * attainableMaxSpeed; } } } @@ -301,7 +284,7 @@ public static void desaturateWheelSpeeds( * * @param moduleStates Reference to array of module states. The array will be mutated with the * normalized speeds! - * @param attainableMaxSpeed The absolute max speed that a module can reach. + * @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach. */ public static void desaturateWheelSpeeds( SwerveModuleState[] moduleStates, LinearVelocity attainableMaxSpeed) { @@ -320,38 +303,37 @@ public static void desaturateWheelSpeeds( * @param moduleStates Reference to array of module states. The array will be mutated with the * normalized speeds! * @param desiredChassisSpeed The desired speed of the robot - * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach - * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot - * can reach while translating - * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can - * reach while rotating + * @param attainableMaxModuleSpeed The absolute max speed in meters per second that a module can + * reach + * @param attainableMaxTranslationalSpeed The absolute max speed in meters per second that your + * robot can reach while translating + * @param attainableMaxRotationalVelocity The absolute max speed in radians per second the robot + * can reach while rotating */ public static void desaturateWheelSpeeds( SwerveModuleState[] moduleStates, ChassisSpeeds desiredChassisSpeed, - double attainableMaxModuleSpeedMetersPerSecond, - double attainableMaxTranslationalSpeedMetersPerSecond, - double attainableMaxRotationalVelocityRadiansPerSecond) { + double attainableMaxModuleSpeed, + double attainableMaxTranslationalSpeed, + double attainableMaxRotationalVelocity) { double realMaxSpeed = 0; for (SwerveModuleState moduleState : moduleStates) { - realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond)); + realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed)); } - if (attainableMaxTranslationalSpeedMetersPerSecond == 0 - || attainableMaxRotationalVelocityRadiansPerSecond == 0 + if (attainableMaxTranslationalSpeed == 0 + || attainableMaxRotationalVelocity == 0 || realMaxSpeed == 0) { return; } double translationalK = - Math.hypot(desiredChassisSpeed.vxMetersPerSecond, desiredChassisSpeed.vyMetersPerSecond) - / attainableMaxTranslationalSpeedMetersPerSecond; - double rotationalK = - Math.abs(desiredChassisSpeed.omegaRadiansPerSecond) - / attainableMaxRotationalVelocityRadiansPerSecond; + Math.hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy) + / attainableMaxTranslationalSpeed; + double rotationalK = Math.abs(desiredChassisSpeed.omega) / attainableMaxRotationalVelocity; double k = Math.max(translationalK, rotationalK); - double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1); + double scale = Math.min(k * attainableMaxModuleSpeed / realMaxSpeed, 1); for (SwerveModuleState moduleState : moduleStates) { - moduleState.speedMetersPerSecond *= scale; + moduleState.speed *= scale; } } @@ -402,7 +384,7 @@ public void copyInto(SwerveModulePosition[] positions, SwerveModulePosition[] ou throw new IllegalArgumentException("Inconsistent number of modules!"); } for (int i = 0; i < positions.length; ++i) { - output[i].distanceMeters = positions[i].distanceMeters; + output[i].distance = positions[i].distance; output[i].angle = positions[i].angle; } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java index a081a702498..fc65e5f788f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java @@ -22,8 +22,8 @@ public class SwerveModulePosition Interpolatable, ProtobufSerializable, StructSerializable { - /** Distance measured by the wheel of the module. */ - public double distanceMeters; + /** Distance measured by the wheel of the module in meters. */ + public double distance; /** Angle of the module. */ public Rotation2d angle = Rotation2d.kZero; @@ -40,11 +40,11 @@ public SwerveModulePosition() {} /** * Constructs a SwerveModulePosition. * - * @param distanceMeters The distance measured by the wheel of the module. + * @param distance The distance measured by the wheel of the module in meters. * @param angle The angle of the module. */ - public SwerveModulePosition(double distanceMeters, Rotation2d angle) { - this.distanceMeters = distanceMeters; + public SwerveModulePosition(double distance, Rotation2d angle) { + this.distance = distance; this.angle = angle; } @@ -61,13 +61,13 @@ public SwerveModulePosition(Distance distance, Rotation2d angle) { @Override public boolean equals(Object obj) { return obj instanceof SwerveModulePosition other - && Math.abs(other.distanceMeters - distanceMeters) < 1E-9 + && Math.abs(other.distance - distance) < 1E-9 && angle.equals(other.angle); } @Override public int hashCode() { - return Objects.hash(distanceMeters, angle); + return Objects.hash(distance, angle); } /** @@ -79,13 +79,12 @@ public int hashCode() { */ @Override public int compareTo(SwerveModulePosition other) { - return Double.compare(this.distanceMeters, other.distanceMeters); + return Double.compare(this.distance, other.distance); } @Override public String toString() { - return String.format( - "SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle); + return String.format("SwerveModulePosition(Distance: %.2f m, Angle: %s)", distance, angle); } /** @@ -94,13 +93,13 @@ public String toString() { * @return A copy. */ public SwerveModulePosition copy() { - return new SwerveModulePosition(distanceMeters, angle); + return new SwerveModulePosition(distance, angle); } @Override public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) { return new SwerveModulePosition( - MathUtil.interpolate(this.distanceMeters, endValue.distanceMeters, t), + MathUtil.interpolate(this.distance, endValue.distance, t), this.angle.interpolate(endValue.angle, t)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java index 6cadd35b4a8..8855f980bea 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java @@ -17,8 +17,8 @@ /** Represents the state of one swerve module. */ public class SwerveModuleState implements Comparable, ProtobufSerializable, StructSerializable { - /** Speed of the wheel of the module. */ - public double speedMetersPerSecond; + /** Speed of the wheel of the module in meters per second. */ + public double speed; /** Angle of the module. */ public Rotation2d angle = Rotation2d.kZero; @@ -35,11 +35,11 @@ public SwerveModuleState() {} /** * Constructs a SwerveModuleState. * - * @param speedMetersPerSecond The speed of the wheel of the module. + * @param speed The speed of the wheel of the module in meters per second. * @param angle The angle of the module. */ - public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) { - this.speedMetersPerSecond = speedMetersPerSecond; + public SwerveModuleState(double speed, Rotation2d angle) { + this.speed = speed; this.angle = angle; } @@ -56,13 +56,13 @@ public SwerveModuleState(LinearVelocity speed, Rotation2d angle) { @Override public boolean equals(Object obj) { return obj instanceof SwerveModuleState other - && Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9 + && Math.abs(other.speed - speed) < 1E-9 && angle.equals(other.angle); } @Override public int hashCode() { - return Objects.hash(speedMetersPerSecond, angle); + return Objects.hash(speed, angle); } /** @@ -74,13 +74,12 @@ public int hashCode() { */ @Override public int compareTo(SwerveModuleState other) { - return Double.compare(this.speedMetersPerSecond, other.speedMetersPerSecond); + return Double.compare(this.speed, other.speed); } @Override public String toString() { - return String.format( - "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle); + return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speed, angle); } /** @@ -93,7 +92,7 @@ public String toString() { public void optimize(Rotation2d currentAngle) { var delta = angle.minus(currentAngle); if (Math.abs(delta.getDegrees()) > 90.0) { - speedMetersPerSecond *= -1; + speed *= -1; angle = angle.rotateBy(Rotation2d.kPi); } } @@ -114,9 +113,9 @@ public static SwerveModuleState optimize( var delta = desiredState.angle.minus(currentAngle); if (Math.abs(delta.getDegrees()) > 90.0) { return new SwerveModuleState( - -desiredState.speedMetersPerSecond, desiredState.angle.rotateBy(Rotation2d.kPi)); + -desiredState.speed, desiredState.angle.rotateBy(Rotation2d.kPi)); } else { - return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); + return new SwerveModuleState(desiredState.speed, desiredState.angle); } } @@ -128,6 +127,6 @@ public static SwerveModuleState optimize( * @param currentAngle The current module angle. */ public void cosineScale(Rotation2d currentAngle) { - speedMetersPerSecond *= angle.minus(currentAngle).getCos(); + speed *= angle.minus(currentAngle).getCos(); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java index b20d23bbc9f..8a83b416782 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java @@ -32,8 +32,8 @@ public ChassisSpeeds unpack(ProtobufChassisSpeeds msg) { @Override public void pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) { - msg.setVx(value.vxMetersPerSecond); - msg.setVy(value.vyMetersPerSecond); - msg.setOmega(value.omegaRadiansPerSecond); + msg.setVx(value.vx); + msg.setVy(value.vy); + msg.setOmega(value.omega); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java index d1a8969d162..311b4d80576 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java @@ -28,11 +28,11 @@ public ProtobufDifferentialDriveKinematics createMessage() { @Override public DifferentialDriveKinematics unpack(ProtobufDifferentialDriveKinematics msg) { - return new DifferentialDriveKinematics(msg.getTrackWidth()); + return new DifferentialDriveKinematics(msg.getTrackwidth()); } @Override public void pack(ProtobufDifferentialDriveKinematics msg, DifferentialDriveKinematics value) { - msg.setTrackWidth(value.trackWidthMeters); + msg.setTrackwidth(value.trackwidth); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java index 982756b5b7c..23a70393831 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java @@ -34,7 +34,7 @@ public DifferentialDriveWheelPositions unpack(ProtobufDifferentialDriveWheelPosi @Override public void pack( ProtobufDifferentialDriveWheelPositions msg, DifferentialDriveWheelPositions value) { - msg.setLeft(value.leftMeters); - msg.setRight(value.rightMeters); + msg.setLeft(value.left); + msg.setRight(value.right); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java index e30df565749..c6023ac761c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java @@ -33,7 +33,7 @@ public DifferentialDriveWheelSpeeds unpack(ProtobufDifferentialDriveWheelSpeeds @Override public void pack(ProtobufDifferentialDriveWheelSpeeds msg, DifferentialDriveWheelSpeeds value) { - msg.setLeft(value.leftMetersPerSecond); - msg.setRight(value.rightMetersPerSecond); + msg.setLeft(value.left); + msg.setRight(value.right); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java index d4f0f5a3ce4..6469657f9de 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java @@ -34,9 +34,9 @@ public MecanumDriveWheelPositions unpack(ProtobufMecanumDriveWheelPositions msg) @Override public void pack(ProtobufMecanumDriveWheelPositions msg, MecanumDriveWheelPositions value) { - msg.setFrontLeft(value.frontLeftMeters); - msg.setFrontRight(value.frontRightMeters); - msg.setRearLeft(value.rearLeftMeters); - msg.setRearRight(value.rearRightMeters); + msg.setFrontLeft(value.frontLeft); + msg.setFrontRight(value.frontRight); + msg.setRearLeft(value.rearLeft); + msg.setRearRight(value.rearRight); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java index 85e8fd10a35..dffa38bcc78 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java @@ -34,9 +34,9 @@ public MecanumDriveWheelSpeeds unpack(ProtobufMecanumDriveWheelSpeeds msg) { @Override public void pack(ProtobufMecanumDriveWheelSpeeds msg, MecanumDriveWheelSpeeds value) { - msg.setFrontLeft(value.frontLeftMetersPerSecond); - msg.setFrontRight(value.frontRightMetersPerSecond); - msg.setRearLeft(value.rearLeftMetersPerSecond); - msg.setRearRight(value.rearRightMetersPerSecond); + msg.setFrontLeft(value.frontLeft); + msg.setFrontRight(value.frontRight); + msg.setRearLeft(value.rearLeft); + msg.setRearRight(value.rearRight); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java index 97cadf28c3a..a445c7b05f5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java @@ -34,7 +34,7 @@ public SwerveModulePosition unpack(ProtobufSwerveModulePosition msg) { @Override public void pack(ProtobufSwerveModulePosition msg, SwerveModulePosition value) { - msg.setDistance(value.distanceMeters); + msg.setDistance(value.distance); Rotation2d.proto.pack(msg.getMutableAngle(), value.angle); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java index cccff4d5910..f8de1375204 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java @@ -34,7 +34,7 @@ public SwerveModuleState unpack(ProtobufSwerveModuleState msg) { @Override public void pack(ProtobufSwerveModuleState msg, SwerveModuleState value) { - msg.setSpeed(value.speedMetersPerSecond); + msg.setSpeed(value.speed); Rotation2d.proto.pack(msg.getMutableAngle(), value.angle); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java index 16457f5e096..14d2030389d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java @@ -39,8 +39,8 @@ public ChassisSpeeds unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, ChassisSpeeds value) { - bb.putDouble(value.vxMetersPerSecond); - bb.putDouble(value.vyMetersPerSecond); - bb.putDouble(value.omegaRadiansPerSecond); + bb.putDouble(value.vx); + bb.putDouble(value.vy); + bb.putDouble(value.omega); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java index cc696ec908c..621af5741a7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java @@ -26,17 +26,17 @@ public int getSize() { @Override public String getSchema() { - return "double track_width"; + return "double trackwidth"; } @Override public DifferentialDriveKinematics unpack(ByteBuffer bb) { - double trackWidth = bb.getDouble(); - return new DifferentialDriveKinematics(trackWidth); + double trackwidth = bb.getDouble(); + return new DifferentialDriveKinematics(trackwidth); } @Override public void pack(ByteBuffer bb, DifferentialDriveKinematics value) { - bb.putDouble(value.trackWidthMeters); + bb.putDouble(value.trackwidth); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java index e8d86d1733e..8b2aeaf0bbc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java @@ -39,7 +39,7 @@ public DifferentialDriveWheelPositions unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, DifferentialDriveWheelPositions value) { - bb.putDouble(value.leftMeters); - bb.putDouble(value.rightMeters); + bb.putDouble(value.left); + bb.putDouble(value.right); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java index 1d5819737b5..dca590766d2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java @@ -38,7 +38,7 @@ public DifferentialDriveWheelSpeeds unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, DifferentialDriveWheelSpeeds value) { - bb.putDouble(value.leftMetersPerSecond); - bb.putDouble(value.rightMetersPerSecond); + bb.putDouble(value.left); + bb.putDouble(value.right); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java index 5b8d7bf2199..5974282996b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java @@ -40,9 +40,9 @@ public MecanumDriveWheelPositions unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, MecanumDriveWheelPositions value) { - bb.putDouble(value.frontLeftMeters); - bb.putDouble(value.frontRightMeters); - bb.putDouble(value.rearLeftMeters); - bb.putDouble(value.rearRightMeters); + bb.putDouble(value.frontLeft); + bb.putDouble(value.frontRight); + bb.putDouble(value.rearLeft); + bb.putDouble(value.rearRight); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java index 66956541fc6..8d2f6382bcc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java @@ -40,9 +40,9 @@ public MecanumDriveWheelSpeeds unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, MecanumDriveWheelSpeeds value) { - bb.putDouble(value.frontLeftMetersPerSecond); - bb.putDouble(value.frontRightMetersPerSecond); - bb.putDouble(value.rearLeftMetersPerSecond); - bb.putDouble(value.rearRightMetersPerSecond); + bb.putDouble(value.frontLeft); + bb.putDouble(value.frontRight); + bb.putDouble(value.rearLeft); + bb.putDouble(value.rearRight); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java index 6fff1cbb778..85ebdc47c3c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java @@ -44,7 +44,7 @@ public SwerveModulePosition unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, SwerveModulePosition value) { - bb.putDouble(value.distanceMeters); + bb.putDouble(value.distance); Rotation2d.struct.pack(bb, value.angle); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java index af29a8d628f..90c538221d9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java @@ -44,7 +44,7 @@ public SwerveModuleState unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, SwerveModuleState value) { - bb.putDouble(value.speedMetersPerSecond); + bb.putDouble(value.speed); Rotation2d.struct.pack(bb, value.angle); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java b/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java index 94ab7616d00..c51ef56c1f5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java @@ -9,24 +9,24 @@ /** Represents a pair of a pose and a curvature. */ public class PoseWithCurvature { /** Represents the pose. */ - public Pose2d poseMeters; + public Pose2d pose; - /** Represents the curvature. */ - public double curvatureRadPerMeter; + /** Represents the curvature in radians per meter. */ + public double curvature; /** * Constructs a PoseWithCurvature. * - * @param poseMeters The pose. - * @param curvatureRadPerMeter The curvature. + * @param pose The pose. + * @param curvature The curvature in radians per meter. */ - public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) { - this.poseMeters = poseMeters; - this.curvatureRadPerMeter = curvatureRadPerMeter; + public PoseWithCurvature(Pose2d pose, double curvature) { + this.pose = pose; + this.curvature = curvature; } /** Constructs a PoseWithCurvature with default values. */ public PoseWithCurvature() { - poseMeters = Pose2d.kZero; + pose = Pose2d.kZero; } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java index b08c8c78a8b..bc63d1a6726 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java @@ -126,7 +126,7 @@ public static List parameterize(Spline spline, double t0, dou throw new MalformedSplineException(kMalformedSplineExceptionMsg); } - final var twist = start.get().poseMeters.log(end.get().poseMeters); + final var twist = start.get().pose.log(end.get().pose); if (Math.abs(twist.dy) > kMaxDy || Math.abs(twist.dx) > kMaxDx || Math.abs(twist.dtheta) > kMaxDtheta) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java index b4afdf476af..978a2f3c115 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java @@ -20,13 +20,13 @@ private Discretization() { * * @param Num representing the number of states. * @param contA Continuous system matrix. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. * @return the discrete matrix system. */ public static Matrix discretizeA( - Matrix contA, double dtSeconds) { + Matrix contA, double dt) { // A_d = eᴬᵀ - return contA.times(dtSeconds).exp(); + return contA.times(dt).exp(); } /** @@ -36,12 +36,12 @@ public static Matrix discretizeA( * @param Nat representing the inputs to the system. * @param contA Continuous system matrix. * @param contB Continuous input matrix. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. * @return a Pair representing discA and diskB. */ public static Pair, Matrix> discretizeAB( - Matrix contA, Matrix contB, double dtSeconds) { + Matrix contA, Matrix contB, double dt) { int states = contA.getNumRows(); int inputs = contB.getNumCols(); @@ -53,7 +53,7 @@ Pair, Matrix> discretizeAB( // ϕ = eᴹᵀ = [A_d B_d] // [ 0 I ] - var phi = M.times(dtSeconds).exp(); + var phi = M.times(dt).exp(); var discA = new Matrix(new SimpleMatrix(states, states)); discA.extractFrom(0, 0, phi); @@ -70,12 +70,12 @@ Pair, Matrix> discretizeAB( * @param Nat representing the number of states. * @param contA Continuous system matrix. * @param contQ Continuous process noise covariance matrix. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. * @return a pair representing the discrete system matrix and process noise covariance matrix. */ public static Pair, Matrix> discretizeAQ( - Matrix contA, Matrix contQ, double dtSeconds) { + Matrix contA, Matrix contQ, double dt) { int states = contA.getNumRows(); // Make continuous Q symmetric if it isn't already @@ -91,7 +91,7 @@ Pair, Matrix> discretizeAQ( // ϕ = eᴹᵀ = [−A_d A_d⁻¹Q_d] // [ 0 A_dᵀ ] - final var phi = M.times(dtSeconds).exp(); + final var phi = M.times(dt).exp(); // ϕ₁₂ = A_d⁻¹Q_d final Matrix phi12 = phi.block(states, states, 0, states); @@ -115,11 +115,11 @@ Pair, Matrix> discretizeAQ( * * @param Nat representing the number of outputs. * @param contR Continuous measurement noise covariance matrix. - * @param dtSeconds Discretization timestep. + * @param dt Discretization timestep in seconds. * @return Discretized version of the provided continuous measurement noise covariance matrix. */ - public static Matrix discretizeR(Matrix contR, double dtSeconds) { + public static Matrix discretizeR(Matrix contR, double dt) { // R_d = 1/T R - return contR.div(dtSeconds); + return contR.div(dt); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java index 2b29d1954a4..00989e79d61 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java @@ -205,12 +205,12 @@ public double getD(int row, int col) { * * @param x The current state. * @param clampedU The control input. - * @param dtSeconds Timestep for model update. + * @param dt Timestep for model update in seconds. * @return the updated x. */ public Matrix calculateX( - Matrix x, Matrix clampedU, double dtSeconds) { - var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds); + Matrix x, Matrix clampedU, double dt) { + var discABpair = Discretization.discretizeAB(m_A, m_B, dt); return discABpair.getFirst().times(x).plus(discABpair.getSecond().times(clampedU)); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java index 78cace9a21d..08448c2d97f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java @@ -48,17 +48,17 @@ public class LinearSystemLoop plant, LinearQuadraticRegulator controller, KalmanFilter observer, double maxVoltageVolts, - double dtSeconds) { + double dt) { this( controller, - new LinearPlantInversionFeedforward<>(plant, dtSeconds), + new LinearPlantInversionFeedforward<>(plant, dt), observer, u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts)); } @@ -72,19 +72,15 @@ public LinearSystemLoop( * @param controller State-space controller. * @param observer State-space observer. * @param clampFunction The function used to clamp the input U. - * @param dtSeconds The nominal timestep. + * @param dt The nominal timestep in seconds. */ public LinearSystemLoop( LinearSystem plant, LinearQuadraticRegulator controller, KalmanFilter observer, Function, Matrix> clampFunction, - double dtSeconds) { - this( - controller, - new LinearPlantInversionFeedforward<>(plant, dtSeconds), - observer, - clampFunction); + double dt) { + this(controller, new LinearPlantInversionFeedforward<>(plant, dt), observer, clampFunction); } /** @@ -327,15 +323,15 @@ public void correct(Matrix y) { * *

After calling this, the user should send the elements of u to the actuators. * - * @param dtSeconds Timestep for model update. + * @param dt Timestep for model update in seconds. */ - public void predict(double dtSeconds) { + public void predict(double dt) { var u = clampInput( m_controller .calculate(getObserver().getXhat(), m_nextR) .plus(m_feedforward.calculate(m_nextR))); - getObserver().predict(u, dtSeconds); + getObserver().predict(u, dt); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java index a54e2eae448..3eb6dda8710 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java @@ -23,11 +23,11 @@ private NumericalIntegration() { * * @param f The function to integrate, which takes one argument x. * @param x The initial value of x. - * @param dtSeconds The time over which to integrate. + * @param dt The time over which to integrate in seconds. * @return the integration of dx/dt = f(x) for dt. */ - public static double rk4(DoubleUnaryOperator f, double x, double dtSeconds) { - final var h = dtSeconds; + public static double rk4(DoubleUnaryOperator f, double x, double dt) { + final var h = dt; 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); @@ -42,11 +42,11 @@ public static double rk4(DoubleUnaryOperator f, double x, double dtSeconds) { * @param f The function to integrate. It must take two arguments x and u. * @param x The initial value of x. * @param u The value u held constant over the integration period. - * @param dtSeconds The time over which to integrate. + * @param dt The time over which to integrate in seconds. * @return The result of Runge Kutta integration (4th order). */ - public static double rk4(DoubleBinaryOperator f, double x, double u, double dtSeconds) { - final var h = dtSeconds; + public static double rk4(DoubleBinaryOperator f, double x, double u, double dt) { + final var h = dt; final var k1 = f.applyAsDouble(x, u); final var k2 = f.applyAsDouble(x + h * k1 * 0.5, u); @@ -64,15 +64,15 @@ public static double rk4(DoubleBinaryOperator f, double x, double u, double dtSe * @param f The function to integrate. It must take two arguments x and u. * @param x The initial value of x. * @param u The value u held constant over the integration period. - * @param dtSeconds The time over which to integrate. + * @param dt The time over which to integrate in seconds. * @return the integration of dx/dt = f(x, u) for dt. */ public static Matrix rk4( BiFunction, Matrix, Matrix> f, Matrix x, Matrix u, - double dtSeconds) { - final var h = dtSeconds; + double dt) { + final var h = dt; Matrix k1 = f.apply(x, u); Matrix k2 = f.apply(x.plus(k1.times(h * 0.5)), u); @@ -88,12 +88,12 @@ public static Matrix rk4( * @param A Num representing the states of the system. * @param f The function to integrate. It must take one argument x. * @param x The initial value of x. - * @param dtSeconds The time over which to integrate. + * @param dt The time over which to integrate in seconds. * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt. */ public static Matrix rk4( - UnaryOperator> f, Matrix x, double dtSeconds) { - final var h = dtSeconds; + UnaryOperator> f, Matrix x, double dt) { + final var h = dt; Matrix k1 = f.apply(x); Matrix k2 = f.apply(x.plus(k1.times(h * 0.5))); @@ -111,20 +111,20 @@ public static Matrix rk4( * @param f The function to integrate. It must take two arguments t and y. * @param t The initial value of t. * @param y The initial value of y. - * @param dtSeconds The time over which to integrate. + * @param dt The time over which to integrate in seconds. * @return the integration of dx/dt = f(x) for dt. */ public static Matrix rk4( BiFunction, Matrix> f, double t, Matrix y, - double dtSeconds) { - final var h = dtSeconds; + double dt) { + final var h = dt; Matrix k1 = f.apply(t, y); - Matrix k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5))); - Matrix k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5))); - Matrix k4 = f.apply(t + dtSeconds, y.plus(k3.times(h))); + Matrix k2 = f.apply(t + dt * 0.5, y.plus(k1.times(h * 0.5))); + Matrix k3 = f.apply(t + dt * 0.5, y.plus(k2.times(h * 0.5))); + Matrix k4 = f.apply(t + dt, y.plus(k3.times(h))); return y.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0)); } @@ -138,15 +138,15 @@ public static Matrix rk4( * @param f The function to integrate. It must take two arguments x and u. * @param x The initial value of x. * @param u The value u held constant over the integration period. - * @param dtSeconds The time over which to integrate. + * @param dt The time over which to integrate in seconds. * @return the integration of dx/dt = f(x, u) for dt. */ public static Matrix rkdp( BiFunction, Matrix, Matrix> f, Matrix x, Matrix u, - double dtSeconds) { - return rkdp(f, x, u, dtSeconds, 1e-6); + double dt) { + return rkdp(f, x, u, dt, 1e-6); } /** @@ -157,7 +157,7 @@ public static Matrix rkdp( * @param f The function to integrate. It must take two arguments x and u. * @param x The initial value of x. * @param u The value u held constant over the integration period. - * @param dtSeconds The time over which to integrate. + * @param dt The time over which to integrate in seconds. * @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. */ @@ -165,7 +165,7 @@ public static Matrix rkdp( BiFunction, Matrix, Matrix> f, Matrix x, Matrix u, - double dtSeconds, + double dt, double maxError) { // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the // Butcher tableau the following arrays came from. @@ -200,13 +200,13 @@ public static Matrix rkdp( double truncationError; double dtElapsed = 0.0; - double h = dtSeconds; + double h = dt; // Loop until we've gotten to our desired dt - while (dtElapsed < dtSeconds) { + while (dtElapsed < dt) { do { // Only allow us to advance up to the dt remaining - h = Math.min(h, dtSeconds - dtElapsed); + h = Math.min(h, dt - dtElapsed); var k1 = f.apply(x, u); var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u); @@ -260,7 +260,7 @@ public static Matrix rkdp( .normF(); if (truncationError == 0.0) { - h = dtSeconds - dtElapsed; + h = dt - dtElapsed; } else { h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0); } @@ -281,7 +281,7 @@ public static Matrix rkdp( * @param f The function to integrate. It must take two arguments t and y. * @param t The initial value of t. * @param y The initial value of y. - * @param dtSeconds The time over which to integrate. + * @param dt The time over which to integrate in seconds. * @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. */ @@ -289,7 +289,7 @@ public static Matrix rkdp( BiFunction, Matrix> f, double t, Matrix y, - double dtSeconds, + double dt, double maxError) { // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the // Butcher tableau the following arrays came from. @@ -327,13 +327,13 @@ public static Matrix rkdp( double truncationError; double dtElapsed = 0.0; - double h = dtSeconds; + double h = dt; // Loop until we've gotten to our desired dt - while (dtElapsed < dtSeconds) { + while (dtElapsed < dt) { do { // Only allow us to advance up to the dt remaining - h = Math.min(h, dtSeconds - dtElapsed); + h = Math.min(h, dt - dtElapsed); var k1 = f.apply(t, y); var k2 = f.apply(t + h * c[0], y.plus(k1.times(A[0][0]).times(h))); @@ -387,7 +387,7 @@ public static Matrix rkdp( .normF(); if (truncationError == 0.0) { - h = dtSeconds - dtElapsed; + h = dt - dtElapsed; } else { h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java index de8346d7ddb..9fb001221ca 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java @@ -13,28 +13,28 @@ /** 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; + public final double nominalVoltage; - /** Torque when stalled. */ - public final double stallTorqueNewtonMeters; + /** Torque when stalled in Newton-meters. */ + public final double stallTorque; - /** Current draw when stalled. */ - public final double stallCurrentAmps; + /** Current draw when stalled in amps. */ + public final double stallCurrent; - /** Current draw under no load. */ - public final double freeCurrentAmps; + /** Current draw under no load in amps. */ + public final double freeCurrent; - /** Angular velocity under no load. */ - public final double freeSpeedRadPerSec; + /** Angular velocity under no load in radians per second. */ + public final double freeSpeed; - /** Motor internal resistance. */ - public final double rOhms; + /** Motor internal resistance in Ohms. */ + public final double R; - /** Motor velocity constant. */ - public final double KvRadPerSecPerVolt; + /** Motor velocity constant in (rad/s)/V. */ + public final double Kv; - /** Motor torque constant. */ - public final double KtNMPerAmp; + /** Motor torque constant in Newton-meters per amp. */ + public final double Kt; /** DCMotor protobuf for serialization. */ public static final DCMotorProto proto = new DCMotorProto(); @@ -45,84 +45,82 @@ public class DCMotor implements ProtobufSerializable, StructSerializable { /** * Constructs a DC motor. * - * @param nominalVoltageVolts Voltage at which the motor constants were measured. - * @param stallTorqueNewtonMeters Torque when stalled. - * @param stallCurrentAmps Current draw when stalled. - * @param freeCurrentAmps Current draw under no load. - * @param freeSpeedRadPerSec Angular velocity under no load. + * @param nominalVoltage Voltage at which the motor constants were measured. + * @param stallTorque Torque when stalled. + * @param stallCurrent Current draw when stalled. + * @param freeCurrent Current draw under no load. + * @param freeSpeed Angular velocity under no load. * @param numMotors Number of motors in a gearbox. */ public DCMotor( - double nominalVoltageVolts, - double stallTorqueNewtonMeters, - double stallCurrentAmps, - double freeCurrentAmps, - double freeSpeedRadPerSec, + double nominalVoltage, + double stallTorque, + double stallCurrent, + double freeCurrent, + double freeSpeed, int numMotors) { - this.nominalVoltageVolts = nominalVoltageVolts; - this.stallTorqueNewtonMeters = stallTorqueNewtonMeters * numMotors; - this.stallCurrentAmps = stallCurrentAmps * numMotors; - this.freeCurrentAmps = freeCurrentAmps * numMotors; - this.freeSpeedRadPerSec = freeSpeedRadPerSec; - - this.rOhms = nominalVoltageVolts / this.stallCurrentAmps; - this.KvRadPerSecPerVolt = - freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps); - this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps; + this.nominalVoltage = nominalVoltage; + this.stallTorque = stallTorque * numMotors; + this.stallCurrent = stallCurrent * numMotors; + this.freeCurrent = freeCurrent * numMotors; + this.freeSpeed = freeSpeed; + + this.R = nominalVoltage / this.stallCurrent; + this.Kv = freeSpeed / (nominalVoltage - R * this.freeCurrent); + this.Kt = this.stallTorque / this.stallCurrent; } /** * Calculate current drawn by motor with given speed and input voltage. * - * @param speedRadiansPerSec The current angular velocity of the motor. - * @param voltageInputVolts The voltage being applied to the motor. + * @param speed The current angular velocity of the motor. + * @param voltageInput The voltage being applied to the motor. * @return The estimated current. */ - public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) { - return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts; + public double getCurrent(double speed, double voltageInput) { + return -1.0 / Kv / R * speed + 1.0 / R * voltageInput; } /** * Calculate current drawn by motor for a given torque. * - * @param torqueNm The torque produced by the motor. + * @param torque The torque produced by the motor in Newton-meters. * @return The current drawn by the motor. */ - public double getCurrent(double torqueNm) { - return torqueNm / KtNMPerAmp; + public double getCurrent(double torque) { + return torque / Kt; } /** * Calculate torque produced by the motor with a given current. * - * @param currentAmpere The current drawn by the motor. - * @return The torque output. + * @param current The current drawn by the motor in amps. + * @return The torque output in Newton-meters. */ - public double getTorque(double currentAmpere) { - return currentAmpere * KtNMPerAmp; + public double getTorque(double current) { + return current * Kt; } /** * Calculate the voltage provided to the motor for a given torque and angular velocity. * - * @param torqueNm The torque produced by the motor. - * @param speedRadiansPerSec The current angular velocity of the motor. + * @param torque The torque produced by the motor in Newton-meters. + * @param speed The current angular velocity of the motor in radians per second. * @return The voltage of the motor. */ - public double getVoltage(double torqueNm, double speedRadiansPerSec) { - return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm; + public double getVoltage(double torque, double speed) { + return 1.0 / Kv * speed + 1.0 / Kt * R * torque; } /** * Calculates the angular speed produced by the motor at a given torque and input voltage. * - * @param torqueNm The torque produced by the motor. - * @param voltageInputVolts The voltage applied to the motor. + * @param torque The torque produced by the motor in Newton-meters. + * @param voltageInput The voltage applied to the motor. * @return The angular speed of the motor. */ - public double getSpeed(double torqueNm, double voltageInputVolts) { - return voltageInputVolts * KvRadPerSecPerVolt - - 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt; + public double getSpeed(double torque, double voltageInput) { + return voltageInput * Kv - 1.0 / Kt * torque * R * Kv; } /** @@ -133,11 +131,11 @@ public double getSpeed(double torqueNm, double voltageInputVolts) { */ public DCMotor withReduction(double gearboxReduction) { return new DCMotor( - nominalVoltageVolts, - stallTorqueNewtonMeters * gearboxReduction, - stallCurrentAmps, - freeCurrentAmps, - freeSpeedRadPerSec / gearboxReduction, + nominalVoltage, + stallTorque * gearboxReduction, + stallCurrent, + freeCurrent, + freeSpeed / gearboxReduction, 1); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index ce1129b059a..fd2e70a1b0d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -23,19 +23,19 @@ private LinearSystemId() { * velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ. * * @param motor The motor (or gearbox) attached to the carriage. - * @param massKg The mass of the elevator carriage, in kilograms. - * @param radiusMeters The radius of the elevator's driving drum, in meters. + * @param mass The mass of the elevator carriage, in kilograms. + * @param radius The radius of the elevator's driving drum, in meters. * @param gearing The reduction between motor and drum, as a ratio of output to input. * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if massKg <= 0, radiusMeters <= 0, or gearing <= 0. + * @throws IllegalArgumentException if mass <= 0, radius <= 0, or gearing <= 0. */ public static LinearSystem createElevatorSystem( - DCMotor motor, double massKg, double radiusMeters, double gearing) { - if (massKg <= 0.0) { - throw new IllegalArgumentException("massKg must be greater than zero."); + DCMotor motor, double mass, double radius, double gearing) { + if (mass <= 0.0) { + throw new IllegalArgumentException("mass must be greater than zero."); } - if (radiusMeters <= 0.0) { - throw new IllegalArgumentException("radiusMeters must be greater than zero."); + if (radius <= 0.0) { + throw new IllegalArgumentException("radius must be greater than zero."); } if (gearing <= 0) { throw new IllegalArgumentException("gearing must be greater than zero."); @@ -48,10 +48,8 @@ public static LinearSystem createElevatorSystem( 0, 1, 0, - -Math.pow(gearing, 2) - * motor.KtNMPerAmp - / (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)), - VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)), + -Math.pow(gearing, 2) * motor.Kt / (motor.R * radius * radius * mass * motor.Kv)), + VecBuilder.fill(0, gearing * motor.Kt / (motor.R * radius * mass)), Matrix.eye(Nat.N2()), new Matrix<>(Nat.N2(), Nat.N1())); } @@ -61,14 +59,14 @@ public static LinearSystem createElevatorSystem( * velocity], inputs are [voltage], and outputs are [angular velocity]. * * @param motor The motor (or gearbox) attached to the flywheel. - * @param JKgMetersSquared The moment of inertia J of the flywheel. + * @param J The moment of inertia J of the flywheel in kg-m². * @param gearing The reduction between motor and drum, as a ratio of output to input. * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if JKgMetersSquared <= 0 or gearing <= 0. + * @throws IllegalArgumentException if J <= 0 or gearing <= 0. */ public static LinearSystem createFlywheelSystem( - DCMotor motor, double JKgMetersSquared, double gearing) { - if (JKgMetersSquared <= 0.0) { + DCMotor motor, double J, double gearing) { + if (J <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } if (gearing <= 0.0) { @@ -76,12 +74,8 @@ public static LinearSystem createFlywheelSystem( } return new LinearSystem<>( - VecBuilder.fill( - -gearing - * gearing - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), - VecBuilder.fill(gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), + VecBuilder.fill(-gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)), + VecBuilder.fill(gearing * motor.Kt / (motor.R * J)), Matrix.eye(Nat.N1()), new Matrix<>(Nat.N1(), Nat.N1())); } @@ -92,14 +86,14 @@ public static LinearSystem createFlywheelSystem( * velocity]ᵀ. * * @param motor The motor (or gearbox) attached to system. - * @param JKgMetersSquared The moment of inertia J of the DC motor. + * @param J The moment of inertia J of the DC motor in kg-m². * @param gearing The reduction between motor and drum, as a ratio of output to input. * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if JKgMetersSquared <= 0 or gearing <= 0. + * @throws IllegalArgumentException if J <= 0 or gearing <= 0. */ public static LinearSystem createDCMotorSystem( - DCMotor motor, double JKgMetersSquared, double gearing) { - if (JKgMetersSquared <= 0.0) { + DCMotor motor, double J, double gearing) { + if (J <= 0.0) { throw new IllegalArgumentException("J must be greater than zero."); } if (gearing <= 0.0) { @@ -108,16 +102,8 @@ public static LinearSystem createDCMotorSystem( return new LinearSystem<>( MatBuilder.fill( - Nat.N2(), - Nat.N2(), - 0, - 1, - 0, - -gearing - * gearing - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)), - VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)), + Nat.N2(), Nat.N2(), 0, 1, 0, -gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)), + VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)), Matrix.eye(Nat.N2()), new Matrix<>(Nat.N2(), Nat.N1())); } @@ -161,46 +147,38 @@ public static LinearSystem createDCMotorSystem(double kV, double kA) * [left velocity, right velocity]ᵀ. * * @param motor The motor (or gearbox) driving the drivetrain. - * @param massKg The mass of the robot in kilograms. - * @param rMeters The radius of the wheels in meters. - * @param rbMeters The radius of the base (half the track width) in meters. - * @param JKgMetersSquared The moment of inertia of the robot. + * @param mass The mass of the robot in kilograms. + * @param r The radius of the wheels in meters. + * @param rb The radius of the base (half the trackwidth) in meters. + * @param J The moment of inertia of the robot in kg-m². * @param gearing The gearing reduction as output over input. * @return A LinearSystem representing a differential drivetrain. * @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or gearing * <= 0. */ public static LinearSystem createDrivetrainVelocitySystem( - DCMotor motor, - double massKg, - double rMeters, - double rbMeters, - double JKgMetersSquared, - double gearing) { - if (massKg <= 0.0) { - throw new IllegalArgumentException("massKg must be greater than zero."); + DCMotor motor, double mass, double r, double rb, double J, double gearing) { + if (mass <= 0.0) { + throw new IllegalArgumentException("mass must be greater than zero."); } - if (rMeters <= 0.0) { - throw new IllegalArgumentException("rMeters must be greater than zero."); + if (r <= 0.0) { + throw new IllegalArgumentException("r must be greater than zero."); } - if (rbMeters <= 0.0) { - throw new IllegalArgumentException("rbMeters must be greater than zero."); + if (rb <= 0.0) { + throw new IllegalArgumentException("rb must be greater than zero."); } - if (JKgMetersSquared <= 0.0) { - throw new IllegalArgumentException("JKgMetersSquared must be greater than zero."); + if (J <= 0.0) { + throw new IllegalArgumentException("J must be greater than zero."); } if (gearing <= 0.0) { throw new IllegalArgumentException("gearing must be greater than zero."); } - var C1 = - -(gearing * gearing) - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters); - var C2 = gearing * motor.KtNMPerAmp / (motor.rOhms * rMeters); + var C1 = -(gearing * gearing) * motor.Kt / (motor.Kv * motor.R * r * r); + var C2 = gearing * motor.Kt / (motor.R * r); - final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; - final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; + final double C3 = 1 / mass + rb * rb / J; + final double C4 = 1 / mass - rb * rb / J; var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1); var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2); var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); @@ -214,15 +192,15 @@ public static LinearSystem createDrivetrainVelocitySystem( * angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ. * * @param motor The motor (or gearbox) attached to the arm. - * @param JKgSquaredMeters The moment of inertia J of the arm. + * @param J The moment of inertia J of the arm in kg-m². * @param gearing The gearing between the motor and arm, in output over input. Most of the time * this will be greater than 1. * @return A LinearSystem representing the given characterized constants. */ public static LinearSystem createSingleJointedArmSystem( - DCMotor motor, double JKgSquaredMeters, double gearing) { - if (JKgSquaredMeters <= 0.0) { - throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero."); + DCMotor motor, double J, double gearing) { + if (J <= 0.0) { + throw new IllegalArgumentException("J must be greater than zero."); } if (gearing <= 0.0) { throw new IllegalArgumentException("gearing must be greater than zero."); @@ -235,10 +213,8 @@ public static LinearSystem createSingleJointedArmSystem( 0, 1, 0, - -Math.pow(gearing, 2) - * motor.KtNMPerAmp - / (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)), - VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)), + -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)), + VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)), Matrix.eye(Nat.N2()), new Matrix<>(Nat.N2(), Nat.N1())); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java index f9e4aa39576..39de80743cb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java @@ -38,10 +38,10 @@ public DCMotor unpack(ProtobufDCMotor msg) { @Override public void pack(ProtobufDCMotor msg, DCMotor value) { - msg.setNominalVoltage(value.nominalVoltageVolts); - msg.setStallTorque(value.stallTorqueNewtonMeters); - msg.setStallCurrent(value.stallCurrentAmps); - msg.setFreeCurrent(value.freeCurrentAmps); - msg.setFreeSpeed(value.freeSpeedRadPerSec); + msg.setNominalVoltage(value.nominalVoltage); + msg.setStallTorque(value.stallTorque); + msg.setStallCurrent(value.stallCurrent); + msg.setFreeCurrent(value.freeCurrent); + msg.setFreeSpeed(value.freeSpeed); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java index a55264b966a..649a3cabaee 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java @@ -42,10 +42,10 @@ public DCMotor unpack(ByteBuffer bb) { @Override public void pack(ByteBuffer bb, DCMotor value) { - bb.putDouble(value.nominalVoltageVolts); - bb.putDouble(value.stallTorqueNewtonMeters); - bb.putDouble(value.stallCurrentAmps); - bb.putDouble(value.freeCurrentAmps); - bb.putDouble(value.freeSpeedRadPerSec); + bb.putDouble(value.nominalVoltage); + bb.putDouble(value.stallTorque); + bb.putDouble(value.stallCurrent); + bb.putDouble(value.freeCurrent); + bb.putDouble(value.freeSpeed); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java index 1222d885b86..bea2261febd 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java @@ -23,13 +23,13 @@ public class Trajectory implements ProtobufSerializable { /** Trajectory protobuf for serialization. */ public static final TrajectoryProto proto = new TrajectoryProto(); - private final double m_totalTimeSeconds; + private final double m_totalTime; private final List m_states; /** Constructs an empty trajectory. */ public Trajectory() { m_states = new ArrayList<>(); - m_totalTimeSeconds = 0.0; + m_totalTime = 0.0; } /** @@ -45,7 +45,7 @@ public Trajectory(final List states) { throw new IllegalArgumentException("Trajectory manually created with no states."); } - m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds; + m_totalTime = m_states.get(m_states.size() - 1).time; } /** @@ -78,16 +78,16 @@ private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) { * @return The initial pose of the trajectory. */ public Pose2d getInitialPose() { - return sample(0).poseMeters; + return sample(0).pose; } /** * Returns the overall duration of the trajectory. * - * @return The duration of the trajectory. + * @return The duration of the trajectory in seconds. */ - public double getTotalTimeSeconds() { - return m_totalTimeSeconds; + public double getTotalTime() { + return m_totalTime; } /** @@ -102,19 +102,19 @@ public List getStates() { /** * Sample the trajectory at a point in time. * - * @param timeSeconds The point in time since the beginning of the trajectory to sample. + * @param time The point in time since the beginning of the trajectory to sample in seconds. * @return The state at that point in time. * @throws IllegalStateException if the trajectory has no states. */ - public State sample(double timeSeconds) { + public State sample(double time) { if (m_states.isEmpty()) { throw new IllegalStateException("Trajectory cannot be sampled if it has no states."); } - if (timeSeconds <= m_states.get(0).timeSeconds) { + if (time <= m_states.get(0).time) { return m_states.get(0); } - if (timeSeconds >= m_totalTimeSeconds) { + if (time >= m_totalTime) { return m_states.get(m_states.size() - 1); } @@ -129,7 +129,7 @@ public State sample(double timeSeconds) { while (low != high) { int mid = (low + high) / 2; - if (m_states.get(mid).timeSeconds < timeSeconds) { + if (m_states.get(mid).time < time) { // This index and everything under it are less than the requested // timestamp. Therefore, we can discard them. low = mid + 1; @@ -150,13 +150,12 @@ public State sample(double timeSeconds) { final State prevSample = m_states.get(low - 1); // If the difference in states is negligible, then we are spot on! - if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) { + if (Math.abs(sample.time - prevSample.time) < 1E-9) { return sample; } // Interpolate between the two states for the state that we want. return prevSample.interpolate( - sample, - (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds)); + sample, (time - prevSample.time) / (sample.time - prevSample.time)); } /** @@ -169,7 +168,7 @@ public State sample(double timeSeconds) { */ public Trajectory transformBy(Transform2d transform) { var firstState = m_states.get(0); - var firstPose = firstState.poseMeters; + var firstPose = firstState.pose; // Calculate the transformed first pose. var newFirstPose = firstPose.plus(transform); @@ -177,22 +176,22 @@ public Trajectory transformBy(Transform2d transform) { newStates.add( new State( - firstState.timeSeconds, - firstState.velocityMetersPerSecond, - firstState.accelerationMetersPerSecondSq, + firstState.time, + firstState.velocity, + firstState.acceleration, newFirstPose, - firstState.curvatureRadPerMeter)); + firstState.curvature)); for (int i = 1; i < m_states.size(); i++) { var state = m_states.get(i); // We are transforming relative to the coordinate frame of the new initial pose. newStates.add( new State( - state.timeSeconds, - state.velocityMetersPerSecond, - state.accelerationMetersPerSecondSq, - newFirstPose.plus(state.poseMeters.minus(firstPose)), - state.curvatureRadPerMeter)); + state.time, + state.velocity, + state.acceleration, + newFirstPose.plus(state.pose.minus(firstPose)), + state.curvature)); } return new Trajectory(newStates); @@ -212,11 +211,11 @@ public Trajectory relativeTo(Pose2d pose) { .map( state -> new State( - state.timeSeconds, - state.velocityMetersPerSecond, - state.accelerationMetersPerSecondSq, - state.poseMeters.relativeTo(pose), - state.curvatureRadPerMeter)) + state.time, + state.velocity, + state.acceleration, + state.pose.relativeTo(pose), + state.curvature)) .collect(Collectors.toList())); } @@ -241,11 +240,11 @@ public Trajectory concatenate(Trajectory other) { .map( state -> new State( - state.timeSeconds, - state.velocityMetersPerSecond, - state.accelerationMetersPerSecondSq, - state.poseMeters, - state.curvatureRadPerMeter)) + state.time, + state.velocity, + state.acceleration, + state.pose, + state.curvature)) .collect(Collectors.toList()); // Here we omit the first state of the other trajectory because we don't want @@ -254,13 +253,7 @@ public Trajectory concatenate(Trajectory other) { // other trajectory. for (int i = 1; i < other.getStates().size(); ++i) { var s = other.getStates().get(i); - states.add( - new State( - s.timeSeconds + m_totalTimeSeconds, - s.velocityMetersPerSecond, - s.accelerationMetersPerSecondSq, - s.poseMeters, - s.curvatureRadPerMeter)); + states.add(new State(s.time + m_totalTime, s.velocity, s.acceleration, s.pose, s.curvature)); } return new Trajectory(states); } @@ -273,51 +266,46 @@ 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 in seconds. */ @JsonProperty("time") - public double timeSeconds; + public double time; - /** The speed at that point of the trajectory. */ + /** The speed at that point of the trajectory in meters per second. */ @JsonProperty("velocity") - public double velocityMetersPerSecond; + public double velocity; - /** The acceleration at that point of the trajectory. */ + /** The acceleration at that point of the trajectory in m/s². */ @JsonProperty("acceleration") - public double accelerationMetersPerSecondSq; + public double acceleration; /** The pose at that point of the trajectory. */ @JsonProperty("pose") - public Pose2d poseMeters; + public Pose2d pose; - /** The curvature at that point of the trajectory. */ + /** The curvature at that point of the trajectory in rad/m. */ @JsonProperty("curvature") - public double curvatureRadPerMeter; + public double curvature; /** Default constructor. */ public State() { - poseMeters = Pose2d.kZero; + pose = Pose2d.kZero; } /** * Constructs a State with the specified parameters. * - * @param timeSeconds The time elapsed since the beginning of the trajectory. - * @param velocityMetersPerSecond The speed at that point of the trajectory. - * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory. - * @param poseMeters The pose at that point of the trajectory. - * @param curvatureRadPerMeter The curvature at that point of the trajectory. + * @param time The time elapsed since the beginning of the trajectory in seconds. + * @param velocity The speed at that point of the trajectory in m/s. + * @param acceleration The acceleration at that point of the trajectory in m/s². + * @param pose The pose at that point of the trajectory. + * @param curvature The curvature at that point of the trajectory in rad/m. */ - public State( - double timeSeconds, - double velocityMetersPerSecond, - double accelerationMetersPerSecondSq, - Pose2d poseMeters, - double curvatureRadPerMeter) { - this.timeSeconds = timeSeconds; - this.velocityMetersPerSecond = velocityMetersPerSecond; - this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq; - this.poseMeters = poseMeters; - this.curvatureRadPerMeter = curvatureRadPerMeter; + public State(double time, double velocity, double acceleration, Pose2d pose, double curvature) { + this.time = time; + this.velocity = velocity; + this.acceleration = acceleration; + this.pose = pose; + this.curvature = curvature; } /** @@ -329,10 +317,10 @@ public State( */ State interpolate(State endValue, double i) { // Find the new t value. - final double newT = lerp(timeSeconds, endValue.timeSeconds, i); + final double newT = lerp(time, endValue.time, i); // Find the delta time between the current state and the interpolated state. - final double deltaT = newT - timeSeconds; + final double deltaT = newT - time; // If delta time is negative, flip the order of interpolation. if (deltaT < 0) { @@ -340,72 +328,59 @@ State interpolate(State endValue, double i) { } // Check whether the robot is reversing at this stage. - final boolean reversing = - velocityMetersPerSecond < 0 - || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0; + final boolean reversing = velocity < 0 || Math.abs(velocity) < 1E-9 && acceleration < 0; // Calculate the new velocity // v_f = v_0 + at - final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT); + final double newV = velocity + (acceleration * deltaT); // Calculate the change in position. // delta_s = v_0 t + 0.5at² final double newS = - (velocityMetersPerSecond * deltaT - + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) - * (reversing ? -1.0 : 1.0); + (velocity * deltaT + 0.5 * acceleration * Math.pow(deltaT, 2)) * (reversing ? -1.0 : 1.0); // Return the new state. To find the new position for the new state, we need // to interpolate between the two endpoint poses. The fraction for // interpolation is the change in position (delta s) divided by the total // distance between the two endpoints. final double interpolationFrac = - newS / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation()); + newS / endValue.pose.getTranslation().getDistance(pose.getTranslation()); return new State( newT, newV, - accelerationMetersPerSecondSq, - lerp(poseMeters, endValue.poseMeters, interpolationFrac), - lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)); + acceleration, + lerp(pose, endValue.pose, interpolationFrac), + lerp(curvature, endValue.curvature, interpolationFrac)); } @Override public String toString() { return String.format( "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)", - timeSeconds, - velocityMetersPerSecond, - accelerationMetersPerSecondSq, - poseMeters, - curvatureRadPerMeter); + time, velocity, acceleration, pose, curvature); } @Override public boolean equals(Object obj) { return obj instanceof State state - && Double.compare(state.timeSeconds, timeSeconds) == 0 - && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0 - && Double.compare(state.accelerationMetersPerSecondSq, accelerationMetersPerSecondSq) == 0 - && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0 - && Objects.equals(poseMeters, state.poseMeters); + && Double.compare(state.time, time) == 0 + && Double.compare(state.velocity, velocity) == 0 + && Double.compare(state.acceleration, acceleration) == 0 + && Double.compare(state.curvature, curvature) == 0 + && Objects.equals(pose, state.pose); } @Override public int hashCode() { - return Objects.hash( - timeSeconds, - velocityMetersPerSecond, - accelerationMetersPerSecondSq, - poseMeters, - curvatureRadPerMeter); + return Objects.hash(time, velocity, acceleration, pose, curvature); } } @Override public String toString() { String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n")); - return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList); + return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTime, stateList); } @Override diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java index 1bcb15f8b56..c93330ede9d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java @@ -38,21 +38,20 @@ public class TrajectoryConfig { /** * Constructs the trajectory configuration class. * - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. + * @param maxVelocity The max velocity for the trajectory in m/s. + * @param maxAcceleration The max acceleration for the trajectory in m/s². */ - public TrajectoryConfig( - double maxVelocityMetersPerSecond, double maxAccelerationMetersPerSecondSq) { - m_maxVelocity = maxVelocityMetersPerSecond; - m_maxAcceleration = maxAccelerationMetersPerSecondSq; + public TrajectoryConfig(double maxVelocity, double maxAcceleration) { + m_maxVelocity = maxVelocity; + m_maxAcceleration = maxAcceleration; m_constraints = new ArrayList<>(); } /** * Constructs the trajectory configuration class. * - * @param maxVelocity The max velocity for the trajectory. - * @param maxAcceleration The max acceleration for the trajectory. + * @param maxVelocity The max velocity for the trajectory in m/s. + * @param maxAcceleration The max acceleration for the trajectory in m/s². */ public TrajectoryConfig(LinearVelocity maxVelocity, LinearAcceleration maxAcceleration) { this(maxVelocity.in(MetersPerSecond), maxAcceleration.in(MetersPerSecondPerSecond)); @@ -119,7 +118,7 @@ public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) { /** * Returns the starting velocity of the trajectory. * - * @return The starting velocity of the trajectory. + * @return The starting velocity of the trajectory in m/s. */ public double getStartVelocity() { return m_startVelocity; @@ -128,11 +127,11 @@ public double getStartVelocity() { /** * Sets the start velocity of the trajectory. * - * @param startVelocityMetersPerSecond The start velocity of the trajectory. + * @param startVelocity The start velocity of the trajectory in m/s. * @return Instance of the current config object. */ - public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) { - m_startVelocity = startVelocityMetersPerSecond; + public TrajectoryConfig setStartVelocity(double startVelocity) { + m_startVelocity = startVelocity; return this; } @@ -149,7 +148,7 @@ public TrajectoryConfig setStartVelocity(LinearVelocity startVelocity) { /** * Returns the starting velocity of the trajectory. * - * @return The starting velocity of the trajectory. + * @return The starting velocity of the trajectory in m/s. */ public double getEndVelocity() { return m_endVelocity; @@ -158,11 +157,11 @@ public double getEndVelocity() { /** * Sets the end velocity of the trajectory. * - * @param endVelocityMetersPerSecond The end velocity of the trajectory. + * @param endVelocity The end velocity of the trajectory in m/s. * @return Instance of the current config object. */ - public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) { - m_endVelocity = endVelocityMetersPerSecond; + public TrajectoryConfig setEndVelocity(double endVelocity) { + m_endVelocity = endVelocity; return this; } @@ -179,7 +178,7 @@ public TrajectoryConfig setEndVelocity(LinearVelocity endVelocity) { /** * Returns the maximum velocity of the trajectory. * - * @return The maximum velocity of the trajectory. + * @return The maximum velocity of the trajectory in m/s. */ public double getMaxVelocity() { return m_maxVelocity; @@ -188,7 +187,7 @@ public double getMaxVelocity() { /** * Returns the maximum acceleration of the trajectory. * - * @return The maximum acceleration of the trajectory. + * @return The maximum acceleration of the trajectory in m/s². */ public double getMaxAcceleration() { return m_maxAcceleration; diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java index a17d5fb1bc1..393e150f1d9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java @@ -91,8 +91,8 @@ public static Trajectory generateTrajectory( // Change the points back to their original orientation. if (config.isReversed()) { for (var point : points) { - point.poseMeters = point.poseMeters.plus(kFlip); - point.curvatureRadPerMeter *= -1; + point.pose = point.pose.plus(kFlip); + point.curvature *= -1; } } @@ -167,8 +167,8 @@ public static Trajectory generateTrajectory( // Change the points back to their original orientation. if (config.isReversed()) { for (var point : points) { - point.poseMeters = point.poseMeters.plus(kFlip); - point.curvatureRadPerMeter *= -1; + point.pose = point.pose.plus(kFlip); + point.curvature *= -1; } } @@ -217,8 +217,8 @@ public static Trajectory generateTrajectory(List waypoints, TrajectoryCo // Change the points back to their original orientation. if (config.isReversed()) { for (var point : points) { - point.poseMeters = point.poseMeters.plus(kFlip); - point.curvatureRadPerMeter *= -1; + point.pose = point.pose.plus(kFlip); + point.curvature *= -1; } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java index 35b2a465056..405f41e800c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java @@ -46,10 +46,10 @@ private TrajectoryParameterizer() {} * * @param points Reference to the spline points. * @param constraints A vector of various velocity and acceleration. constraints. - * @param startVelocityMetersPerSecond The start velocity for the trajectory. - * @param endVelocityMetersPerSecond The end velocity for the trajectory. - * @param maxVelocityMetersPerSecond The max velocity for the trajectory. - * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory. + * @param startVelocity The start velocity for the trajectory in m/s. + * @param endVelocity The end velocity for the trajectory in m/s. + * @param maxVelocity The max velocity for the trajectory in m/s. + * @param maxAcceleration The max acceleration for the trajectory in m/s². * @param reversed Whether the robot should move backwards. Note that the robot will still move * from a -> b -> ... -> z as defined in the waypoints. * @return The trajectory. @@ -57,19 +57,14 @@ private TrajectoryParameterizer() {} public static Trajectory timeParameterizeTrajectory( List points, List constraints, - double startVelocityMetersPerSecond, - double endVelocityMetersPerSecond, - double maxVelocityMetersPerSecond, - double maxAccelerationMetersPerSecondSq, + double startVelocity, + double endVelocity, + double maxVelocity, + double maxAcceleration, boolean reversed) { var constrainedStates = new ArrayList(points.size()); var predecessor = - new ConstrainedState( - points.get(0), - 0, - startVelocityMetersPerSecond, - -maxAccelerationMetersPerSecondSq, - maxAccelerationMetersPerSecondSq); + new ConstrainedState(points.get(0), 0, startVelocity, -maxAcceleration, maxAcceleration); // Forward pass for (int i = 0; i < points.size(); i++) { @@ -81,36 +76,36 @@ public static Trajectory timeParameterizeTrajectory( double ds = constrainedState .pose - .poseMeters + .pose .getTranslation() - .getDistance(predecessor.pose.poseMeters.getTranslation()); - constrainedState.distanceMeters = predecessor.distanceMeters + ds; + .getDistance(predecessor.pose.pose.getTranslation()); + constrainedState.distance = predecessor.distance + ds; // We may need to iterate to find the maximum end velocity and common // acceleration, since acceleration limits may be a function of velocity. while (true) { // Enforce global max velocity and max reachable velocity by global // acceleration limit. v_f = √(v_i² + 2ad). - constrainedState.maxVelocityMetersPerSecond = + constrainedState.maxVelocity = Math.min( - maxVelocityMetersPerSecond, + maxVelocity, Math.sqrt( - predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond - + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0)); + predecessor.maxVelocity * predecessor.maxVelocity + + predecessor.maxAcceleration * ds * 2.0)); - constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq; - constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq; + constrainedState.minAcceleration = -maxAcceleration; + constrainedState.maxAcceleration = maxAcceleration; // At this point, the constrained state is fully constructed apart from // all the custom-defined user constraints. for (final var constraint : constraints) { - constrainedState.maxVelocityMetersPerSecond = + constrainedState.maxVelocity = Math.min( - constrainedState.maxVelocityMetersPerSecond, - constraint.getMaxVelocityMetersPerSecond( - constrainedState.pose.poseMeters, - constrainedState.pose.curvatureRadPerMeter, - constrainedState.maxVelocityMetersPerSecond)); + constrainedState.maxVelocity, + constraint.getMaxVelocity( + constrainedState.pose.pose, + constrainedState.pose.curvature, + constrainedState.maxVelocity)); } // Now enforce all acceleration limits. @@ -124,22 +119,19 @@ public static Trajectory timeParameterizeTrajectory( // acceleration that we applied, then we need to reduce the max // acceleration of the predecessor and try again. double actualAcceleration = - (constrainedState.maxVelocityMetersPerSecond - * constrainedState.maxVelocityMetersPerSecond - - predecessor.maxVelocityMetersPerSecond - * predecessor.maxVelocityMetersPerSecond) + (constrainedState.maxVelocity * constrainedState.maxVelocity + - predecessor.maxVelocity * predecessor.maxVelocity) / (ds * 2.0); // If we violate the max acceleration constraint, let's modify the // predecessor. - if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) { - predecessor.maxAccelerationMetersPerSecondSq = - constrainedState.maxAccelerationMetersPerSecondSq; + if (constrainedState.maxAcceleration < actualAcceleration - 1E-6) { + predecessor.maxAcceleration = constrainedState.maxAcceleration; } else { // Constrain the predecessor's max acceleration to the current // acceleration. - if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) { - predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration; + if (actualAcceleration > predecessor.minAcceleration) { + predecessor.maxAcceleration = actualAcceleration; } // If the actual acceleration is less than the predecessor's min // acceleration, it will be repaired in the backward pass. @@ -152,30 +144,30 @@ public static Trajectory timeParameterizeTrajectory( var successor = new ConstrainedState( points.get(points.size() - 1), - constrainedStates.get(constrainedStates.size() - 1).distanceMeters, - endVelocityMetersPerSecond, - -maxAccelerationMetersPerSecondSq, - maxAccelerationMetersPerSecondSq); + constrainedStates.get(constrainedStates.size() - 1).distance, + endVelocity, + -maxAcceleration, + maxAcceleration); // Backward pass for (int i = points.size() - 1; i >= 0; i--) { var constrainedState = constrainedStates.get(i); - double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative + double ds = constrainedState.distance - successor.distance; // negative while (true) { // Enforce max velocity limit (reverse) // v_f = √(v_i² + 2ad), where v_i = successor. double newMaxVelocity = Math.sqrt( - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond - + successor.minAccelerationMetersPerSecondSq * ds * 2.0); + successor.maxVelocity * successor.maxVelocity + + successor.minAcceleration * ds * 2.0); // No more limits to impose! This state can be finalized. - if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) { + if (newMaxVelocity >= constrainedState.maxVelocity) { break; } - constrainedState.maxVelocityMetersPerSecond = newMaxVelocity; + constrainedState.maxVelocity = newMaxVelocity; // Check all acceleration constraints with the new max velocity. enforceAccelerationLimits(reversed, constraints, constrainedState); @@ -188,16 +180,14 @@ public static Trajectory timeParameterizeTrajectory( // acceleration, then we need to lower the min acceleration of the // successor and try again. double actualAcceleration = - (constrainedState.maxVelocityMetersPerSecond - * constrainedState.maxVelocityMetersPerSecond - - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond) + (constrainedState.maxVelocity * constrainedState.maxVelocity + - successor.maxVelocity * successor.maxVelocity) / (ds * 2.0); - if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) { - successor.minAccelerationMetersPerSecondSq = - constrainedState.minAccelerationMetersPerSecondSq; + if (constrainedState.minAcceleration > actualAcceleration + 1E-6) { + successor.minAcceleration = constrainedState.minAcceleration; } else { - successor.minAccelerationMetersPerSecondSq = actualAcceleration; + successor.minAcceleration = actualAcceleration; break; } } @@ -207,52 +197,49 @@ public static Trajectory timeParameterizeTrajectory( // Now we can integrate the constrained states forward in time to obtain our // trajectory states. var states = new ArrayList(points.size()); - double timeSeconds = 0.0; - double distanceMeters = 0.0; - double velocityMetersPerSecond = 0.0; + double time = 0.0; + double distance = 0.0; + double velocity = 0.0; for (int i = 0; i < constrainedStates.size(); i++) { final var state = constrainedStates.get(i); // Calculate the change in position between the current state and the previous // state. - double ds = state.distanceMeters - distanceMeters; + double ds = state.distance - distance; // Calculate the acceleration between the current state and the previous // state. - double accel = - (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond - - velocityMetersPerSecond * velocityMetersPerSecond) - / (ds * 2); + double accel = (state.maxVelocity * state.maxVelocity - velocity * velocity) / (ds * 2); // Calculate dt double dt = 0.0; if (i > 0) { - states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel; + states.get(i - 1).acceleration = reversed ? -accel : accel; if (Math.abs(accel) > 1E-6) { // v_f = v_0 + a * t - dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel; - } else if (Math.abs(velocityMetersPerSecond) > 1E-6) { + dt = (state.maxVelocity - velocity) / accel; + } else if (Math.abs(velocity) > 1E-6) { // delta_x = v * t - dt = ds / velocityMetersPerSecond; + dt = ds / velocity; } else { throw new TrajectoryGenerationException( "Something went wrong at iteration " + i + " of time parameterization."); } } - velocityMetersPerSecond = state.maxVelocityMetersPerSecond; - distanceMeters = state.distanceMeters; + velocity = state.maxVelocity; + distance = state.distance; - timeSeconds += dt; + time += dt; states.add( new Trajectory.State( - timeSeconds, - reversed ? -velocityMetersPerSecond : velocityMetersPerSecond, + time, + reversed ? -velocity : velocity, reversed ? -accel : accel, - state.pose.poseMeters, - state.pose.curvatureRadPerMeter)); + state.pose.pose, + state.pose.curvature)); } return new Trajectory(states); @@ -263,51 +250,44 @@ private static void enforceAccelerationLimits( for (final var constraint : constraints) { double factor = reverse ? -1.0 : 1.0; final var minMaxAccel = - constraint.getMinMaxAccelerationMetersPerSecondSq( - state.pose.poseMeters, - state.pose.curvatureRadPerMeter, - state.maxVelocityMetersPerSecond * factor); + constraint.getMinMaxAcceleration( + state.pose.pose, state.pose.curvature, state.maxVelocity * factor); - if (minMaxAccel.minAccelerationMetersPerSecondSq - > minMaxAccel.maxAccelerationMetersPerSecondSq) { + if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) { throw new TrajectoryGenerationException( "Infeasible trajectory constraint: " + constraint.getClass().getName() + "\n"); } - state.minAccelerationMetersPerSecondSq = + state.minAcceleration = Math.max( - state.minAccelerationMetersPerSecondSq, - reverse - ? -minMaxAccel.maxAccelerationMetersPerSecondSq - : minMaxAccel.minAccelerationMetersPerSecondSq); + state.minAcceleration, + reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration); - state.maxAccelerationMetersPerSecondSq = + state.maxAcceleration = Math.min( - state.maxAccelerationMetersPerSecondSq, - reverse - ? -minMaxAccel.minAccelerationMetersPerSecondSq - : minMaxAccel.maxAccelerationMetersPerSecondSq); + state.maxAcceleration, + reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration); } } private static class ConstrainedState { PoseWithCurvature pose; - double distanceMeters; - double maxVelocityMetersPerSecond; - double minAccelerationMetersPerSecondSq; - double maxAccelerationMetersPerSecondSq; + double distance; + double maxVelocity; + double minAcceleration; + double maxAcceleration; ConstrainedState( PoseWithCurvature pose, - double distanceMeters, - double maxVelocityMetersPerSecond, - double minAccelerationMetersPerSecondSq, - double maxAccelerationMetersPerSecondSq) { + double distance, + double maxVelocity, + double minAcceleration, + double maxAcceleration) { this.pose = pose; - this.distanceMeters = distanceMeters; - this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond; - this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq; - this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq; + this.distance = distance; + this.maxVelocity = maxVelocity; + this.minAcceleration = minAcceleration; + this.maxAcceleration = maxAcceleration; } ConstrainedState() { diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java index e258e9d4f36..c710cdbe22a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java @@ -58,13 +58,13 @@ private static double[] getElementsFromTrajectory(Trajectory trajectory) { for (int i = 0; i < trajectory.getStates().size() * 7; i += 7) { var state = trajectory.getStates().get(i / 7); - elements[i] = state.timeSeconds; - elements[i + 1] = state.velocityMetersPerSecond; - elements[i + 2] = state.accelerationMetersPerSecondSq; - elements[i + 3] = state.poseMeters.getX(); - elements[i + 4] = state.poseMeters.getY(); - elements[i + 5] = state.poseMeters.getRotation().getRadians(); - elements[i + 6] = state.curvatureRadPerMeter; + elements[i] = state.time; + elements[i + 1] = state.velocity; + elements[i + 2] = state.acceleration; + elements[i + 3] = state.pose.getX(); + elements[i + 4] = state.pose.getY(); + elements[i + 5] = state.pose.getRotation().getRadians(); + elements[i + 6] = state.curvature; } return elements; } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java index 7a47fcac4c1..61e505c2506 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java @@ -15,29 +15,28 @@ * around tight turns, making it easier to track trajectories with sharp turns. */ public class CentripetalAccelerationConstraint implements TrajectoryConstraint { - private final double m_maxCentripetalAccelerationMetersPerSecondSq; + private final double m_maxCentripetalAcceleration; /** * Constructs a centripetal acceleration constraint. * - * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration. + * @param maxCentripetalAcceleration The max centripetal acceleration in m/s². */ - public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) { - m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq; + public CentripetalAccelerationConstraint(double maxCentripetalAcceleration) { + m_maxCentripetalAcceleration = maxCentripetalAcceleration; } /** * Returns the max velocity given the current pose and curvature. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The velocity at the current point in the trajectory before - * constraints are applied. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory in rad/m. + * @param velocity The velocity at the current point in the trajectory before constraints are + * applied in m/s. * @return The absolute maximum velocity. */ @Override - public double getMaxVelocityMetersPerSecond( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { // ac = v²/r // k (curvature) = 1/r @@ -45,22 +44,20 @@ public double getMaxVelocityMetersPerSecond( // ac/k = v² // v = √(ac/k) - return Math.sqrt( - m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter)); + return Math.sqrt(m_maxCentripetalAcceleration / Math.abs(curvature)); } /** * Returns the minimum and maximum allowable acceleration for the trajectory given pose, * curvature, and speed. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory in rad/m. + * @param velocity The speed at the current point in the trajectory in m/s. * @return The min and max acceleration bounds. */ @Override - public MinMax getMinMaxAccelerationMetersPerSecondSq( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { // The acceleration of the robot has no impact on the centripetal acceleration // of the robot. return new MinMax(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java index 21644359544..ca7efe4993f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java @@ -14,58 +14,54 @@ * drivetrain stay below a certain limit. */ public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint { - private final double m_maxSpeedMetersPerSecond; + private final double m_maxSpeed; private final DifferentialDriveKinematics m_kinematics; /** * Constructs a differential drive dynamics constraint. * * @param kinematics A kinematics component describing the drive geometry. - * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at. + * @param maxSpeed The max speed that a side of the robot can travel at in m/s. */ public DifferentialDriveKinematicsConstraint( - final DifferentialDriveKinematics kinematics, double maxSpeedMetersPerSecond) { - m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond; + final DifferentialDriveKinematics kinematics, double maxSpeed) { + m_maxSpeed = maxSpeed; m_kinematics = kinematics; } /** * Returns the max velocity given the current pose and curvature. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The velocity at the current point in the trajectory before - * constraints are applied. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory in rad/m. + * @param velocity The velocity at the current point in the trajectory before constraints are + * applied in m/s. * @return The absolute maximum velocity. */ @Override - public double getMaxVelocityMetersPerSecond( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { // Create an object to represent the current chassis speeds. - var chassisSpeeds = - new ChassisSpeeds( - velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter); + var chassisSpeeds = new ChassisSpeeds(velocity, 0, velocity * curvature); // Get the wheel speeds and normalize them to within the max velocity. var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); - wheelSpeeds.desaturate(m_maxSpeedMetersPerSecond); + wheelSpeeds.desaturate(m_maxSpeed); // Return the new linear chassis speed. - return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond; + return m_kinematics.toChassisSpeeds(wheelSpeeds).vx; } /** * Returns the minimum and maximum allowable acceleration for the trajectory given pose, * curvature, and speed. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory in rad/m. + * @param velocity The speed at the current point in the trajectory in m/s. * @return The min and max acceleration bounds. */ @Override - public MinMax getMinMaxAccelerationMetersPerSecondSq( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { return new MinMax(); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java index 0cc6bc54bc6..9927fc1f268 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java @@ -43,23 +43,17 @@ public DifferentialDriveVoltageConstraint( } @Override - public double getMaxVelocityMetersPerSecond( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { return Double.POSITIVE_INFINITY; } @Override - public MinMax getMinMaxAccelerationMetersPerSecondSq( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { var wheelSpeeds = - m_kinematics.toWheelSpeeds( - new ChassisSpeeds( - velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter)); + m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocity, 0, velocity * curvature)); - double maxWheelSpeed = - Math.max(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond); - double minWheelSpeed = - Math.min(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond); + double maxWheelSpeed = Math.max(wheelSpeeds.left, wheelSpeeds.right); + double minWheelSpeed = Math.min(wheelSpeeds.left, wheelSpeeds.right); // Calculate maximum/minimum possible accelerations from motor dynamics // and max/min wheel speeds @@ -87,28 +81,18 @@ public MinMax getMinMaxAccelerationMetersPerSecondSq( double maxChassisAcceleration; double minChassisAcceleration; - if (velocityMetersPerSecond == 0) { + if (velocity == 0) { maxChassisAcceleration = - maxWheelAcceleration - / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2); + maxWheelAcceleration / (1 + m_kinematics.trackwidth * Math.abs(curvature) / 2); minChassisAcceleration = - minWheelAcceleration - / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2); + minWheelAcceleration / (1 + m_kinematics.trackwidth * Math.abs(curvature) / 2); } else { maxChassisAcceleration = maxWheelAcceleration - / (1 - + m_kinematics.trackWidthMeters - * Math.abs(curvatureRadPerMeter) - * Math.signum(velocityMetersPerSecond) - / 2); + / (1 + m_kinematics.trackwidth * Math.abs(curvature) * Math.signum(velocity) / 2); minChassisAcceleration = minWheelAcceleration - / (1 - - m_kinematics.trackWidthMeters - * Math.abs(curvatureRadPerMeter) - * Math.signum(velocityMetersPerSecond) - / 2); + / (1 - m_kinematics.trackwidth * Math.abs(curvature) * Math.signum(velocity) / 2); } // When turning about a point inside the wheelbase (i.e. radius less than half @@ -116,10 +100,10 @@ public MinMax getMinMaxAccelerationMetersPerSecondSq( // the same. The formula above changes sign for the inner wheel when this happens. // We can accurately account for this by simply negating the inner wheel. - if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) { - if (velocityMetersPerSecond > 0) { + if ((m_kinematics.trackwidth / 2) > (1 / Math.abs(curvature))) { + if (velocity > 0) { minChassisAcceleration = -minChassisAcceleration; - } else if (velocityMetersPerSecond < 0) { + } else if (velocity < 0) { maxChassisAcceleration = -maxChassisAcceleration; } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java index 7daac52c8fa..9501e983ae5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java @@ -18,8 +18,8 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint { * Constructs a new EllipticalRegionConstraint. * * @param center The center of the ellipse in which to enforce the constraint. - * @param xWidth The width of the ellipse in which to enforce the constraint. - * @param yWidth The height of the ellipse in which to enforce the constraint. + * @param xWidth The width of the ellipse in which to enforce the constraint in meters. + * @param yWidth The height of the ellipse in which to enforce the constraint in meters. * @param rotation The rotation to apply to all radii around the origin. * @param constraint The constraint to enforce when the robot is within the region. * @deprecated Use constructor taking Ellipse2d instead. @@ -46,22 +46,18 @@ public EllipticalRegionConstraint(Ellipse2d ellipse, TrajectoryConstraint constr } @Override - public double getMaxVelocityMetersPerSecond( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { - if (m_ellipse.contains(poseMeters.getTranslation())) { - return m_constraint.getMaxVelocityMetersPerSecond( - poseMeters, curvatureRadPerMeter, velocityMetersPerSecond); + public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { + if (m_ellipse.contains(pose.getTranslation())) { + return m_constraint.getMaxVelocity(pose, curvature, velocity); } else { return Double.POSITIVE_INFINITY; } } @Override - public MinMax getMinMaxAccelerationMetersPerSecondSq( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { - if (m_ellipse.contains(poseMeters.getTranslation())) { - return m_constraint.getMinMaxAccelerationMetersPerSecondSq( - poseMeters, curvatureRadPerMeter, velocityMetersPerSecond); + public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { + if (m_ellipse.contains(pose.getTranslation())) { + return m_constraint.getMinMaxAcceleration(pose, curvature, velocity); } else { return new MinMax(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MaxVelocityConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MaxVelocityConstraint.java index d672295c6d9..613f0aa49ee 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MaxVelocityConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MaxVelocityConstraint.java @@ -17,21 +17,20 @@ public class MaxVelocityConstraint implements TrajectoryConstraint { /** * Constructs a new MaxVelocityConstraint. * - * @param maxVelocityMetersPerSecond The max velocity. + * @param maxVelocity The max velocity in m/s. */ - public MaxVelocityConstraint(double maxVelocityMetersPerSecond) { - m_maxVelocity = maxVelocityMetersPerSecond; + public MaxVelocityConstraint(double maxVelocity) { + m_maxVelocity = maxVelocity; } @Override - public double getMaxVelocityMetersPerSecond( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { return m_maxVelocity; } @Override - public TrajectoryConstraint.MinMax getMinMaxAccelerationMetersPerSecondSq( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public TrajectoryConstraint.MinMax getMinMaxAcceleration( + Pose2d pose, double curvature, double velocity) { return new MinMax(); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java index 35224cf3951..aca116ede7e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java @@ -14,66 +14,62 @@ * drivetrain stay below a certain limit. */ public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint { - private final double m_maxSpeedMetersPerSecond; + private final double m_maxSpeed; private final MecanumDriveKinematics m_kinematics; /** * Constructs a mecanum drive kinematics constraint. * * @param kinematics Mecanum drive kinematics. - * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at. + * @param maxSpeed The max speed that a side of the robot can travel at in m/s. */ public MecanumDriveKinematicsConstraint( - final MecanumDriveKinematics kinematics, double maxSpeedMetersPerSecond) { - m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond; + final MecanumDriveKinematics kinematics, double maxSpeed) { + m_maxSpeed = maxSpeed; m_kinematics = kinematics; } /** * Returns the max velocity given the current pose and curvature. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The velocity at the current point in the trajectory before - * constraints are applied. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory in rad/m. + * @param velocity The velocity at the current point in the trajectory before constraints are + * applied in m/s. * @return The absolute maximum velocity. */ @Override - public double getMaxVelocityMetersPerSecond( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { // Represents the velocity of the chassis in the x direction - var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos(); + var xdVelocity = velocity * pose.getRotation().getCos(); // Represents the velocity of the chassis in the y direction - var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin(); + var ydVelocity = velocity * pose.getRotation().getSin(); // Create an object to represent the current chassis speeds. - var chassisSpeeds = - new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter); + var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature); // Get the wheel speeds and normalize them to within the max velocity. - var wheelSpeeds = - m_kinematics.toWheelSpeeds(chassisSpeeds).desaturate(m_maxSpeedMetersPerSecond); + var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds).desaturate(m_maxSpeed); // Convert normalized wheel speeds back to chassis speeds var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); // Return the new linear chassis speed. - return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond); + return Math.hypot(normSpeeds.vx, normSpeeds.vy); } /** * Returns the minimum and maximum allowable acceleration for the trajectory given pose, * curvature, and speed. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory in rad/m. + * @param velocity The speed at the current point in the trajectory in m/s. * @return The min and max acceleration bounds. */ @Override - public MinMax getMinMaxAccelerationMetersPerSecondSq( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { return new MinMax(); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java index 69664719845..f4ee23e32d0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java @@ -41,22 +41,18 @@ public RectangularRegionConstraint(Rectangle2d rectangle, TrajectoryConstraint c } @Override - public double getMaxVelocityMetersPerSecond( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { - if (m_rectangle.contains(poseMeters.getTranslation())) { - return m_constraint.getMaxVelocityMetersPerSecond( - poseMeters, curvatureRadPerMeter, velocityMetersPerSecond); + public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { + if (m_rectangle.contains(pose.getTranslation())) { + return m_constraint.getMaxVelocity(pose, curvature, velocity); } else { return Double.POSITIVE_INFINITY; } } @Override - public MinMax getMinMaxAccelerationMetersPerSecondSq( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { - if (m_rectangle.contains(poseMeters.getTranslation())) { - return m_constraint.getMinMaxAccelerationMetersPerSecondSq( - poseMeters, curvatureRadPerMeter, velocityMetersPerSecond); + public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { + if (m_rectangle.contains(pose.getTranslation())) { + return m_constraint.getMinMaxAcceleration(pose, curvature, velocity); } else { return new MinMax(); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java index 5c2e1f58fe6..0f3867de9b9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java @@ -14,66 +14,62 @@ * stay below a certain limit. */ public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint { - private final double m_maxSpeedMetersPerSecond; + private final double m_maxSpeed; private final SwerveDriveKinematics m_kinematics; /** * Constructs a swerve drive kinematics constraint. * * @param kinematics Swerve drive kinematics. - * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at. + * @param maxSpeed The max speed that a side of the robot can travel at in m/s. */ - public SwerveDriveKinematicsConstraint( - final SwerveDriveKinematics kinematics, double maxSpeedMetersPerSecond) { - m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond; + public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics, double maxSpeed) { + m_maxSpeed = maxSpeed; m_kinematics = kinematics; } /** * Returns the max velocity given the current pose and curvature. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The velocity at the current point in the trajectory before - * constraints are applied. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory in rad/m. + * @param velocity The velocity at the current point in the trajectory before constraints are + * applied in m/s. * @return The absolute maximum velocity. */ @Override - public double getMaxVelocityMetersPerSecond( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { // Represents the velocity of the chassis in the x direction - var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos(); + var xdVelocity = velocity * pose.getRotation().getCos(); // Represents the velocity of the chassis in the y direction - var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin(); + var ydVelocity = velocity * pose.getRotation().getSin(); // Create an object to represent the current chassis speeds. - var chassisSpeeds = - new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter); + var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature); // Get the wheel speeds and normalize them to within the max velocity. var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond); + SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeed); // Convert normalized wheel speeds back to chassis speeds var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); // Return the new linear chassis speed. - return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond); + return Math.hypot(normSpeeds.vx, normSpeeds.vy); } /** * Returns the minimum and maximum allowable acceleration for the trajectory given pose, * curvature, and speed. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory in rad/m. + * @param velocity The speed at the current point in the trajectory in m/s. * @return The min and max acceleration bounds. */ @Override - public MinMax getMinMaxAccelerationMetersPerSecondSq( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { return new MinMax(); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java index 48490d4eba5..3dc689cff39 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java @@ -14,45 +14,42 @@ public interface TrajectoryConstraint { /** * Returns the max velocity given the current pose and curvature. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The velocity at the current point in the trajectory before - * constraints are applied. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory rad/m. + * @param velocity The velocity at the current point in the trajectory before constraints are + * applied in m/s. * @return The absolute maximum velocity. */ - double getMaxVelocityMetersPerSecond( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond); + double getMaxVelocity(Pose2d pose, double curvature, double velocity); /** * Returns the minimum and maximum allowable acceleration for the trajectory given pose, * curvature, and speed. * - * @param poseMeters The pose at the current point in the trajectory. - * @param curvatureRadPerMeter The curvature at the current point in the trajectory. - * @param velocityMetersPerSecond The speed at the current point in the trajectory. + * @param pose The pose at the current point in the trajectory. + * @param curvature The curvature at the current point in the trajectory rad/m. + * @param velocity The speed at the current point in the trajectory in m/s. * @return The min and max acceleration bounds. */ - MinMax getMinMaxAccelerationMetersPerSecondSq( - Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond); + MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity); /** Represents a minimum and maximum acceleration. */ class MinMax { /** The minimum acceleration. */ - public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE; + public double minAcceleration = -Double.MAX_VALUE; /** The maximum acceleration. */ - public double maxAccelerationMetersPerSecondSq = Double.MAX_VALUE; + public double maxAcceleration = Double.MAX_VALUE; /** * Constructs a MinMax. * - * @param minAccelerationMetersPerSecondSq The minimum acceleration. - * @param maxAccelerationMetersPerSecondSq The maximum acceleration. + * @param minAcceleration The minimum acceleration in m/s². + * @param maxAcceleration The maximum acceleration in m/s². */ - public MinMax( - double minAccelerationMetersPerSecondSq, double maxAccelerationMetersPerSecondSq) { - this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq; - this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq; + public MinMax(double minAcceleration, double maxAcceleration) { + this.minAcceleration = minAcceleration; + this.maxAcceleration = maxAcceleration; } /** Constructs a MinMax with default values. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java index 204c814db1b..d0c3198307f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java @@ -38,10 +38,10 @@ public Trajectory.State unpack(ProtobufTrajectoryState msg) { @Override public void pack(ProtobufTrajectoryState msg, Trajectory.State value) { - msg.setTime(value.timeSeconds); - msg.setVelocity(value.velocityMetersPerSecond); - msg.setAcceleration(value.accelerationMetersPerSecondSq); - Pose2d.proto.pack(msg.getMutablePose(), value.poseMeters); - msg.setCurvature(value.curvatureRadPerMeter); + msg.setTime(value.time); + msg.setVelocity(value.velocity); + msg.setAcceleration(value.acceleration); + Pose2d.proto.pack(msg.getMutablePose(), value.pose); + msg.setCurvature(value.curvature); } } diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp index 5ca8372796f..4c33bd710df 100644 --- a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp @@ -14,14 +14,14 @@ wpi::Protobuf::Unpack(InputStream& stream) { } return frc::DifferentialDriveKinematics{ - units::meter_t{msg.track_width}, + units::meter_t{msg.trackwidth}, }; } bool wpi::Protobuf::Pack( OutputStream& stream, const frc::DifferentialDriveKinematics& value) { wpi_proto_ProtobufDifferentialDriveKinematics msg{ - .track_width = value.trackWidth.value(), + .trackwidth = value.trackwidth.value(), }; return stream.Encode(msg); } diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp index b7be0d2266b..8f6b064ba52 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp @@ -5,7 +5,7 @@ #include "frc/kinematics/struct/DifferentialDriveKinematicsStruct.h" namespace { -constexpr size_t kTrackWidthOff = 0; +constexpr size_t kTrackwidthOff = 0; } // namespace using StructType = wpi::Struct; @@ -13,11 +13,11 @@ using StructType = wpi::Struct; frc::DifferentialDriveKinematics StructType::Unpack( std::span data) { return frc::DifferentialDriveKinematics{ - units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, }; } void StructType::Pack(std::span data, const frc::DifferentialDriveKinematics& value) { - wpi::PackStruct(data, value.trackWidth.value()); + wpi::PackStruct(data, value.trackwidth.value()); } diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index 124a84bdaff..517fdf50ac5 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -33,13 +33,13 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics /** * Constructs a differential drive kinematics object. * - * @param trackWidth The track width of the drivetrain. Theoretically, this is + * @param trackwidth The trackwidth of the drivetrain. Theoretically, this is * the distance between the left wheels and right wheels. However, the * empirical value may be larger than the physical measured value due to * scrubbing effects. */ - constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth) - : trackWidth(trackWidth) { + constexpr explicit DifferentialDriveKinematics(units::meter_t trackwidth) + : trackwidth(trackwidth) { if (!std::is_constant_evaluated()) { wpi::math::MathSharedStore::ReportUsage("DifferentialDriveKinematics", ""); @@ -56,7 +56,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics constexpr ChassisSpeeds ToChassisSpeeds( const DifferentialDriveWheelSpeeds& wheelSpeeds) const override { return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps, - (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad}; + (wheelSpeeds.right - wheelSpeeds.left) / trackwidth * 1_rad}; } /** @@ -69,8 +69,8 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics */ constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds( const ChassisSpeeds& chassisSpeeds) const override { - return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad, - chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad}; + return {chassisSpeeds.vx - trackwidth / 2 * chassisSpeeds.omega / 1_rad, + chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega / 1_rad}; } /** @@ -84,7 +84,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics constexpr Twist2d ToTwist2d(const units::meter_t leftDistance, const units::meter_t rightDistance) const { return {(leftDistance + rightDistance) / 2, 0_m, - (rightDistance - leftDistance) / trackWidth * 1_rad}; + (rightDistance - leftDistance) / trackwidth * 1_rad}; } constexpr Twist2d ToTwist2d( @@ -100,7 +100,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics } /// Differential drive trackwidth. - units::meter_t trackWidth; + units::meter_t trackwidth; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h index fa14606f61e..13a31ba02bb 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h @@ -15,7 +15,7 @@ struct WPILIB_DLLEXPORT wpi::Struct { return "DifferentialDriveKinematics"; } static constexpr size_t GetSize() { return 8; } - static constexpr std::string_view GetSchema() { return "double track_width"; } + static constexpr std::string_view GetSchema() { return "double trackwidth"; } static frc::DifferentialDriveKinematics Unpack(std::span data); static void Pack(std::span data, diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index f26c8a03769..c61b287e059 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -419,7 +419,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * @param motor The motor (or gearbox) driving the drivetrain. * @param mass The mass of the robot in kilograms. * @param r The radius of the wheels in meters. - * @param rb The radius of the base (half of the track width), in meters. + * @param rb The radius of the base (half of the trackwidth), in meters. * @param J The moment of inertia of the robot. * @param gearing Gear ratio from motor to wheel. * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h index 3c49e905a20..edafa6f7528 100644 --- a/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h +++ b/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h @@ -90,18 +90,18 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint if (speed == 0_mps) { maxChassisAcceleration = maxWheelAcceleration / - (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad)); + (1 + m_kinematics.trackwidth * units::math::abs(curvature) / (2_rad)); minChassisAcceleration = minWheelAcceleration / - (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad)); + (1 + m_kinematics.trackwidth * units::math::abs(curvature) / (2_rad)); } else { maxChassisAcceleration = maxWheelAcceleration / - (1 + m_kinematics.trackWidth * units::math::abs(curvature) * + (1 + m_kinematics.trackwidth * units::math::abs(curvature) * wpi::sgn(speed) / (2_rad)); minChassisAcceleration = minWheelAcceleration / - (1 - m_kinematics.trackWidth * units::math::abs(curvature) * + (1 - m_kinematics.trackwidth * units::math::abs(curvature) * wpi::sgn(speed) / (2_rad)); } @@ -111,7 +111,7 @@ class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint // wheel when this happens. We can accurately account for this by simply // negating the inner wheel. - if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) { + if ((m_kinematics.trackwidth / 2) > 1_rad / units::math::abs(curvature)) { if (speed > 0_mps) { minChassisAcceleration = -minChassisAcceleration; } else if (speed < 0_mps) { diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto index 2a31b2d7c65..2d25bd4ea4b 100644 --- a/wpimath/src/main/proto/kinematics.proto +++ b/wpimath/src/main/proto/kinematics.proto @@ -13,7 +13,7 @@ message ProtobufChassisSpeeds { } message ProtobufDifferentialDriveKinematics { - double track_width = 1; + double trackwidth = 1; } message ProtobufDifferentialDriveWheelSpeeds { diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java index fdf53eab745..45498574f7f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java @@ -20,7 +20,7 @@ class DifferentialDriveFeedforwardTest { private static final double kVAngular = 1.0; private static final double kAAngular = 1.0; private static final double trackwidth = 1.0; - private static final double dtSeconds = 0.02; + private static final double dt = 0.02; @Test void testCalculateWithTrackwidth() { @@ -39,12 +39,12 @@ void testCalculateWithTrackwidth() { nextLeftVelocity, currentRightVelocity, nextRightVelocity, - dtSeconds); + dt); Matrix nextX = plant.calculateX( VecBuilder.fill(currentLeftVelocity, currentRightVelocity), VecBuilder.fill(u.left, u.right), - dtSeconds); + dt); assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6); assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6); } @@ -69,12 +69,12 @@ void testCalculateWithoutTrackwidth() { nextLeftVelocity, currentRightVelocity, nextRightVelocity, - dtSeconds); + dt); Matrix nextX = plant.calculateX( VecBuilder.fill(currentLeftVelocity, currentRightVelocity), VecBuilder.fill(u.left, u.right), - dtSeconds); + dt); assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6); assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java index 4617fb76aee..870a9bc327b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java @@ -42,22 +42,17 @@ void testReachesReference() { Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); final double kDt = 0.02; - final double kTotalTime = trajectory.getTotalTimeSeconds(); + final double kTotalTime = trajectory.getTotalTime(); for (int i = 0; i < (kTotalTime / kDt); i++) { Trajectory.State state = trajectory.sample(kDt * i); ChassisSpeeds output = controller.calculate(robotPose, state, Rotation2d.kZero); - robotPose = - robotPose.exp( - new Twist2d( - output.vxMetersPerSecond * kDt, - output.vyMetersPerSecond * kDt, - output.omegaRadiansPerSecond * kDt)); + robotPose = robotPose.exp(new Twist2d(output.vx * kDt, output.vy * kDt, output.omega * kDt)); } final List states = trajectory.getStates(); - final Pose2d endPose = states.get(states.size() - 1).poseMeters; + final Pose2d endPose = states.get(states.size() - 1).pose; // Java lambdas require local variables referenced from a lambda expression // must be final or effectively final. @@ -85,6 +80,6 @@ void testDoesNotRotateUnnecessarily() { controller.calculate( new Pose2d(0, 0, new Rotation2d(1.57)), Pose2d.kZero, 0, new Rotation2d(1.57)); - assertEquals(0.0, speeds.omegaRadiansPerSecond); + assertEquals(0.0, speeds.omega); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java index 99bc544bd04..8740c674705 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java @@ -96,7 +96,7 @@ void testReachesReference() { 0.0, 0.0); - final var totalTime = trajectory.getTotalTimeSeconds(); + final var totalTime = trajectory.getTotalTime(); for (int i = 0; i < (totalTime / kDt); ++i) { var state = trajectory.sample(kDt * i); robotPose = @@ -115,7 +115,7 @@ void testReachesReference() { } final var states = trajectory.getStates(); - final var endPose = states.get(states.size() - 1).poseMeters; + final var endPose = states.get(states.size() - 1).pose; // Java lambdas require local variables referenced from a lambda expression // must be final or effectively final. diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java index 23316854e2f..b75dcbd5145 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java @@ -36,18 +36,16 @@ void testReachesReference() { var config = new TrajectoryConfig(8.8, 0.1); final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); - final var totalTime = trajectory.getTotalTimeSeconds(); + final var totalTime = trajectory.getTotalTime(); for (int i = 0; i < (totalTime / kDt); ++i) { var state = trajectory.sample(kDt * i); var output = controller.calculate(robotPose, state); - robotPose = - robotPose.exp( - new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt)); + robotPose = robotPose.exp(new Twist2d(output.vx * kDt, 0, output.omega * kDt)); } final var states = trajectory.getStates(); - final var endPose = states.get(states.size() - 1).poseMeters; + final var endPose = states.get(states.size() - 1).pose; // Java lambdas require local variables referenced from a lambda expression // must be final or effectively final. diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java index 48278ced6e3..fc59c7be49e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java @@ -85,7 +85,7 @@ void testLQROnArm() { * @param Q State cost matrix. * @param R Input cost matrix. * @param Aref Desired state matrix. - * @param dtSeconds Discretization timestep in seconds. + * @param dt Discretization timestep in seconds. */ Matrix getImplicitModelFollowingK( Matrix A, @@ -93,21 +93,21 @@ Matrix getImplicitModel Matrix Q, Matrix R, Matrix Aref, - double dtSeconds) { + double dt) { // Discretize real dynamics - var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + var discABPair = Discretization.discretizeAB(A, B, dt); var discA = discABPair.getFirst(); var discB = discABPair.getSecond(); // Discretize desired dynamics - var discAref = Discretization.discretizeA(Aref, dtSeconds); + var discAref = Discretization.discretizeA(Aref, dt); Matrix Qimf = (discA.minus(discAref)).transpose().times(Q).times(discA.minus(discAref)); Matrix Rimf = discB.transpose().times(Q).times(discB).plus(R); Matrix Nimf = (discA.minus(discAref)).transpose().times(Q).times(discB); - return new LinearQuadraticRegulator<>(A, B, Qimf, Rimf, Nimf, dtSeconds).getK(); + return new LinearQuadraticRegulator<>(A, B, Qimf, Rimf, Nimf, dt).getK(); } @Test diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java index 7912d3f9f54..9a89e32cecf 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java @@ -58,12 +58,8 @@ void testAccuracy() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, trajectory.getInitialPose(), new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -113,12 +109,8 @@ void testBadInitialPose() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, initial_pose, new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -141,11 +133,11 @@ void testFollowTrajectory( final double visionUpdateRate, final double visionUpdateDelay, final boolean checkError) { - double leftDistanceMeters = 0; - double rightDistanceMeters = 0; + double leftDistance = 0; + double rightDistance = 0; estimator.resetPosition( - Rotation3d.kZero, leftDistanceMeters, rightDistanceMeters, new Pose3d(startingPose)); + Rotation3d.kZero, leftDistance, rightDistance, new Pose3d(startingPose)); var rand = new Random(3538); @@ -155,7 +147,7 @@ void testFollowTrajectory( double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the @@ -183,24 +175,24 @@ void testFollowTrajectory( var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); - leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt; - rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt; + leftDistance += wheelSpeeds.left * dt; + rightDistance += wheelSpeeds.right * dt; var xHat = estimator.updateWithTime( t, new Rotation3d( groundTruthState - .poseMeters + .pose .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05)) .minus(trajectory.getInitialPose().getRotation())), - leftDistanceMeters, - rightDistanceMeters); + leftDistance, + rightDistance); double error = groundTruthState - .poseMeters + .pose .getTranslation() .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { @@ -222,8 +214,7 @@ void testFollowTrajectory( "Incorrect Final Theta"); if (checkError) { - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error"); assertEquals(0.0, maxError, 0.2, "Incorrect max error"); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java index 09ebfeda98e..a22a0e36266 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java @@ -55,12 +55,8 @@ void testAccuracy() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, trajectory.getInitialPose(), new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -110,12 +106,8 @@ void testBadInitialPose() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, initial_pose, new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -138,11 +130,10 @@ void testFollowTrajectory( final double visionUpdateRate, final double visionUpdateDelay, final boolean checkError) { - double leftDistanceMeters = 0; - double rightDistanceMeters = 0; + double leftDistance = 0; + double rightDistance = 0; - estimator.resetPosition( - Rotation2d.kZero, leftDistanceMeters, rightDistanceMeters, startingPose); + estimator.resetPosition(Rotation2d.kZero, leftDistance, rightDistance, startingPose); var rand = new Random(3538); @@ -152,7 +143,7 @@ void testFollowTrajectory( double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the @@ -180,22 +171,21 @@ void testFollowTrajectory( var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); - leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt; - rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt; + leftDistance += wheelSpeeds.left * dt; + rightDistance += wheelSpeeds.right * dt; var xHat = estimator.updateWithTime( t, groundTruthState - .poseMeters + .pose .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05)) .minus(trajectory.getInitialPose().getRotation()), - leftDistanceMeters, - rightDistanceMeters); + leftDistance, + rightDistance); - double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) { maxError = error; } @@ -215,8 +205,7 @@ void testFollowTrajectory( "Incorrect Final Theta"); if (checkError) { - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error"); assertEquals(0.0, maxError, 0.2, "Incorrect max error"); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java index a1b5b3a1cf0..29554b176da 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java @@ -30,14 +30,13 @@ private static Matrix getDynamics(final Matrix x, final Matrix getGlobalMeasurementModel(Matrix x, Matrix @Test void testInit() { - double dtSeconds = 0.00505; + double dt = 0.00505; assertDoesNotThrow( () -> { @@ -81,10 +80,10 @@ void testInit() { ExtendedKalmanFilterTest::getLocalMeasurementModel, VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), VecBuilder.fill(0.0001, 0.01, 0.01), - dtSeconds); + dt); Matrix u = VecBuilder.fill(12.0, 12.0); - observer.predict(u, dtSeconds); + observer.predict(u, dt); var localY = getLocalMeasurementModel(observer.getXhat(), u); observer.correct(u, localY); @@ -98,8 +97,8 @@ void testInit() { @Test void testConvergence() { - double dtSeconds = 0.00505; - double rbMeters = 0.8382 / 2.0; // Robot radius + double dt = 0.00505; + double rb = 0.8382 / 2.0; // Robot radius ExtendedKalmanFilter observer = new ExtendedKalmanFilter<>( @@ -110,7 +109,7 @@ void testConvergence() { ExtendedKalmanFilterTest::getLocalMeasurementModel, VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), VecBuilder.fill(0.001, 0.01, 0.01), - dtSeconds); + dt); List waypoints = List.of( @@ -142,15 +141,15 @@ void testConvergence() { var groundTruthX = observer.getXhat(); - double totalTime = trajectory.getTotalTimeSeconds(); - for (int i = 0; i < (totalTime / dtSeconds); i++) { - var ref = trajectory.sample(dtSeconds * i); - double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)); - double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)); + double totalTime = trajectory.getTotalTime(); + for (int i = 0; i < (totalTime / dt); i++) { + var ref = trajectory.sample(dt * i); + double vl = ref.velocity * (1 - (ref.curvature * rb)); + double vr = ref.velocity * (1 + (ref.curvature * rb)); - nextR.set(0, 0, ref.poseMeters.getTranslation().getX()); - nextR.set(1, 0, ref.poseMeters.getTranslation().getY()); - nextR.set(2, 0, ref.poseMeters.getRotation().getRadians()); + nextR.set(0, 0, ref.pose.getTranslation().getX()); + nextR.set(1, 0, ref.pose.getTranslation().getY()); + nextR.set(2, 0, ref.pose.getRotation().getRadians()); nextR.set(3, 0, vl); nextR.set(4, 0, vr); @@ -158,14 +157,13 @@ void testConvergence() { var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5); observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs))); - Matrix rdot = nextR.minus(r).div(dtSeconds); + Matrix rdot = nextR.minus(r).div(dt); u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1()))))); - observer.predict(u, dtSeconds); + observer.predict(u, dt); groundTruthX = - NumericalIntegration.rk4( - ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds); + NumericalIntegration.rk4(ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dt); r = nextR; } @@ -177,10 +175,10 @@ void testConvergence() { var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5)); observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R); - var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds()); - assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 1.0); - assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 1.0); - assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0); + var finalPosition = trajectory.sample(trajectory.getTotalTime()); + assertEquals(finalPosition.pose.getTranslation().getX(), observer.getXhat(0), 1.0); + assertEquals(finalPosition.pose.getTranslation().getY(), observer.getXhat(1), 1.0); + assertEquals(finalPosition.pose.getRotation().getRadians(), observer.getXhat(2), 1.0); assertEquals(0.0, observer.getXhat(3), 1.0); assertEquals(0.0, observer.getXhat(4), 1.0); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java index 42fbd92db0a..35f36bb6e9b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java @@ -159,19 +159,19 @@ void testSwerveKFMovingOverTrajectory() { var time = 0.0; var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0); - while (time <= trajectory.getTotalTimeSeconds()) { + while (time <= trajectory.getTotalTime()) { var sample = trajectory.sample(time); var measurement = VecBuilder.fill( - sample.poseMeters.getTranslation().getX() + random.nextGaussian() / 5d, - sample.poseMeters.getTranslation().getY() + random.nextGaussian() / 5d, - sample.poseMeters.getRotation().getRadians() + random.nextGaussian() / 3d); + sample.pose.getTranslation().getX() + random.nextGaussian() / 5d, + sample.pose.getTranslation().getY() + random.nextGaussian() / 5d, + sample.pose.getRotation().getRadians() + random.nextGaussian() / 3d); var velocity = VecBuilder.fill( - sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getCos(), - sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getSin(), - sample.curvatureRadPerMeter * sample.velocityMetersPerSecond); + sample.velocity * sample.pose.getRotation().getCos(), + sample.velocity * sample.pose.getRotation().getSin(), + sample.curvature * sample.velocity); var u = (velocity.minus(lastVelocity)).div(0.020); lastVelocity = velocity; @@ -182,11 +182,11 @@ void testSwerveKFMovingOverTrajectory() { } assertEquals( - trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters.getTranslation().getX(), + trajectory.sample(trajectory.getTotalTime()).pose.getTranslation().getX(), filter.getXhat(0), 0.2); assertEquals( - trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters.getTranslation().getY(), + trajectory.sample(trajectory.getTotalTime()).pose.getTranslation().getY(), filter.getXhat(1), 0.2); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java index 2ec6a200e7c..f8894e1effd 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java @@ -64,12 +64,8 @@ void testAccuracyFacingTrajectory() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, trajectory.getInitialPose(), new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -123,12 +119,8 @@ void testBadInitialPose() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, initial_pose, new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -163,7 +155,7 @@ void testFollowTrajectory( double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the @@ -191,17 +183,17 @@ void testFollowTrajectory( var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); - wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; - wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; - wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; - wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; var xHat = estimator.updateWithTime( t, new Rotation3d( groundTruthState - .poseMeters + .pose .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05)) .minus(trajectory.getInitialPose().getRotation())), @@ -209,7 +201,7 @@ void testFollowTrajectory( double error = groundTruthState - .poseMeters + .pose .getTranslation() .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { @@ -231,8 +223,7 @@ void testFollowTrajectory( "Incorrect Final Theta"); if (checkError) { - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error"); assertEquals(0.0, maxError, 0.2, "Incorrect max error"); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java index ca81e7d866b..ef164c4ae3a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java @@ -61,12 +61,8 @@ void testAccuracyFacingTrajectory() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, trajectory.getInitialPose(), new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -120,12 +116,8 @@ void testBadInitialPose() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, initial_pose, new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -160,7 +152,7 @@ void testFollowTrajectory( double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the @@ -188,23 +180,22 @@ void testFollowTrajectory( var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); - wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; - wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; - wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; - wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; var xHat = estimator.updateWithTime( t, groundTruthState - .poseMeters + .pose .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05)) .minus(trajectory.getInitialPose().getRotation()), wheelPositions); - double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) { maxError = error; } @@ -224,8 +215,7 @@ void testFollowTrajectory( "Incorrect Final Theta"); if (checkError) { - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error"); assertEquals(0.0, maxError, 0.2, "Incorrect max error"); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java index e3cd11a2815..98c3c39fb29 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java @@ -69,12 +69,8 @@ void testAccuracyFacingTrajectory() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, trajectory.getInitialPose(), new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -132,12 +128,8 @@ void testBadInitialPose() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, initial_pose, new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -177,7 +169,7 @@ void testFollowTrajectory( double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the @@ -206,8 +198,7 @@ void testFollowTrajectory( var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); for (int i = 0; i < moduleStates.length; i++) { - positions[i].distanceMeters += - moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt; + positions[i].distance += moduleStates[i].speed * (1 - rand.nextGaussian() * 0.05) * dt; positions[i].angle = moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); } @@ -217,7 +208,7 @@ void testFollowTrajectory( t, new Rotation3d( groundTruthState - .poseMeters + .pose .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05)) .minus(trajectory.getInitialPose().getRotation())), @@ -225,7 +216,7 @@ void testFollowTrajectory( double error = groundTruthState - .poseMeters + .pose .getTranslation() .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { @@ -247,8 +238,7 @@ void testFollowTrajectory( "Incorrect Final Theta"); if (checkError) { - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error"); assertEquals(0.0, maxError, 0.2, "Incorrect max error"); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java index 2b259a6a894..719c90f476a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java @@ -66,12 +66,8 @@ void testAccuracyFacingTrajectory() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, trajectory.getInitialPose(), new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -129,12 +125,8 @@ void testBadInitialPose() { kinematics, estimator, trajectory, - state -> - new ChassisSpeeds( - state.velocityMetersPerSecond, - 0, - state.velocityMetersPerSecond * state.curvatureRadPerMeter), - state -> state.poseMeters, + state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature), + state -> state.pose, initial_pose, new Pose2d(0, 0, Rotation2d.fromDegrees(45)), 0.02, @@ -174,7 +166,7 @@ void testFollowTrajectory( double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the @@ -203,8 +195,7 @@ void testFollowTrajectory( var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); for (int i = 0; i < moduleStates.length; i++) { - positions[i].distanceMeters += - moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt; + positions[i].distance += moduleStates[i].speed * (1 - rand.nextGaussian() * 0.05) * dt; positions[i].angle = moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); } @@ -213,14 +204,13 @@ void testFollowTrajectory( estimator.updateWithTime( t, groundTruthState - .poseMeters + .pose .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05)) .minus(trajectory.getInitialPose().getRotation()), positions); - double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) { maxError = error; } @@ -240,8 +230,7 @@ void testFollowTrajectory( "Incorrect Final Theta"); if (checkError) { - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error"); assertEquals(0.0, maxError, 0.2, "Incorrect max error"); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java index c5a6cd11a5b..d9b645b9c03 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java @@ -40,11 +40,8 @@ private static Matrix getDynamics(Matrix x, Matrix u) { var m = 63.503; // Robot mass var J = 5.6; // Robot moment of inertia - var C1 = - -Math.pow(gHigh, 2) - * motors.KtNMPerAmp - / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r); - var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r); + var C1 = -Math.pow(gHigh, 2) * motors.Kt / (motors.Kv * motors.R * r * r); + var C2 = gHigh * motors.Kt / (motors.R * r); var k1 = 1.0 / m + Math.pow(rb, 2) / J; var k2 = 1.0 / m - Math.pow(rb, 2) / J; @@ -74,7 +71,7 @@ private static Matrix getGlobalMeasurementModel(Matrix x, Matrix @Test void testInit() { - var dtSeconds = 0.005; + var dt = 0.005; assertDoesNotThrow( () -> { UnscentedKalmanFilter observer = @@ -90,10 +87,10 @@ void testInit() { AngleStatistics.angleResidual(2), AngleStatistics.angleResidual(0), AngleStatistics.angleAdd(2), - dtSeconds); + dt); var u = VecBuilder.fill(12.0, 12.0); - observer.predict(u, dtSeconds); + observer.predict(u, dt); var localY = getLocalMeasurementModel(observer.getXhat(), u); observer.correct(u, localY); @@ -117,8 +114,8 @@ void testInit() { @Test void testConvergence() { - double dtSeconds = 0.005; - double rbMeters = 0.8382 / 2.0; // Robot radius + double dt = 0.005; + double rb = 0.8382 / 2.0; // Robot radius UnscentedKalmanFilter observer = new UnscentedKalmanFilter<>( @@ -133,7 +130,7 @@ void testConvergence() { AngleStatistics.angleResidual(2), AngleStatistics.angleResidual(0), AngleStatistics.angleAdd(2), - dtSeconds); + dt); List waypoints = List.of( @@ -163,17 +160,17 @@ void testConvergence() { var trueXhat = observer.getXhat(); - double totalTime = trajectory.getTotalTimeSeconds(); - for (int i = 0; i < (totalTime / dtSeconds); i++) { - var ref = trajectory.sample(dtSeconds * i); - double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)); - double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)); + double totalTime = trajectory.getTotalTime(); + for (int i = 0; i < (totalTime / dt); i++) { + var ref = trajectory.sample(dt * i); + double vl = ref.velocity * (1 - (ref.curvature * rb)); + double vr = ref.velocity * (1 + (ref.curvature * rb)); var nextR = VecBuilder.fill( - ref.poseMeters.getTranslation().getX(), - ref.poseMeters.getTranslation().getY(), - ref.poseMeters.getRotation().getRadians(), + ref.pose.getTranslation().getX(), + ref.pose.getTranslation().getY(), + ref.pose.getRotation().getRadians(), vl, vr); @@ -182,14 +179,13 @@ void testConvergence() { observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev))); - var rdot = nextR.minus(r).div(dtSeconds); + var rdot = nextR.minus(r).div(dt); u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1()))))); - observer.predict(u, dtSeconds); + observer.predict(u, dt); r = nextR; - trueXhat = - NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds); + trueXhat = NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dt); } var localY = getLocalMeasurementModel(trueXhat, u); @@ -210,12 +206,11 @@ void testConvergence() { AngleStatistics.angleResidual(2), AngleStatistics.angleAdd(2)); - final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds()); + final var finalPosition = trajectory.sample(trajectory.getTotalTime()); - assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.055); - assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.15); - assertEquals( - finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 0.000005); + assertEquals(finalPosition.pose.getTranslation().getX(), observer.getXhat(0), 0.055); + assertEquals(finalPosition.pose.getTranslation().getY(), observer.getXhat(1), 0.15); + assertEquals(finalPosition.pose.getRotation().getRadians(), observer.getXhat(2), 0.000005); assertEquals(0.0, observer.getXhat(3), 0.1); assertEquals(0.0, observer.getXhat(4), 0.1); } @@ -252,7 +247,7 @@ void testLinearUKF() { @Test void testRoundTripP() { - var dtSeconds = 0.005; + var dt = 0.005; var observer = new UnscentedKalmanFilter<>( @@ -262,7 +257,7 @@ void testRoundTripP() { (x, u) -> x, VecBuilder.fill(0.0, 0.0), VecBuilder.fill(0.0, 0.0), - dtSeconds); + dt); var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0); observer.setP(P); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index 361f42520d3..38ac1ce2498 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -24,11 +24,7 @@ void testDiscretize() { final var dt = 0.01; final var speeds = target.discretize(duration); - final var twist = - new Twist2d( - speeds.vxMetersPerSecond * dt, - speeds.vyMetersPerSecond * dt, - speeds.omegaRadiansPerSecond * dt); + final var twist = new Twist2d(speeds.vx * dt, speeds.vy * dt, speeds.omega * dt); var pose = Pose2d.kZero; for (double time = 0; time < duration; time += dt) { @@ -37,13 +33,9 @@ void testDiscretize() { final var result = pose; // For lambda capture assertAll( - () -> assertEquals(target.vxMetersPerSecond * duration, result.getX(), kEpsilon), - () -> assertEquals(target.vyMetersPerSecond * duration, result.getY(), kEpsilon), - () -> - assertEquals( - target.omegaRadiansPerSecond * duration, - result.getRotation().getRadians(), - kEpsilon)); + () -> assertEquals(target.vx * duration, result.getX(), kEpsilon), + () -> assertEquals(target.vy * duration, result.getY(), kEpsilon), + () -> assertEquals(target.omega * duration, result.getRotation().getRadians(), kEpsilon)); } @Test @@ -54,9 +46,9 @@ void testMeasureConstructor() { var speeds = new ChassisSpeeds(vx, vy, omega); assertAll( - () -> assertEquals(0.368808, speeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(0, speeds.vyMetersPerSecond, kEpsilon), - () -> assertEquals(0.002094395102, speeds.omegaRadiansPerSecond, kEpsilon)); + () -> assertEquals(0.368808, speeds.vx, kEpsilon), + () -> assertEquals(0, speeds.vy, kEpsilon), + () -> assertEquals(0.002094395102, speeds.omega, kEpsilon)); } @Test @@ -64,9 +56,9 @@ void testToRobotRelative() { final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2); assertAll( - () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), - () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); + () -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon), + () -> assertEquals(1.0, chassisSpeeds.vy, kEpsilon), + () -> assertEquals(0.5, chassisSpeeds.omega, kEpsilon)); } @Test @@ -75,9 +67,9 @@ void testToFieldRelative() { new ChassisSpeeds(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0)); assertAll( - () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vyMetersPerSecond, kEpsilon), - () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); + () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vx, kEpsilon), + () -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vy, kEpsilon), + () -> assertEquals(0.5, chassisSpeeds.omega, kEpsilon)); } @Test @@ -88,9 +80,9 @@ void testPlus() { final var chassisSpeeds = left.plus(right); assertAll( - () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond), - () -> assertEquals(2.0, chassisSpeeds.vyMetersPerSecond), - () -> assertEquals(1.0, chassisSpeeds.omegaRadiansPerSecond)); + () -> assertEquals(3.0, chassisSpeeds.vx), + () -> assertEquals(2.0, chassisSpeeds.vy), + () -> assertEquals(1.0, chassisSpeeds.omega)); } @Test @@ -101,9 +93,9 @@ void testMinus() { final var chassisSpeeds = left.minus(right); assertAll( - () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond), - () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond), - () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond)); + () -> assertEquals(-1.0, chassisSpeeds.vx), + () -> assertEquals(0.0, chassisSpeeds.vy), + () -> assertEquals(0.5, chassisSpeeds.omega)); } @Test @@ -111,9 +103,9 @@ void testUnaryMinus() { final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).unaryMinus(); assertAll( - () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond), - () -> assertEquals(-0.5, chassisSpeeds.vyMetersPerSecond), - () -> assertEquals(-0.75, chassisSpeeds.omegaRadiansPerSecond)); + () -> assertEquals(-1.0, chassisSpeeds.vx), + () -> assertEquals(-0.5, chassisSpeeds.vy), + () -> assertEquals(-0.75, chassisSpeeds.omega)); } @Test @@ -121,9 +113,9 @@ void testMultiplication() { final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).times(2.0); assertAll( - () -> assertEquals(2.0, chassisSpeeds.vxMetersPerSecond), - () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond), - () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond)); + () -> assertEquals(2.0, chassisSpeeds.vx), + () -> assertEquals(1.0, chassisSpeeds.vy), + () -> assertEquals(1.5, chassisSpeeds.omega)); } @Test @@ -131,8 +123,8 @@ void testDivision() { final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).div(2.0); assertAll( - () -> assertEquals(0.5, chassisSpeeds.vxMetersPerSecond), - () -> assertEquals(0.25, chassisSpeeds.vyMetersPerSecond), - () -> assertEquals(0.375, chassisSpeeds.omegaRadiansPerSecond)); + () -> assertEquals(0.5, chassisSpeeds.vx), + () -> assertEquals(0.25, chassisSpeeds.vy), + () -> assertEquals(0.375, chassisSpeeds.omega)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java index adee41fabc7..d6a4473e3f7 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java @@ -20,8 +20,8 @@ void testInverseKinematicsForZeros() { var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); assertAll( - () -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)); + () -> assertEquals(0.0, wheelSpeeds.left, kEpsilon), + () -> assertEquals(0.0, wheelSpeeds.right, kEpsilon)); } @Test @@ -30,9 +30,9 @@ void testForwardKinematicsForZeros() { var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); assertAll( - () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); + () -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon)); } @Test @@ -41,8 +41,8 @@ void testInverseKinematicsForStraightLine() { var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); assertAll( - () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon), - () -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)); + () -> assertEquals(3.0, wheelSpeeds.left, kEpsilon), + () -> assertEquals(3.0, wheelSpeeds.right, kEpsilon)); } @Test @@ -51,9 +51,9 @@ void testForwardKinematicsForStraightLine() { var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); assertAll( - () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); + () -> assertEquals(3.0, chassisSpeeds.vx, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon)); } @Test @@ -62,8 +62,8 @@ void testInverseKinematicsForRotateInPlace() { var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds); assertAll( - () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon), - () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon)); + () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.left, kEpsilon), + () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.right, kEpsilon)); } @Test @@ -72,8 +72,8 @@ void testForwardKinematicsForRotateInPlace() { var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); assertAll( - () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), - () -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); + () -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon), + () -> assertEquals(-Math.PI, chassisSpeeds.omega, kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java index 166a615ca9d..3c30ad6e1cc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java @@ -25,7 +25,7 @@ void testInitialize() { 0, 0, new Pose3d(1, 2, 0, new Rotation3d(0, 0, Units.degreesToRadians(45)))); - var pose = odometry.getPoseMeters(); + var pose = odometry.getPose(); assertAll( () -> assertEquals(pose.getX(), 1.0, kEpsilon), () -> assertEquals(pose.getY(), 2.0, kEpsilon), diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java index 7f2618012df..d1716d9c67c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java @@ -18,8 +18,7 @@ void testPlus() { final var wheelSpeeds = left.plus(right); assertAll( - () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond), - () -> assertEquals(2.0, wheelSpeeds.rightMetersPerSecond)); + () -> assertEquals(3.0, wheelSpeeds.left), () -> assertEquals(2.0, wheelSpeeds.right)); } @Test @@ -30,8 +29,7 @@ void testMinus() { final var wheelSpeeds = left.minus(right); assertAll( - () -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond), - () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond)); + () -> assertEquals(-1.0, wheelSpeeds.left), () -> assertEquals(0.0, wheelSpeeds.right)); } @Test @@ -39,8 +37,7 @@ void testUnaryMinus() { final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).unaryMinus(); assertAll( - () -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond), - () -> assertEquals(-0.5, wheelSpeeds.rightMetersPerSecond)); + () -> assertEquals(-1.0, wheelSpeeds.left), () -> assertEquals(-0.5, wheelSpeeds.right)); } @Test @@ -48,8 +45,7 @@ void testMultiplication() { final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).times(2.0); assertAll( - () -> assertEquals(2.0, wheelSpeeds.leftMetersPerSecond), - () -> assertEquals(1.0, wheelSpeeds.rightMetersPerSecond)); + () -> assertEquals(2.0, wheelSpeeds.left), () -> assertEquals(1.0, wheelSpeeds.right)); } @Test @@ -57,7 +53,6 @@ void testDivision() { final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).div(2.0); assertAll( - () -> assertEquals(0.5, wheelSpeeds.leftMetersPerSecond), - () -> assertEquals(0.25, wheelSpeeds.rightMetersPerSecond)); + () -> assertEquals(0.5, wheelSpeeds.left), () -> assertEquals(0.25, wheelSpeeds.right)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java index ad0214072f5..73e6f74a003 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java @@ -27,10 +27,10 @@ void testStraightLineInverseKinematics() { var moduleStates = m_kinematics.toWheelSpeeds(speeds); assertAll( - () -> assertEquals(5.0, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(5.0, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(5.0, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(5.0, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(5.0, moduleStates.frontLeft, 0.1), + () -> assertEquals(5.0, moduleStates.frontRight, 0.1), + () -> assertEquals(5.0, moduleStates.rearLeft, 0.1), + () -> assertEquals(5.0, moduleStates.rearRight, 0.1)); } @Test @@ -39,9 +39,9 @@ void testStraightLineForwardKinematicsKinematics() { var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); assertAll( - () -> assertEquals(3.536, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1), - () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(3.536, moduleStates.vx, 0.1), + () -> assertEquals(0, moduleStates.vy, 0.1), + () -> assertEquals(0, moduleStates.omega, 0.1)); } @Test @@ -61,10 +61,10 @@ void testStrafeInverseKinematics() { var moduleStates = m_kinematics.toWheelSpeeds(speeds); assertAll( - () -> assertEquals(-4.0, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(4.0, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(4.0, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(-4.0, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(-4.0, moduleStates.frontLeft, 0.1), + () -> assertEquals(4.0, moduleStates.frontRight, 0.1), + () -> assertEquals(4.0, moduleStates.rearLeft, 0.1), + () -> assertEquals(-4.0, moduleStates.rearRight, 0.1)); } @Test @@ -73,9 +73,9 @@ void testStrafeForwardKinematicsKinematics() { var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); assertAll( - () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(2.8284, moduleStates.vyMetersPerSecond, 0.1), - () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(0, moduleStates.vx, 0.1), + () -> assertEquals(2.8284, moduleStates.vy, 0.1), + () -> assertEquals(0, moduleStates.omega, 0.1)); } @Test @@ -95,10 +95,10 @@ void testRotationInverseKinematics() { var moduleStates = m_kinematics.toWheelSpeeds(speeds); assertAll( - () -> assertEquals(-150.79645, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(150.79645, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(-150.79645, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(150.79645, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(-150.79645, moduleStates.frontLeft, 0.1), + () -> assertEquals(150.79645, moduleStates.frontRight, 0.1), + () -> assertEquals(-150.79645, moduleStates.rearLeft, 0.1), + () -> assertEquals(150.79645, moduleStates.rearRight, 0.1)); } @Test @@ -107,9 +107,9 @@ void testRotationForwardKinematicsKinematics() { var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); assertAll( - () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1), - () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(0, moduleStates.vx, 0.1), + () -> assertEquals(0, moduleStates.vy, 0.1), + () -> assertEquals(2 * Math.PI, moduleStates.omega, 0.1)); } @Test @@ -129,10 +129,10 @@ void testMixedTranslationRotationInverseKinematics() { var moduleStates = m_kinematics.toWheelSpeeds(speeds); assertAll( - () -> assertEquals(-25.0, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(29.0, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(-19.0, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(23.0, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(-25.0, moduleStates.frontLeft, 0.1), + () -> assertEquals(29.0, moduleStates.frontRight, 0.1), + () -> assertEquals(-19.0, moduleStates.rearLeft, 0.1), + () -> assertEquals(23.0, moduleStates.rearRight, 0.1)); } @Test @@ -141,9 +141,9 @@ void testMixedTranslationRotationForwardKinematicsKinematics() { var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); assertAll( - () -> assertEquals(1.413, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(2.122, moduleStates.vyMetersPerSecond, 0.1), - () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(1.413, moduleStates.vx, 0.1), + () -> assertEquals(2.122, moduleStates.vy, 0.1), + () -> assertEquals(0.707, moduleStates.omega, 0.1)); } @Test @@ -163,10 +163,10 @@ void testOffCenterRotationInverseKinematics() { var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl); assertAll( - () -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(24.0, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(-24.0, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(48.0, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(0, moduleStates.frontLeft, 0.1), + () -> assertEquals(24.0, moduleStates.frontRight, 0.1), + () -> assertEquals(-24.0, moduleStates.rearLeft, 0.1), + () -> assertEquals(48.0, moduleStates.rearRight, 0.1)); } @Test @@ -175,9 +175,9 @@ void testOffCenterRotationForwardKinematicsKinematics() { var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); assertAll( - () -> assertEquals(8.48525, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(-8.48525, moduleStates.vyMetersPerSecond, 0.1), - () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(8.48525, moduleStates.vx, 0.1), + () -> assertEquals(-8.48525, moduleStates.vy, 0.1), + () -> assertEquals(0.707, moduleStates.omega, 0.1)); } @Test @@ -197,10 +197,10 @@ void testOffCenterTranslationRotationInverseKinematics() { var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl); assertAll( - () -> assertEquals(3.0, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(31.0, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(-17.0, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(51.0, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(3.0, moduleStates.frontLeft, 0.1), + () -> assertEquals(31.0, moduleStates.frontRight, 0.1), + () -> assertEquals(-17.0, moduleStates.rearLeft, 0.1), + () -> assertEquals(51.0, moduleStates.rearRight, 0.1)); } @Test @@ -209,9 +209,9 @@ void testOffCenterRotationTranslationForwardKinematicsKinematics() { var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); assertAll( - () -> assertEquals(12.02, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(-7.07, moduleStates.vyMetersPerSecond, 0.1), - () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(12.02, moduleStates.vx, 0.1), + () -> assertEquals(-7.07, moduleStates.vy, 0.1), + () -> assertEquals(0.707, moduleStates.omega, 0.1)); } @Test @@ -232,10 +232,10 @@ void testDesaturate() { double factor = 5.5 / 7.0; assertAll( - () -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon), - () -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon), - () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon), - () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon)); + () -> assertEquals(5.0 * factor, wheelSpeeds.frontLeft, kEpsilon), + () -> assertEquals(6.0 * factor, wheelSpeeds.frontRight, kEpsilon), + () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeft, kEpsilon), + () -> assertEquals(7.0 * factor, wheelSpeeds.rearRight, kEpsilon)); } @Test @@ -245,9 +245,9 @@ void testDesaturateNegativeSpeeds() { final double kFactor = 5.5 / 7.0; assertAll( - () -> assertEquals(-5.0 * kFactor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon), - () -> assertEquals(6.0 * kFactor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon), - () -> assertEquals(4.0 * kFactor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon), - () -> assertEquals(-7.0 * kFactor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon)); + () -> assertEquals(-5.0 * kFactor, wheelSpeeds.frontLeft, kEpsilon), + () -> assertEquals(6.0 * kFactor, wheelSpeeds.frontRight, kEpsilon), + () -> assertEquals(4.0 * kFactor, wheelSpeeds.rearLeft, kEpsilon), + () -> assertEquals(-7.0 * kFactor, wheelSpeeds.rearRight, kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java index d417e2a120a..25802a1d37f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java @@ -42,7 +42,7 @@ void testInitialize() { Rotation3d.kZero, zero, new Pose3d(1, 2, 0, new Rotation3d(0, 0, Units.degreesToRadians(45)))); - var pose = odometry.getPoseMeters(); + var pose = odometry.getPose(); assertAll( () -> assertEquals(pose.getX(), 1.0, 1e-9), () -> assertEquals(pose.getY(), 2.0, 1e-9), @@ -148,38 +148,36 @@ void testAccuracyFacingTrajectory() { double errorSum = 0; double odometryDistanceTravelled = 0; double trajectoryDistanceTravelled = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); trajectoryDistanceTravelled += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; var wheelSpeeds = kinematics.toWheelSpeeds( new ChassisSpeeds( - groundTruthState.velocityMetersPerSecond, + groundTruthState.velocity, 0, - groundTruthState.velocityMetersPerSecond - * groundTruthState.curvatureRadPerMeter)); + groundTruthState.velocity * groundTruthState.curvature)); - wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRight += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRight += rand.nextGaussian() * 0.1; - wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; - wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; - wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; - wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; - var lastPose = odometry.getPoseMeters(); + var lastPose = odometry.getPose(); var xHat = odometry.update( new Rotation3d( groundTruthState - .poseMeters + .pose .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05))), wheelPositions); @@ -188,7 +186,7 @@ void testAccuracyFacingTrajectory() { double error = groundTruthState - .poseMeters + .pose .getTranslation() .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { @@ -199,8 +197,7 @@ void testAccuracyFacingTrajectory() { t += dt; } - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.35, "Incorrect mean error"); assertEquals(0.0, maxError, 0.35, "Incorrect max error"); assertEquals( 1.0, @@ -241,33 +238,30 @@ void testAccuracyFacingXAxis() { double errorSum = 0; double odometryDistanceTravelled = 0; double trajectoryDistanceTravelled = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); trajectoryDistanceTravelled += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; var wheelSpeeds = kinematics.toWheelSpeeds( new ChassisSpeeds( - groundTruthState.velocityMetersPerSecond - * groundTruthState.poseMeters.getRotation().getCos(), - groundTruthState.velocityMetersPerSecond - * groundTruthState.poseMeters.getRotation().getSin(), + groundTruthState.velocity * groundTruthState.pose.getRotation().getCos(), + groundTruthState.velocity * groundTruthState.pose.getRotation().getSin(), 0)); - wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRight += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRight += rand.nextGaussian() * 0.1; - wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; - wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; - wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; - wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; - var lastPose = odometry.getPoseMeters(); + var lastPose = odometry.getPose(); var xHat = odometry.update(new Rotation3d(0, 0, rand.nextGaussian() * 0.05), wheelPositions); @@ -275,7 +269,7 @@ void testAccuracyFacingXAxis() { double error = groundTruthState - .poseMeters + .pose .getTranslation() .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { @@ -286,8 +280,7 @@ void testAccuracyFacingXAxis() { t += dt; } - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.15, "Incorrect mean error"); assertEquals(0.0, maxError, 0.3, "Incorrect max error"); assertEquals( 1.0, diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java index 86b2b405950..e1fbe8e6d32 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java @@ -124,45 +124,39 @@ void testAccuracyFacingTrajectory() { double errorSum = 0; double odometryDistanceTravelled = 0; double trajectoryDistanceTravelled = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); trajectoryDistanceTravelled += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; var wheelSpeeds = kinematics.toWheelSpeeds( new ChassisSpeeds( - groundTruthState.velocityMetersPerSecond, + groundTruthState.velocity, 0, - groundTruthState.velocityMetersPerSecond - * groundTruthState.curvatureRadPerMeter)); + groundTruthState.velocity * groundTruthState.curvature)); - wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRight += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRight += rand.nextGaussian() * 0.1; - wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; - wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; - wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; - wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; - var lastPose = odometry.getPoseMeters(); + var lastPose = odometry.getPose(); var xHat = odometry.update( - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)), + groundTruthState.pose.getRotation().plus(new Rotation2d(rand.nextGaussian() * 0.05)), wheelPositions); odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation()); - double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) { maxError = error; } @@ -171,8 +165,7 @@ void testAccuracyFacingTrajectory() { t += dt; } - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.35, "Incorrect mean error"); assertEquals(0.0, maxError, 0.35, "Incorrect max error"); assertEquals( 1.0, @@ -213,40 +206,36 @@ void testAccuracyFacingXAxis() { double errorSum = 0; double odometryDistanceTravelled = 0; double trajectoryDistanceTravelled = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); trajectoryDistanceTravelled += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; var wheelSpeeds = kinematics.toWheelSpeeds( new ChassisSpeeds( - groundTruthState.velocityMetersPerSecond - * groundTruthState.poseMeters.getRotation().getCos(), - groundTruthState.velocityMetersPerSecond - * groundTruthState.poseMeters.getRotation().getSin(), + groundTruthState.velocity * groundTruthState.pose.getRotation().getCos(), + groundTruthState.velocity * groundTruthState.pose.getRotation().getSin(), 0)); - wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; - wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRight += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRight += rand.nextGaussian() * 0.1; - wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; - wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; - wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; - wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; - var lastPose = odometry.getPoseMeters(); + var lastPose = odometry.getPose(); var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), wheelPositions); odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation()); - double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) { maxError = error; } @@ -255,8 +244,7 @@ void testAccuracyFacingXAxis() { t += dt; } - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.15, "Incorrect mean error"); assertEquals(0.0, maxError, 0.3, "Incorrect max error"); assertEquals( 1.0, diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java index 22efb44332f..6080bedb15a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java @@ -18,10 +18,10 @@ void testPlus() { final var wheelSpeeds = left.plus(right); assertAll( - () -> assertEquals(3.0, wheelSpeeds.frontLeftMetersPerSecond), - () -> assertEquals(2.0, wheelSpeeds.frontRightMetersPerSecond), - () -> assertEquals(2.5, wheelSpeeds.rearLeftMetersPerSecond), - () -> assertEquals(2.5, wheelSpeeds.rearRightMetersPerSecond)); + () -> assertEquals(3.0, wheelSpeeds.frontLeft), + () -> assertEquals(2.0, wheelSpeeds.frontRight), + () -> assertEquals(2.5, wheelSpeeds.rearLeft), + () -> assertEquals(2.5, wheelSpeeds.rearRight)); } @Test @@ -32,10 +32,10 @@ void testMinus() { final var wheelSpeeds = left.minus(right); assertAll( - () -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond), - () -> assertEquals(0.0, wheelSpeeds.frontRightMetersPerSecond), - () -> assertEquals(1.5, wheelSpeeds.rearLeftMetersPerSecond), - () -> assertEquals(0.5, wheelSpeeds.rearRightMetersPerSecond)); + () -> assertEquals(-1.0, wheelSpeeds.frontLeft), + () -> assertEquals(0.0, wheelSpeeds.frontRight), + () -> assertEquals(1.5, wheelSpeeds.rearLeft), + () -> assertEquals(0.5, wheelSpeeds.rearRight)); } @Test @@ -43,10 +43,10 @@ void testUnaryMinus() { final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).unaryMinus(); assertAll( - () -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond), - () -> assertEquals(-0.5, wheelSpeeds.frontRightMetersPerSecond), - () -> assertEquals(-2.0, wheelSpeeds.rearLeftMetersPerSecond), - () -> assertEquals(-1.5, wheelSpeeds.rearRightMetersPerSecond)); + () -> assertEquals(-1.0, wheelSpeeds.frontLeft), + () -> assertEquals(-0.5, wheelSpeeds.frontRight), + () -> assertEquals(-2.0, wheelSpeeds.rearLeft), + () -> assertEquals(-1.5, wheelSpeeds.rearRight)); } @Test @@ -54,10 +54,10 @@ void testMultiplication() { final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).times(2.0); assertAll( - () -> assertEquals(2.0, wheelSpeeds.frontLeftMetersPerSecond), - () -> assertEquals(1.0, wheelSpeeds.frontRightMetersPerSecond), - () -> assertEquals(4.0, wheelSpeeds.rearLeftMetersPerSecond), - () -> assertEquals(3.0, wheelSpeeds.rearRightMetersPerSecond)); + () -> assertEquals(2.0, wheelSpeeds.frontLeft), + () -> assertEquals(1.0, wheelSpeeds.frontRight), + () -> assertEquals(4.0, wheelSpeeds.rearLeft), + () -> assertEquals(3.0, wheelSpeeds.rearRight)); } @Test @@ -65,9 +65,9 @@ void testDivision() { final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).div(2.0); assertAll( - () -> assertEquals(0.5, wheelSpeeds.frontLeftMetersPerSecond), - () -> assertEquals(0.25, wheelSpeeds.frontRightMetersPerSecond), - () -> assertEquals(1.0, wheelSpeeds.rearLeftMetersPerSecond), - () -> assertEquals(0.75, wheelSpeeds.rearRightMetersPerSecond)); + () -> assertEquals(0.5, wheelSpeeds.frontLeft), + () -> assertEquals(0.25, wheelSpeeds.frontRight), + () -> assertEquals(1.0, wheelSpeeds.rearLeft), + () -> assertEquals(0.75, wheelSpeeds.rearRight)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 293d94e554a..fc3921f52f4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -29,10 +29,10 @@ void testStraightLineInverseKinematics() { // test inverse kinematics going in a var moduleStates = m_kinematics.toSwerveModuleStates(speeds); assertAll( - () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, moduleStates[0].speed, kEpsilon), + () -> assertEquals(5.0, moduleStates[1].speed, kEpsilon), + () -> assertEquals(5.0, moduleStates[2].speed, kEpsilon), + () -> assertEquals(5.0, moduleStates[3].speed, kEpsilon), () -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon), () -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon), () -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon), @@ -45,9 +45,9 @@ void testStraightLineForwardKinematics() { // test forward kinematics going in a var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state); assertAll( - () -> assertEquals(5.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); + () -> assertEquals(5.0, chassisSpeeds.vx, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon)); } @Test @@ -68,10 +68,10 @@ void testStraightStrafeInverseKinematics() { var moduleStates = m_kinematics.toSwerveModuleStates(speeds); assertAll( - () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon), + () -> assertEquals(5.0, moduleStates[0].speed, kEpsilon), + () -> assertEquals(5.0, moduleStates[1].speed, kEpsilon), + () -> assertEquals(5.0, moduleStates[2].speed, kEpsilon), + () -> assertEquals(5.0, moduleStates[3].speed, kEpsilon), () -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon), () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon), () -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon), @@ -84,9 +84,9 @@ void testStraightStrafeForwardKinematics() { var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state); assertAll( - () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); + () -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon), + () -> assertEquals(5.0, chassisSpeeds.vy, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon)); } @Test @@ -109,10 +109,10 @@ void testConserveWheelAngle() { // Robot is stationary, but module angles are preserved. assertAll( - () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, moduleStates[0].speed, kEpsilon), + () -> assertEquals(0.0, moduleStates[1].speed, kEpsilon), + () -> assertEquals(0.0, moduleStates[2].speed, kEpsilon), + () -> assertEquals(0.0, moduleStates[3].speed, kEpsilon), () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon), () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon), () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon), @@ -131,10 +131,10 @@ void testResetWheelAngle() { // Robot is stationary, but module angles are preserved. assertAll( - () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, moduleStates[0].speed, kEpsilon), + () -> assertEquals(0.0, moduleStates[1].speed, kEpsilon), + () -> assertEquals(0.0, moduleStates[2].speed, kEpsilon), + () -> assertEquals(0.0, moduleStates[3].speed, kEpsilon), () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon), () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon), () -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon), @@ -154,10 +154,10 @@ void testTurnInPlaceInverseKinematics() { */ assertAll( - () -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1), - () -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1), - () -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1), - () -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1), + () -> assertEquals(106.63, moduleStates[0].speed, 0.1), + () -> assertEquals(106.63, moduleStates[1].speed, 0.1), + () -> assertEquals(106.63, moduleStates[2].speed, 0.1), + () -> assertEquals(106.63, moduleStates[3].speed, 0.1), () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon), () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon), () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon), @@ -174,9 +174,9 @@ void testTurnInPlaceForwardKinematics() { var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState); assertAll( - () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon), - () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), - () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon), + () -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon), + () -> assertEquals(2 * Math.PI, chassisSpeeds.omega, 0.1)); } @Test @@ -209,10 +209,10 @@ void testOffCenterCORRotationInverseKinematics() { */ assertAll( - () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1), - () -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1), - () -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1), - () -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1), + () -> assertEquals(0.0, moduleStates[0].speed, 0.1), + () -> assertEquals(150.796, moduleStates[1].speed, 0.1), + () -> assertEquals(150.796, moduleStates[2].speed, 0.1), + () -> assertEquals(213.258, moduleStates[3].speed, 0.1), () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon), () -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon), () -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon), @@ -238,9 +238,9 @@ void testOffCenterCORRotationForwardKinematics() { */ assertAll( - () -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1), - () -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1), - () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(75.398, chassisSpeeds.vx, 0.1), + () -> assertEquals(-75.398, chassisSpeeds.vy, 0.1), + () -> assertEquals(2 * Math.PI, chassisSpeeds.omega, 0.1)); } @Test @@ -270,11 +270,7 @@ void testOffCenterCORRotationForwardKinematicsWithDeltas() { private void assertModuleState( SwerveModuleState expected, SwerveModuleState actual, SwerveModuleState tolerance) { assertAll( - () -> - assertEquals( - expected.speedMetersPerSecond, - actual.speedMetersPerSecond, - tolerance.speedMetersPerSecond), + () -> assertEquals(expected.speed, actual.speed, tolerance.speed), () -> assertEquals( expected.angle.getDegrees(), @@ -326,9 +322,9 @@ pseudoinverse of the inverseKinematics matrix (with the center of rotation at */ assertAll( - () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1), - () -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1), - () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(0.0, chassisSpeeds.vx, 0.1), + () -> assertEquals(-33.0, chassisSpeeds.vy, 0.1), + () -> assertEquals(1.5, chassisSpeeds.omega, 0.1)); } @Test @@ -368,10 +364,10 @@ void testDesaturate() { double factor = 5.5 / 7.0; assertAll( - () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)); + () -> assertEquals(5.0 * factor, arr[0].speed, kEpsilon), + () -> assertEquals(6.0 * factor, arr[1].speed, kEpsilon), + () -> assertEquals(4.0 * factor, arr[2].speed, kEpsilon), + () -> assertEquals(7.0 * factor, arr[3].speed, kEpsilon)); } @Test @@ -388,10 +384,10 @@ void testDesaturateSmooth() { double factor = 5.5 / 7.0; assertAll( - () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)); + () -> assertEquals(5.0 * factor, arr[0].speed, kEpsilon), + () -> assertEquals(6.0 * factor, arr[1].speed, kEpsilon), + () -> assertEquals(4.0 * factor, arr[2].speed, kEpsilon), + () -> assertEquals(7.0 * factor, arr[3].speed, kEpsilon)); } @Test @@ -405,9 +401,9 @@ void testDesaturateNegativeSpeed() { SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1); assertAll( - () -> assertEquals(0.5, arr[0].speedMetersPerSecond, kEpsilon), - () -> assertEquals(0.5, arr[1].speedMetersPerSecond, kEpsilon), - () -> assertEquals(-1.0, arr[2].speedMetersPerSecond, kEpsilon), - () -> assertEquals(-1.0, arr[3].speedMetersPerSecond, kEpsilon)); + () -> assertEquals(0.5, arr[0].speed, kEpsilon), + () -> assertEquals(0.5, arr[1].speed, kEpsilon), + () -> assertEquals(-1.0, arr[2].speed, kEpsilon), + () -> assertEquals(-1.0, arr[3].speed, kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java index dfc446fceea..1eb97ae5ba1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java @@ -43,7 +43,7 @@ void testInitialize() { Rotation3d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}, new Pose3d(1, 2, 0, new Rotation3d(0, 0, Units.degreesToRadians(45)))); - var pose = odometry.getPoseMeters(); + var pose = odometry.getPose(); assertAll( () -> assertEquals(pose.getX(), 1.0, 1e-9), () -> assertEquals(pose.getY(), 2.0, 1e-9), @@ -158,25 +158,24 @@ void testAccuracyFacingTrajectory() { double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); var moduleStates = kinematics.toSwerveModuleStates( new ChassisSpeeds( - groundTruthState.velocityMetersPerSecond, + groundTruthState.velocity, 0.0, - groundTruthState.velocityMetersPerSecond - * groundTruthState.curvatureRadPerMeter)); + groundTruthState.velocity * groundTruthState.curvature)); for (var moduleState : moduleStates) { moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); - moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1; + moduleState.speed += rand.nextGaussian() * 0.1; } - fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt; - fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt; - bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt; - br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt; + fl.distance += moduleStates[0].speed * dt; + fr.distance += moduleStates[1].speed * dt; + bl.distance += moduleStates[2].speed * dt; + br.distance += moduleStates[3].speed * dt; fl.angle = moduleStates[0].angle; fr.angle = moduleStates[1].angle; @@ -187,14 +186,14 @@ void testAccuracyFacingTrajectory() { odometry.update( new Rotation3d( groundTruthState - .poseMeters + .pose .getRotation() .plus(new Rotation2d(rand.nextGaussian() * 0.05))), new SwerveModulePosition[] {fl, fr, bl, br}); double error = groundTruthState - .poseMeters + .pose .getTranslation() .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { @@ -205,17 +204,16 @@ void testAccuracyFacingTrajectory() { t += dt; } - assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X"); - assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y"); - assertEquals(0.0, odometry.getPoseMeters().getZ(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPose().getX(), 1e-1, "Incorrect Final X"); + assertEquals(0.0, odometry.getPose().getY(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPose().getZ(), 1e-1, "Incorrect Final Y"); assertEquals( Math.PI / 4, - odometry.getPoseMeters().getRotation().toRotation2d().getRadians(), + odometry.getPose().getRotation().toRotation2d().getRadians(), 10 * Math.PI / 180, "Incorrect Final Theta"); - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.05, "Incorrect mean error"); assertEquals(0.0, maxError, 0.125, "Incorrect max error"); } @@ -253,26 +251,18 @@ void testAccuracyFacingXAxis() { double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); - fl.distanceMeters += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; - fr.distanceMeters += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; - bl.distanceMeters += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; - br.distanceMeters += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; - - fl.angle = groundTruthState.poseMeters.getRotation(); - fr.angle = groundTruthState.poseMeters.getRotation(); - bl.angle = groundTruthState.poseMeters.getRotation(); - br.angle = groundTruthState.poseMeters.getRotation(); + fl.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; + fr.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; + bl.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; + br.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; + + fl.angle = groundTruthState.pose.getRotation(); + fr.angle = groundTruthState.pose.getRotation(); + bl.angle = groundTruthState.pose.getRotation(); + br.angle = groundTruthState.pose.getRotation(); var xHat = odometry.update( @@ -281,7 +271,7 @@ void testAccuracyFacingXAxis() { double error = groundTruthState - .poseMeters + .pose .getTranslation() .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { @@ -292,17 +282,16 @@ void testAccuracyFacingXAxis() { t += dt; } - assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X"); - assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y"); - assertEquals(0.0, odometry.getPoseMeters().getZ(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPose().getX(), 1e-1, "Incorrect Final X"); + assertEquals(0.0, odometry.getPose().getY(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPose().getZ(), 1e-1, "Incorrect Final Y"); assertEquals( 0.0, - odometry.getPoseMeters().getRotation().toRotation2d().getRadians(), + odometry.getPose().getRotation().toRotation2d().getRadians(), 10 * Math.PI / 180, "Incorrect Final Theta"); - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.06, "Incorrect mean error"); assertEquals(0.0, maxError, 0.125, "Incorrect max error"); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java index 411ba705f47..c41a8b3fb37 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java @@ -135,25 +135,24 @@ void testAccuracyFacingTrajectory() { double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); var moduleStates = kinematics.toSwerveModuleStates( new ChassisSpeeds( - groundTruthState.velocityMetersPerSecond, + groundTruthState.velocity, 0.0, - groundTruthState.velocityMetersPerSecond - * groundTruthState.curvatureRadPerMeter)); + groundTruthState.velocity * groundTruthState.curvature)); for (var moduleState : moduleStates) { moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); - moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1; + moduleState.speed += rand.nextGaussian() * 0.1; } - fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt; - fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt; - bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt; - br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt; + fl.distance += moduleStates[0].speed * dt; + fr.distance += moduleStates[1].speed * dt; + bl.distance += moduleStates[2].speed * dt; + br.distance += moduleStates[3].speed * dt; fl.angle = moduleStates[0].angle; fr.angle = moduleStates[1].angle; @@ -162,14 +161,10 @@ void testAccuracyFacingTrajectory() { var xHat = odometry.update( - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)), + groundTruthState.pose.getRotation().plus(new Rotation2d(rand.nextGaussian() * 0.05)), new SwerveModulePosition[] {fl, fr, bl, br}); - double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) { maxError = error; } @@ -178,16 +173,15 @@ void testAccuracyFacingTrajectory() { t += dt; } - assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X"); - assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPose().getX(), 1e-1, "Incorrect Final X"); + assertEquals(0.0, odometry.getPose().getY(), 1e-1, "Incorrect Final Y"); assertEquals( Math.PI / 4, - odometry.getPoseMeters().getRotation().getRadians(), + odometry.getPose().getRotation().getRadians(), 10 * Math.PI / 180, "Incorrect Final Theta"); - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.05, "Incorrect mean error"); assertEquals(0.0, maxError, 0.125, "Incorrect max error"); } @@ -225,34 +219,25 @@ void testAccuracyFacingXAxis() { double maxError = Double.NEGATIVE_INFINITY; double errorSum = 0; - while (t <= trajectory.getTotalTimeSeconds()) { + while (t <= trajectory.getTotalTime()) { var groundTruthState = trajectory.sample(t); - fl.distanceMeters += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; - fr.distanceMeters += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; - bl.distanceMeters += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; - br.distanceMeters += - groundTruthState.velocityMetersPerSecond * dt - + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; - - fl.angle = groundTruthState.poseMeters.getRotation(); - fr.angle = groundTruthState.poseMeters.getRotation(); - bl.angle = groundTruthState.poseMeters.getRotation(); - br.angle = groundTruthState.poseMeters.getRotation(); + fl.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; + fr.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; + bl.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; + br.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt; + + fl.angle = groundTruthState.pose.getRotation(); + fr.angle = groundTruthState.pose.getRotation(); + bl.angle = groundTruthState.pose.getRotation(); + br.angle = groundTruthState.pose.getRotation(); var xHat = odometry.update( new Rotation2d(rand.nextGaussian() * 0.05), new SwerveModulePosition[] {fl, fr, bl, br}); - double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation()); if (error > maxError) { maxError = error; } @@ -261,16 +246,15 @@ void testAccuracyFacingXAxis() { t += dt; } - assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X"); - assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPose().getX(), 1e-1, "Incorrect Final X"); + assertEquals(0.0, odometry.getPose().getY(), 1e-1, "Incorrect Final Y"); assertEquals( 0.0, - odometry.getPoseMeters().getRotation().getRadians(), + odometry.getPose().getRotation().getRadians(), 10 * Math.PI / 180, "Incorrect Final Theta"); - assertEquals( - 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error"); + assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.06, "Incorrect mean error"); assertEquals(0.0, maxError, 0.125, "Incorrect max error"); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java index e8c1d4be0e0..fdccdccb08b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java @@ -20,7 +20,7 @@ void testOptimize() { refA.optimize(angleA); assertAll( - () -> assertEquals(2.0, refA.speedMetersPerSecond, kEpsilon), + () -> assertEquals(2.0, refA.speed, kEpsilon), () -> assertEquals(0.0, refA.angle.getDegrees(), kEpsilon)); var angleB = Rotation2d.fromDegrees(-50); @@ -28,7 +28,7 @@ void testOptimize() { refB.optimize(angleB); assertAll( - () -> assertEquals(-4.7, refB.speedMetersPerSecond, kEpsilon), + () -> assertEquals(-4.7, refB.speed, kEpsilon), () -> assertEquals(-139.0, refB.angle.getDegrees(), kEpsilon)); } @@ -39,7 +39,7 @@ void testNoOptimize() { refA.optimize(angleA); assertAll( - () -> assertEquals(2.0, refA.speedMetersPerSecond, kEpsilon), + () -> assertEquals(2.0, refA.speed, kEpsilon), () -> assertEquals(89.0, refA.angle.getDegrees(), kEpsilon)); var angleB = Rotation2d.kZero; @@ -47,7 +47,7 @@ void testNoOptimize() { refB.optimize(angleB); assertAll( - () -> assertEquals(-2.0, refB.speedMetersPerSecond, kEpsilon), + () -> assertEquals(-2.0, refB.speed, kEpsilon), () -> assertEquals(-2.0, refB.angle.getDegrees(), kEpsilon)); } @@ -58,7 +58,7 @@ void testCosineScale() { refA.cosineScale(angleA); assertAll( - () -> assertEquals(Math.sqrt(2.0), refA.speedMetersPerSecond, kEpsilon), + () -> assertEquals(Math.sqrt(2.0), refA.speed, kEpsilon), () -> assertEquals(45.0, refA.angle.getDegrees(), kEpsilon)); var angleB = Rotation2d.fromDegrees(45.0); @@ -66,7 +66,7 @@ void testCosineScale() { refB.cosineScale(angleB); assertAll( - () -> assertEquals(2.0, refB.speedMetersPerSecond, kEpsilon), + () -> assertEquals(2.0, refB.speed, kEpsilon), () -> assertEquals(45.0, refB.angle.getDegrees(), kEpsilon)); var angleC = Rotation2d.fromDegrees(-45.0); @@ -74,7 +74,7 @@ void testCosineScale() { refC.cosineScale(angleC); assertAll( - () -> assertEquals(0.0, refC.speedMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, refC.speed, kEpsilon), () -> assertEquals(45.0, refC.angle.getDegrees(), kEpsilon)); var angleD = Rotation2d.fromDegrees(135.0); @@ -82,7 +82,7 @@ void testCosineScale() { refD.cosineScale(angleD); assertAll( - () -> assertEquals(0.0, refD.speedMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, refD.speed, kEpsilon), () -> assertEquals(45.0, refD.angle.getDegrees(), kEpsilon)); var angleE = Rotation2d.fromDegrees(-135.0); @@ -90,7 +90,7 @@ void testCosineScale() { refE.cosineScale(angleE); assertAll( - () -> assertEquals(-2.0, refE.speedMetersPerSecond, kEpsilon), + () -> assertEquals(-2.0, refE.speed, kEpsilon), () -> assertEquals(45.0, refE.angle.getDegrees(), kEpsilon)); var angleF = Rotation2d.fromDegrees(180.0); @@ -98,7 +98,7 @@ void testCosineScale() { refF.cosineScale(angleF); assertAll( - () -> assertEquals(-Math.sqrt(2.0), refF.speedMetersPerSecond, kEpsilon), + () -> assertEquals(-Math.sqrt(2.0), refF.speed, kEpsilon), () -> assertEquals(45.0, refF.angle.getDegrees(), kEpsilon)); var angleG = Rotation2d.fromDegrees(0.0); @@ -106,7 +106,7 @@ void testCosineScale() { refG.cosineScale(angleG); assertAll( - () -> assertEquals(-Math.sqrt(2.0), refG.speedMetersPerSecond, kEpsilon), + () -> assertEquals(-Math.sqrt(2.0), refG.speed, kEpsilon), () -> assertEquals(45.0, refG.angle.getDegrees(), kEpsilon)); var angleH = Rotation2d.fromDegrees(45.0); @@ -114,7 +114,7 @@ void testCosineScale() { refH.cosineScale(angleH); assertAll( - () -> assertEquals(-2.0, refH.speedMetersPerSecond, kEpsilon), + () -> assertEquals(-2.0, refH.speed, kEpsilon), () -> assertEquals(45.0, refH.angle.getDegrees(), kEpsilon)); var angleI = Rotation2d.fromDegrees(-45.0); @@ -122,7 +122,7 @@ void testCosineScale() { refI.cosineScale(angleI); assertAll( - () -> assertEquals(0.0, refI.speedMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, refI.speed, kEpsilon), () -> assertEquals(45.0, refI.angle.getDegrees(), kEpsilon)); var angleJ = Rotation2d.fromDegrees(135.0); @@ -130,7 +130,7 @@ void testCosineScale() { refJ.cosineScale(angleJ); assertAll( - () -> assertEquals(0.0, refJ.speedMetersPerSecond, kEpsilon), + () -> assertEquals(0.0, refJ.speed, kEpsilon), () -> assertEquals(45.0, refJ.angle.getDegrees(), kEpsilon)); var angleK = Rotation2d.fromDegrees(-135.0); @@ -138,7 +138,7 @@ void testCosineScale() { refK.cosineScale(angleK); assertAll( - () -> assertEquals(2.0, refK.speedMetersPerSecond, kEpsilon), + () -> assertEquals(2.0, refK.speed, kEpsilon), () -> assertEquals(45.0, refK.angle.getDegrees(), kEpsilon)); var angleL = Rotation2d.fromDegrees(180.0); @@ -146,7 +146,7 @@ void testCosineScale() { refL.cosineScale(angleL); assertAll( - () -> assertEquals(Math.sqrt(2.0), refL.speedMetersPerSecond, kEpsilon), + () -> assertEquals(Math.sqrt(2.0), refL.speed, kEpsilon), () -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java index 055e8e7d5a8..963e0813fe4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java @@ -19,8 +19,8 @@ void testRoundtrip() { ChassisSpeeds.proto.pack(proto, DATA); ChassisSpeeds data = ChassisSpeeds.proto.unpack(proto); - assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond); - assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond); - assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond); + assertEquals(DATA.vx, data.vx); + assertEquals(DATA.vy, data.vy); + assertEquals(DATA.omega, data.omega); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java index 0d426ad2401..a0e3c7fa226 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java @@ -19,6 +19,6 @@ void testRoundtrip() { DifferentialDriveKinematics.proto.pack(proto, DATA); DifferentialDriveKinematics data = DifferentialDriveKinematics.proto.unpack(proto); - assertEquals(DATA.trackWidthMeters, data.trackWidthMeters); + assertEquals(DATA.trackwidth, data.trackwidth); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java index 1f0b237d3fc..af1c40017c8 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java @@ -20,7 +20,7 @@ void testRoundtrip() { DifferentialDriveWheelSpeeds.proto.pack(proto, DATA); DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.proto.unpack(proto); - assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond); - assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond); + assertEquals(DATA.left, data.left); + assertEquals(DATA.right, data.right); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java index 3e4551d176e..4f67312b01e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java @@ -20,9 +20,9 @@ void testRoundtrip() { MecanumDriveWheelPositions.proto.pack(proto, DATA); MecanumDriveWheelPositions data = MecanumDriveWheelPositions.proto.unpack(proto); - assertEquals(DATA.frontLeftMeters, data.frontLeftMeters); - assertEquals(DATA.frontRightMeters, data.frontRightMeters); - assertEquals(DATA.rearLeftMeters, data.rearLeftMeters); - assertEquals(DATA.rearRightMeters, data.rearRightMeters); + assertEquals(DATA.frontLeft, data.frontLeft); + assertEquals(DATA.frontRight, data.frontRight); + assertEquals(DATA.rearLeft, data.rearLeft); + assertEquals(DATA.rearRight, data.rearRight); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java index 3e1d589bcca..dcc4f8c2589 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java @@ -20,9 +20,9 @@ void testRoundtrip() { MecanumDriveWheelSpeeds.proto.pack(proto, DATA); MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.proto.unpack(proto); - assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond); - assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond); - assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond); - assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond); + assertEquals(DATA.frontLeft, data.frontLeft); + assertEquals(DATA.frontRight, data.frontRight); + assertEquals(DATA.rearLeft, data.rearLeft); + assertEquals(DATA.rearRight, data.rearRight); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java index 459d11958e9..108f85ad92e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java @@ -21,7 +21,7 @@ void testRoundtrip() { SwerveModulePosition.proto.pack(proto, DATA); SwerveModulePosition data = SwerveModulePosition.proto.unpack(proto); - assertEquals(DATA.distanceMeters, data.distanceMeters); + assertEquals(DATA.distance, data.distance); assertEquals(DATA.angle, data.angle); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java index 269b7b18b27..3671a953604 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java @@ -20,7 +20,7 @@ void testRoundtrip() { SwerveModuleState.proto.pack(proto, DATA); SwerveModuleState data = SwerveModuleState.proto.unpack(proto); - assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond); + assertEquals(DATA.speed, data.speed); assertEquals(DATA.angle, data.angle); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java index 16ccdb9dfc3..79916f7f95a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java @@ -22,8 +22,8 @@ void testRoundtrip() { buffer.rewind(); ChassisSpeeds data = ChassisSpeeds.struct.unpack(buffer); - assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond); - assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond); - assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond); + assertEquals(DATA.vx, data.vx); + assertEquals(DATA.vy, data.vy); + assertEquals(DATA.omega, data.omega); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java index ffcf7e7c1f0..3a75e2c76cf 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java @@ -22,6 +22,6 @@ void testRoundtrip() { buffer.rewind(); DifferentialDriveKinematics data = DifferentialDriveKinematics.struct.unpack(buffer); - assertEquals(DATA.trackWidthMeters, data.trackWidthMeters); + assertEquals(DATA.trackwidth, data.trackwidth); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java index 84cc9967e1f..eeaa7a40a4b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java @@ -23,7 +23,7 @@ void testRoundtrip() { buffer.rewind(); DifferentialDriveWheelPositions data = DifferentialDriveWheelPositions.struct.unpack(buffer); - assertEquals(DATA.leftMeters, data.leftMeters); - assertEquals(DATA.rightMeters, data.rightMeters); + assertEquals(DATA.left, data.left); + assertEquals(DATA.right, data.right); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java index 720f941c064..e23dc3a54bf 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java @@ -23,7 +23,7 @@ void testRoundtrip() { buffer.rewind(); DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.struct.unpack(buffer); - assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond); - assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond); + assertEquals(DATA.left, data.left); + assertEquals(DATA.right, data.right); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java index 69f8e034011..fb8409b9e46 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java @@ -23,9 +23,9 @@ void testRoundtrip() { buffer.rewind(); MecanumDriveWheelPositions data = MecanumDriveWheelPositions.struct.unpack(buffer); - assertEquals(DATA.frontLeftMeters, data.frontLeftMeters); - assertEquals(DATA.frontRightMeters, data.frontRightMeters); - assertEquals(DATA.rearLeftMeters, data.rearLeftMeters); - assertEquals(DATA.rearRightMeters, data.rearRightMeters); + assertEquals(DATA.frontLeft, data.frontLeft); + assertEquals(DATA.frontRight, data.frontRight); + assertEquals(DATA.rearLeft, data.rearLeft); + assertEquals(DATA.rearRight, data.rearRight); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java index b962acd80aa..e3b2199909f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java @@ -23,9 +23,9 @@ void testRoundtrip() { buffer.rewind(); MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.struct.unpack(buffer); - assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond); - assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond); - assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond); - assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond); + assertEquals(DATA.frontLeft, data.frontLeft); + assertEquals(DATA.frontRight, data.frontRight); + assertEquals(DATA.rearLeft, data.rearLeft); + assertEquals(DATA.rearRight, data.rearRight); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java index 819d98170cf..c45c1f0c3ef 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java @@ -24,7 +24,7 @@ void testRoundtrip() { buffer.rewind(); SwerveModulePosition data = SwerveModulePosition.struct.unpack(buffer); - assertEquals(DATA.distanceMeters, data.distanceMeters); + assertEquals(DATA.distance, data.distance); assertEquals(DATA.angle, data.angle); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java index 625514c04b9..3473525cf4a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java @@ -23,7 +23,7 @@ void testRoundtrip() { buffer.rewind(); SwerveModuleState data = SwerveModuleState.struct.unpack(buffer); - assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond); + assertEquals(DATA.speed, data.speed); assertEquals(DATA.angle, data.angle); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java index 74b06947c7f..f613abcc6c2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java @@ -53,7 +53,7 @@ private void run(Pose2d a, List waypoints, Pose2d b) { var p1 = poses.get(i + 1); // Make sure the twist is under the tolerance defined by the Spline class. - var twist = p0.poseMeters.log(p1.poseMeters); + var twist = p0.pose.log(p1.pose); assertAll( () -> assertTrue(Math.abs(twist.dx) < kMaxDx), () -> assertTrue(Math.abs(twist.dy) < kMaxDy), @@ -62,20 +62,18 @@ private void run(Pose2d a, List waypoints, Pose2d b) { // Check first point assertAll( - () -> assertEquals(a.getX(), poses.get(0).poseMeters.getX(), 1E-9), - () -> assertEquals(a.getY(), poses.get(0).poseMeters.getY(), 1E-9), + () -> assertEquals(a.getX(), poses.get(0).pose.getX(), 1E-9), + () -> assertEquals(a.getY(), poses.get(0).pose.getY(), 1E-9), () -> assertEquals( - a.getRotation().getRadians(), - poses.get(0).poseMeters.getRotation().getRadians(), - 1E-9)); + a.getRotation().getRadians(), poses.get(0).pose.getRotation().getRadians(), 1E-9)); // Check interior waypoints boolean interiorsGood = true; for (var waypoint : waypoints) { boolean found = false; for (var state : poses) { - if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) { + if (waypoint.getDistance(state.pose.getTranslation()) == 0) { found = true; } } @@ -86,12 +84,12 @@ private void run(Pose2d a, List waypoints, Pose2d b) { // Check last point assertAll( - () -> assertEquals(b.getX(), poses.get(poses.size() - 1).poseMeters.getX(), 1E-9), - () -> assertEquals(b.getY(), poses.get(poses.size() - 1).poseMeters.getY(), 1E-9), + () -> assertEquals(b.getX(), poses.get(poses.size() - 1).pose.getX(), 1E-9), + () -> assertEquals(b.getY(), poses.get(poses.size() - 1).pose.getY(), 1E-9), () -> assertEquals( b.getRotation().getRadians(), - poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), + poses.get(poses.size() - 1).pose.getRotation().getRadians(), 1E-9)); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java index 9ae887e4f43..de8835f20ec 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java @@ -39,7 +39,7 @@ private void run(Pose2d a, Pose2d b) { var p1 = poses.get(i + 1); // Make sure the twist is under the tolerance defined by the Spline class. - var twist = p0.poseMeters.log(p1.poseMeters); + var twist = p0.pose.log(p1.pose); assertAll( () -> assertTrue(Math.abs(twist.dx) < kMaxDx), () -> assertTrue(Math.abs(twist.dy) < kMaxDy), @@ -48,22 +48,20 @@ private void run(Pose2d a, Pose2d b) { // Check first point assertAll( - () -> assertEquals(a.getX(), poses.get(0).poseMeters.getX(), 1E-9), - () -> assertEquals(a.getY(), poses.get(0).poseMeters.getY(), 1E-9), + () -> assertEquals(a.getX(), poses.get(0).pose.getX(), 1E-9), + () -> assertEquals(a.getY(), poses.get(0).pose.getY(), 1E-9), () -> assertEquals( - a.getRotation().getRadians(), - poses.get(0).poseMeters.getRotation().getRadians(), - 1E-9)); + a.getRotation().getRadians(), poses.get(0).pose.getRotation().getRadians(), 1E-9)); // Check last point assertAll( - () -> assertEquals(b.getX(), poses.get(poses.size() - 1).poseMeters.getX(), 1E-9), - () -> assertEquals(b.getY(), poses.get(poses.size() - 1).poseMeters.getY(), 1E-9), + () -> assertEquals(b.getX(), poses.get(poses.size() - 1).pose.getX(), 1E-9), + () -> assertEquals(b.getY(), poses.get(poses.size() - 1).pose.getY(), 1E-9), () -> assertEquals( b.getRotation().getRadians(), - poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), + poses.get(poses.size() - 1).pose.getRotation().getRadians(), 1E-9)); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java index 3a6e64a0db8..885ed27eb23 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java @@ -19,13 +19,13 @@ void testRoundtrip() { DCMotor.proto.pack(proto, DATA); DCMotor data = DCMotor.proto.unpack(proto); - assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts); - assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters); - assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps); - assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps); - assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec); - assertEquals(DATA.rOhms, data.rOhms); - assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt); - assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp); + assertEquals(DATA.nominalVoltage, data.nominalVoltage); + assertEquals(DATA.stallTorque, data.stallTorque); + assertEquals(DATA.stallCurrent, data.stallCurrent); + assertEquals(DATA.freeCurrent, data.freeCurrent); + assertEquals(DATA.freeSpeed, data.freeSpeed); + assertEquals(DATA.R, data.R); + assertEquals(DATA.Kv, data.Kv); + assertEquals(DATA.Kt, data.Kt); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java index 806254dc23c..74d47bd06cd 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java @@ -22,13 +22,13 @@ void testRoundtrip() { buffer.rewind(); DCMotor data = DCMotor.struct.unpack(buffer); - assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts); - assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters); - assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps); - assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps); - assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec); - assertEquals(DATA.rOhms, data.rOhms); - assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt); - assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp); + assertEquals(DATA.nominalVoltage, data.nominalVoltage); + assertEquals(DATA.stallTorque, data.stallTorque); + assertEquals(DATA.stallCurrent, data.stallCurrent); + assertEquals(DATA.freeCurrent, data.freeCurrent); + assertEquals(DATA.freeSpeed, data.freeSpeed); + assertEquals(DATA.R, data.R); + assertEquals(DATA.Kv, data.Kv); + assertEquals(DATA.Kt, data.Kt); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java index fa2314edd0a..2232d4ff553 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java @@ -20,14 +20,13 @@ void testCentripetalAccelerationConstraint() { Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint)); - var duration = trajectory.getTotalTimeSeconds(); + var duration = trajectory.getTotalTime(); var t = 0.0; var dt = 0.02; while (t < duration) { var point = trajectory.sample(t); - var centripetalAcceleration = - Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter; + var centripetalAcceleration = Math.pow(point.velocity, 2) * point.curvature; t += dt; assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05); diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java index bcc3c16f42e..54bed80b9f5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java @@ -24,24 +24,20 @@ void testDifferentialDriveKinematicsConstraint() { Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint)); - var duration = trajectory.getTotalTimeSeconds(); + var duration = trajectory.getTotalTime(); var t = 0.0; var dt = 0.02; while (t < duration) { var point = trajectory.sample(t); - var chassisSpeeds = - new ChassisSpeeds( - point.velocityMetersPerSecond, - 0, - point.velocityMetersPerSecond * point.curvatureRadPerMeter); + var chassisSpeeds = new ChassisSpeeds(point.velocity, 0, point.velocity * point.curvature); var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); t += dt; assertAll( - () -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05), - () -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05)); + () -> assertTrue(wheelSpeeds.left <= maxVelocity + 0.05), + () -> assertTrue(wheelSpeeds.right <= maxVelocity + 0.05)); } } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java index 11a47bd71c5..3e1b443f86b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java @@ -30,48 +30,36 @@ void testDifferentialDriveVoltageConstraint() { Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint)); - var duration = trajectory.getTotalTimeSeconds(); + var duration = trajectory.getTotalTime(); var t = 0.0; var dt = 0.02; while (t < duration) { var point = trajectory.sample(t); - var chassisSpeeds = - new ChassisSpeeds( - point.velocityMetersPerSecond, - 0, - point.velocityMetersPerSecond * point.curvatureRadPerMeter); + var chassisSpeeds = new ChassisSpeeds(point.velocity, 0, point.velocity * point.curvature); var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); t += dt; - var acceleration = point.accelerationMetersPerSecondSq; + var acceleration = point.acceleration; // Not really a strictly-correct test as we're using the chassis accel instead of the // wheel accel, but much easier than doing it "properly" and a reasonable check anyway assertAll( () -> assertTrue( - feedforward.calculate( - wheelSpeeds.leftMetersPerSecond, - wheelSpeeds.leftMetersPerSecond + dt * acceleration) + feedforward.calculate(wheelSpeeds.left, wheelSpeeds.left + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward.calculate( - wheelSpeeds.leftMetersPerSecond, - wheelSpeeds.leftMetersPerSecond + dt * acceleration) + feedforward.calculate(wheelSpeeds.left, wheelSpeeds.left + dt * acceleration) >= -maxVoltage - 0.05), () -> assertTrue( - feedforward.calculate( - wheelSpeeds.rightMetersPerSecond, - wheelSpeeds.rightMetersPerSecond + dt * acceleration) + feedforward.calculate(wheelSpeeds.right, wheelSpeeds.right + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward.calculate( - wheelSpeeds.rightMetersPerSecond, - wheelSpeeds.rightMetersPerSecond + dt * acceleration) + feedforward.calculate(wheelSpeeds.right, wheelSpeeds.right + dt * acceleration) >= -maxVoltage - 0.05)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java index 273b67a3f54..6ea18e540da 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java @@ -32,9 +32,9 @@ void testConstraint() { boolean exceededConstraintOutsideRegion = false; for (var point : trajectory.getStates()) { - if (ellipse.contains(point.poseMeters.getTranslation())) { - assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05); - } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) { + if (ellipse.contains(point.pose.getTranslation())) { + assertTrue(Math.abs(point.velocity) < maxVelocity + 0.05); + } else if (Math.abs(point.velocity) >= maxVelocity + 0.05) { exceededConstraintOutsideRegion = true; } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java index b244beacf91..75f9300601f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java @@ -31,9 +31,9 @@ void testConstraint() { boolean exceededConstraintOutsideRegion = false; for (var point : trajectory.getStates()) { - if (rectangle.contains(point.poseMeters.getTranslation())) { - assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05); - } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) { + if (rectangle.contains(point.pose.getTranslation())) { + assertTrue(Math.abs(point.velocity) < maxVelocity + 0.05); + } else if (Math.abs(point.velocity) >= maxVelocity + 0.05) { exceededConstraintOutsideRegion = true; } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java index 6635dfb86a3..b1e07c9b386 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java @@ -36,15 +36,15 @@ void testStates() { var state = t.getStates().get(i); // Make sure that the timestamps are strictly increasing. - assertTrue(state.timeSeconds > time); - time = state.timeSeconds; + assertTrue(state.time > time); + time = state.time; // Ensure that the states in t are the same as those in t1 and t2. if (i < t1.getStates().size()) { assertEquals(state, t1.getStates().get(i)); } else { var st = t2.getStates().get(i - t1.getStates().size() + 1); - st.timeSeconds += t1.getTotalTimeSeconds(); + st.time += t1.getTotalTime(); assertEquals(state, st); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java index 9cc23a07ec7..0e9c6d86471 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java @@ -51,7 +51,7 @@ static Trajectory getTrajectory(List constraints void testGenerationAndConstraints() { Trajectory trajectory = getTrajectory(new ArrayList<>()); - double duration = trajectory.getTotalTimeSeconds(); + double duration = trajectory.getTotalTime(); double t = 0.0; double dt = 0.02; @@ -59,10 +59,8 @@ void testGenerationAndConstraints() { var point = trajectory.sample(t); t += dt; assertAll( - () -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05), - () -> - assertTrue( - Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0) + 0.05)); + () -> assertTrue(Math.abs(point.velocity) < feetToMeters(12.0) + 0.05), + () -> assertTrue(Math.abs(point.acceleration) < feetToMeters(12.0) + 0.05)); } } @@ -74,7 +72,7 @@ void testMalformedTrajectory() { new TrajectoryConfig(feetToMeters(12), feetToMeters(12))); assertEquals(traj.getStates().size(), 1); - assertEquals(traj.getTotalTimeSeconds(), 0); + assertEquals(traj.getTotalTime(), 0); } @Test @@ -90,7 +88,7 @@ void testQuinticCurvatureOptimization() { new TrajectoryConfig(2, 2)); for (int i = 1; i < t.getStates().size() - 1; ++i) { - assertNotEquals(0, t.getStates().get(i).curvatureRadPerMeter); + assertNotEquals(0, t.getStates().get(i).curvature); } } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java index 7f1a9d6622f..08ca37198d2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java @@ -27,7 +27,7 @@ void testTransformBy() { // Test initial pose. assertEquals( - new Pose2d(1, 2, Rotation2d.fromDegrees(30)), transformedTrajectory.sample(0).poseMeters); + new Pose2d(1, 2, Rotation2d.fromDegrees(30)), transformedTrajectory.sample(0).pose); testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates()); } @@ -45,18 +45,18 @@ void testRelativeTo() { var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30))); // Test initial pose. - assertEquals(Pose2d.kZero, transformedTrajectory.sample(0).poseMeters); + assertEquals(Pose2d.kZero, transformedTrajectory.sample(0).pose); testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates()); } void testSameShapedTrajectory(List statesA, List statesB) { for (int i = 0; i < statesA.size() - 1; i++) { - var a1 = statesA.get(i).poseMeters; - var a2 = statesA.get(i + 1).poseMeters; + var a1 = statesA.get(i).pose; + var a2 = statesA.get(i + 1).pose; - var b1 = statesB.get(i).poseMeters; - var b2 = statesB.get(i + 1).poseMeters; + var b1 = statesB.get(i).pose; + var b2 = statesB.get(i + 1).pose; assertEquals(a2.relativeTo(a1), b2.relativeTo(b1)); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java index 8a9fdae5ac9..b5e6402183b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java @@ -24,10 +24,10 @@ void testRoundtrip() { Trajectory.State.proto.pack(proto, DATA); Trajectory.State data = Trajectory.State.proto.unpack(proto); - assertEquals(DATA.timeSeconds, data.timeSeconds); - assertEquals(DATA.velocityMetersPerSecond, data.velocityMetersPerSecond); - assertEquals(DATA.accelerationMetersPerSecondSq, data.accelerationMetersPerSecondSq); - assertEquals(DATA.poseMeters, data.poseMeters); - assertEquals(DATA.curvatureRadPerMeter, data.curvatureRadPerMeter); + assertEquals(DATA.time, data.time); + assertEquals(DATA.velocity, data.velocity); + assertEquals(DATA.acceleration, data.acceleration); + assertEquals(DATA.pose, data.pose); + assertEquals(DATA.curvature, data.curvature); } } diff --git a/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp index 5c9b7507c7e..38323b16f11 100644 --- a/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp @@ -24,6 +24,6 @@ TEST(DifferentialDriveKinematicsProtoTest, Roundtrip) { auto unpacked_data = message.Unpack(buf); ASSERT_TRUE(unpacked_data.has_value()); - EXPECT_EQ(kExpectedData.trackWidth.value(), - unpacked_data->trackWidth.value()); + EXPECT_EQ(kExpectedData.trackwidth.value(), + unpacked_data->trackwidth.value()); } diff --git a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp index 80b4ad565cf..43959927831 100644 --- a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp @@ -22,5 +22,5 @@ TEST(DifferentialDriveKinematicsStructTest, Roundtrip) { DifferentialDriveKinematics unpacked_data = StructType::Unpack(buffer); - EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value()); + EXPECT_EQ(kExpectedData.trackwidth.value(), unpacked_data.trackwidth.value()); } diff --git a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPRangefinder.java b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPRangefinder.java index 70dbce59242..3ac74229e94 100644 --- a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPRangefinder.java +++ b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPRangefinder.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.xrp; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogInput; /** This class represents the ultrasonic rangefinder on an XRP robot. */ @@ -23,16 +22,7 @@ public XRPRangefinder() {} * * @return distance in meters */ - public double getDistanceMeters() { + public double getDistance() { return (m_rangefinder.getVoltage() / 5.0) * 4.0; } - - /** - * Get the measured distance in inches. - * - * @return distance in inches - */ - public double getDistanceInches() { - return Units.metersToInches(getDistanceMeters()); - } }