diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java index c8de5ba226..57185a9df6 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java @@ -10,6 +10,7 @@ import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Timer; import java.util.function.Consumer; +import java.util.function.Supplier; /** * A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism motion. @@ -19,7 +20,9 @@ import java.util.function.Consumer; public class TrapezoidProfileCommand extends Command { private final TrapezoidProfile m_profile; private final Consumer m_output; - + private final Supplier m_goal; + private final Supplier m_currentState; + private final boolean m_newAPI; // TODO: Remove private final Timer m_timer = new Timer(); /** @@ -28,12 +31,42 @@ public class TrapezoidProfileCommand extends Command { * * @param profile The motion profile to execute. * @param output The consumer for the profile output. + * @param goal The supplier for the desired state + * @param currentState The current state * @param requirements The subsystems required by this command. */ + public TrapezoidProfileCommand( + TrapezoidProfile profile, + Consumer output, + Supplier goal, + Supplier currentState, + Subsystem... requirements) { + m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand"); + m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand"); + m_goal = goal; + m_currentState = currentState; + m_newAPI = true; + addRequirements(requirements); + } + + /** + * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}. + * Output will be piped to the provided consumer function. + * + * @param profile The motion profile to execute. + * @param output The consumer for the profile output. + * @param requirements The subsystems required by this command. + * @deprecated The new constructor allows you to pass in a supplier for desired and current state. + * This allows you to change goals at runtime. + */ + @Deprecated(since = "2024", forRemoval = true) public TrapezoidProfileCommand( TrapezoidProfile profile, Consumer output, Subsystem... requirements) { m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand"); m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand"); + m_newAPI = false; + m_goal = null; + m_currentState = null; addRequirements(requirements); } @@ -43,8 +76,13 @@ public class TrapezoidProfileCommand extends Command { } @Override + @SuppressWarnings("removal") public void execute() { - m_output.accept(m_profile.calculate(m_timer.get())); + if (m_newAPI) { + m_output.accept(m_profile.calculate(m_timer.get(), m_goal.get(), m_currentState.get())); + } else { + m_output.accept(m_profile.calculate(m_timer.get())); + } } @Override diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java index 372e223d34..4362a5e4e6 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java @@ -16,7 +16,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; */ public abstract class TrapezoidProfileSubsystem extends Subsystem { private final double m_period; - private final TrapezoidProfile.Constraints m_constraints; + private final TrapezoidProfile m_profile; private TrapezoidProfile.State m_state; private TrapezoidProfile.State m_goal; @@ -33,7 +33,8 @@ public abstract class TrapezoidProfileSubsystem extends Subsystem { */ public TrapezoidProfileSubsystem( TrapezoidProfile.Constraints constraints, double initialPosition, double period) { - m_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem"); + requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem"); + m_profile = new TrapezoidProfile(constraints); m_state = new TrapezoidProfile.State(initialPosition, 0); setGoal(initialPosition); m_period = period; @@ -62,8 +63,7 @@ public abstract class TrapezoidProfileSubsystem extends Subsystem { @Override public void periodic() { - var profile = new TrapezoidProfile(m_constraints, m_goal, m_state); - m_state = profile.calculate(m_period); + m_state = m_profile.calculate(m_period, m_goal, m_state); if (m_enabled) { useState(m_state); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h index 292bea9dbc..56be97a497 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h @@ -39,13 +39,44 @@ class TrapezoidProfileCommand * * @param profile The motion profile to execute. * @param output The consumer for the profile output. + * @param goal The supplier for the desired state + * @param currentState The current state * @param requirements The list of requirements. */ TrapezoidProfileCommand(frc::TrapezoidProfile profile, std::function output, + std::function goal, + std::function currentState, std::initializer_list requirements) - : m_profile(profile), m_output(output) { + : m_profile(profile), + m_output(output), + m_goal(goal), + m_currentState(currentState) { this->AddRequirements(requirements); + m_newAPI = true; + } + + /** + * Creates a new TrapezoidProfileCommand that will execute the given + * TrapezoidalProfile. Output will be piped to the provided consumer function. + * + * @param profile The motion profile to execute. + * @param output The consumer for the profile output. + * @param goal The supplier for the desired state + * @param currentState The current state + * @param requirements The list of requirements. + */ + TrapezoidProfileCommand(frc::TrapezoidProfile profile, + std::function output, + std::function goal, + std::function currentState, + std::span requirements = {}) + : m_profile(profile), + m_output(output), + m_goal(goal), + m_currentState(currentState) { + this->AddRequirements(requirements); + m_newAPI = true; } /** @@ -55,17 +86,46 @@ class TrapezoidProfileCommand * @param profile The motion profile to execute. * @param output The consumer for the profile output. * @param requirements The list of requirements. + * @deprecated The new constructor allows you to pass in a supplier for + * desired and current state. This allows you to change goals at runtime. */ + WPI_DEPRECATED( + "The new constructor allows you to pass in a supplier for desired and " + "current state. This allows you to change goals at runtime.") + TrapezoidProfileCommand(frc::TrapezoidProfile profile, + std::function output, + std::initializer_list requirements) + : m_profile(profile), m_output(output) { + this->AddRequirements(requirements); + m_newAPI = false; + } + + /** + * Creates a new TrapezoidProfileCommand that will execute the given + * TrapezoidalProfile. Output will be piped to the provided consumer function. + * + * @param profile The motion profile to execute. + * @param output The consumer for the profile output. + * @param requirements The list of requirements. + * @deprecated The new constructor allows you to pass in a supplier for + * desired and current state. This allows you to change goals at runtime. + */ + WPI_DEPRECATED( + "The new constructor allows you to pass in a supplier for desired and " + "current state. This allows you to change goals at runtime.") TrapezoidProfileCommand(frc::TrapezoidProfile profile, std::function output, std::span requirements = {}) : m_profile(profile), m_output(output) { this->AddRequirements(requirements); + m_newAPI = false; } void Initialize() override { m_timer.Restart(); } - void Execute() override { m_output(m_profile.Calculate(m_timer.Get())); } + void Execute() override { + m_output(m_profile.Calculate(m_timer.Get(), m_goal(), m_currentState())); + } void End(bool interrupted) override { m_timer.Stop(); } @@ -76,7 +136,9 @@ class TrapezoidProfileCommand private: frc::TrapezoidProfile m_profile; std::function m_output; - + std::function m_goal; + std::function m_currentState; + bool m_newAPI; // TODO: Remove frc::Timer m_timer; }; diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h index 2686a30aa0..adfc7f1fe4 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h @@ -39,15 +39,13 @@ class TrapezoidProfileSubsystem : public Subsystem { explicit TrapezoidProfileSubsystem(Constraints constraints, Distance_t initialPosition = Distance_t{0}, units::second_t period = 20_ms) - : m_constraints(constraints), + : m_profile(constraints), m_state{initialPosition, Velocity_t(0)}, m_goal{initialPosition, Velocity_t{0}}, m_period(period) {} void Periodic() override { - auto profile = - frc::TrapezoidProfile(m_constraints, m_goal, m_state); - m_state = profile.Calculate(m_period); + m_state = m_profile.Calculate(m_period, m_goal, m_state); if (m_enabled) { UseState(m_state); } @@ -87,7 +85,7 @@ class TrapezoidProfileSubsystem : public Subsystem { void Disable() { m_enabled = false; } private: - Constraints m_constraints; + frc::TrapezoidProfile m_profile; State m_state; State m_goal; units::second_t m_period; diff --git a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp index 3928d3ba24..c8131689ff 100644 --- a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp +++ b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp @@ -13,13 +13,18 @@ ReplaceMeTrapezoidProfileCommand::ReplaceMeTrapezoidProfileCommand() : CommandHelper // The profile to execute - (frc::TrapezoidProfile( - // The maximum velocity and acceleration of the profile - {5_mps, 5_mps_sq}, - // The goal state of the profile - {10_m, 0_mps}, - // The initial state of the profile - {0_m, 0_mps}), - [](frc::TrapezoidProfile::State state) { - // Use the computed intermediate trajectory state here - }) {} + ( + frc::TrapezoidProfile( + // The maximum velocity and acceleration of the profile + {5_mps, 5_mps_sq}), + [](frc::TrapezoidProfile::State state) { + // Use the computed intermediate trajectory state here + }, + // The goal state of the profile + [] { + return frc::TrapezoidProfile::State{10_m, 0_mps}; + }, + // The initial state of the profile + [] { + return frc::TrapezoidProfile::State{0_m, 0_mps}; + }) {} diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp index 8c2423b0f3..bcb7e7367b 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp @@ -40,13 +40,17 @@ void RobotContainer::ConfigureButtonBindings() { frc2::TrapezoidProfileCommand( frc::TrapezoidProfile( // Limit the max acceleration and velocity - {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}, - // End at desired position in meters; implicitly starts at 0 - {3_m, 0_mps}), + {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}), // Pipe the profile state to the drive [this](auto setpointState) { m_drive.SetDriveStates(setpointState, setpointState); }, + // End at desired position in meters; implicitly starts at 0 + [] { + return frc::TrapezoidProfile::State{3_m, 0_mps}; + }, + // Current position + [] { return frc::TrapezoidProfile::State{}; }, // Require the drive {&m_drive}) // Convert to CommandPtr diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp index 6780c3336c..8d45c91b67 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp @@ -13,13 +13,16 @@ DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance, : CommandHelper{ frc::TrapezoidProfile{ // Limit the max acceleration and velocity - {kMaxSpeed, kMaxAcceleration}, - // End at desired position in meters; implicitly starts at 0 - {distance, 0_mps}}, + {kMaxSpeed, kMaxAcceleration}}, // Pipe the profile state to the drive [drive](auto setpointState) { drive->SetDriveStates(setpointState, setpointState); }, + // End at desired position in meters; implicitly starts at 0 + [distance] { + return frc::TrapezoidProfile::State{distance, 0_mps}; + }, + [] { return frc::TrapezoidProfile::State{}; }, // Require the drive {drive}} { // Reset drive encoders since we're starting at 0 diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp index f708143ceb..b1b1389074 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp @@ -33,14 +33,12 @@ class Robot : public frc::TimedRobot { } // Create a motion profile with the given maximum velocity and maximum - // acceleration constraints for the next setpoint, the desired goal, and the - // current setpoint. - frc::TrapezoidProfile profile{m_constraints, m_goal, - m_setpoint}; + // acceleration constraints for the next setpoint. + frc::TrapezoidProfile profile{m_constraints}; // Retrieve the profiled setpoint for the next timestep. This setpoint moves // toward the goal while obeying the constraints. - m_setpoint = profile.Calculate(kDt); + m_setpoint = profile.Calculate(kDt, m_goal, m_setpoint); // Send setpoint to offboard controller PID m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp index aec97381af..f6a9eeaf94 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -86,8 +86,8 @@ class Robot : public frc::TimedRobot { frc::PWMSparkMax m_motor{kMotorPort}; frc::XboxController m_joystick{kJoystickPort}; - frc::TrapezoidProfile::Constraints m_constraints{ - 45_deg_per_s, 90_deg_per_s / 1_s}; + frc::TrapezoidProfile m_profile{ + {45_deg_per_s, 90_deg_per_s / 1_s}}; frc::TrapezoidProfile::State m_lastProfiledReference; @@ -117,9 +117,7 @@ class Robot : public frc::TimedRobot { goal = {kLoweredPosition, 0_rad_per_s}; } m_lastProfiledReference = - (frc::TrapezoidProfile(m_constraints, goal, - m_lastProfiledReference)) - .Calculate(20_ms); + m_profile.Calculate(20_ms, goal, m_lastProfiledReference); m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(), m_lastProfiledReference.velocity.value()}); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp index 018456a9a6..3b83793330 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp @@ -86,8 +86,7 @@ class Robot : public frc::TimedRobot { frc::PWMSparkMax m_motor{kMotorPort}; frc::XboxController m_joystick{kJoystickPort}; - frc::TrapezoidProfile::Constraints m_constraints{3_fps, - 6_fps_sq}; + frc::TrapezoidProfile m_profile{{3_fps, 6_fps_sq}}; frc::TrapezoidProfile::State m_lastProfiledReference; @@ -118,9 +117,7 @@ class Robot : public frc::TimedRobot { goal = {kLoweredPosition, 0_fps}; } m_lastProfiledReference = - (frc::TrapezoidProfile(m_constraints, goal, - m_lastProfiledReference)) - .Calculate(20_ms); + m_profile.Calculate(20_ms, goal, m_lastProfiledReference); m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(), m_lastProfiledReference.velocity.value()}); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java index 7a5571983b..5ed9fda3d2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java @@ -17,13 +17,13 @@ public class ReplaceMeTrapezoidProfileCommand extends TrapezoidProfileCommand { // The motion profile to be executed new TrapezoidProfile( // The motion profile constraints - new TrapezoidProfile.Constraints(0, 0), - // Goal state - new TrapezoidProfile.State(), - // Initial state - new TrapezoidProfile.State()), + new TrapezoidProfile.Constraints(0, 0)), state -> { // Use current trajectory state here - }); + }, + // Goal state + () -> new TrapezoidProfile.State(), + // Current state + () -> new TrapezoidProfile.State()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java index 6bc4b13c1f..ab71ff442f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java @@ -72,11 +72,13 @@ public class RobotContainer { // Limit the max acceleration and velocity new TrapezoidProfile.Constraints( DriveConstants.kMaxSpeedMetersPerSecond, - DriveConstants.kMaxAccelerationMetersPerSecondSquared), - // End at desired position in meters; implicitly starts at 0 - new TrapezoidProfile.State(3, 0)), + DriveConstants.kMaxAccelerationMetersPerSecondSquared)), // Pipe the profile state to the drive setpointState -> m_robotDrive.setDriveStates(setpointState, setpointState), + // End at desired position in meters; implicitly starts at 0 + () -> new TrapezoidProfile.State(3, 0), + // Current position + () -> new TrapezoidProfile.State(), // Require the drive m_robotDrive) .beforeStarting(m_robotDrive::resetEncoders) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java index 5bfa10d177..5c107d2375 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java @@ -23,11 +23,13 @@ public class DriveDistanceProfiled extends TrapezoidProfileCommand { // Limit the max acceleration and velocity new TrapezoidProfile.Constraints( DriveConstants.kMaxSpeedMetersPerSecond, - DriveConstants.kMaxAccelerationMetersPerSecondSquared), - // End at desired position in meters; implicitly starts at 0 - new TrapezoidProfile.State(meters, 0)), + DriveConstants.kMaxAccelerationMetersPerSecondSquared)), // Pipe the profile state to the drive setpointState -> drive.setDriveStates(setpointState, setpointState), + // End at desired position in meters; implicitly starts at 0 + () -> new TrapezoidProfile.State(meters, 0), + // Current position + () -> new TrapezoidProfile.State(), // Require the drive drive); // Reset drive encoders since we're starting at 0 diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java index d459eeb899..d6c25d6396 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java @@ -33,17 +33,16 @@ public class Robot extends TimedRobot { if (m_joystick.getRawButtonPressed(2)) { m_goal = new TrapezoidProfile.State(5, 0); } else if (m_joystick.getRawButtonPressed(3)) { - m_goal = new TrapezoidProfile.State(0, 0); + m_goal = new TrapezoidProfile.State(); } // Create a motion profile with the given maximum velocity and maximum - // acceleration constraints for the next setpoint, the desired goal, and the - // current setpoint. - var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint); + // acceleration constraints for the next setpoint. + var profile = new TrapezoidProfile(m_constraints); // Retrieve the profiled setpoint for the next timestep. This setpoint moves // toward the goal while obeying the constraints. - m_setpoint = profile.calculate(kDt); + m_setpoint = profile.calculate(kDt, m_goal, m_setpoint); // Send setpoint to offboard controller PID m_motor.setSetpoint( diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java index 9877906c05..69997d17f1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java @@ -41,10 +41,11 @@ public class Robot extends TimedRobot { // the motors, this number should be greater than one. private static final double kArmGearing = 10.0; - private final TrapezoidProfile.Constraints m_constraints = - new TrapezoidProfile.Constraints( - Units.degreesToRadians(45), - Units.degreesToRadians(90)); // Max arm speed and acceleration. + private final TrapezoidProfile m_profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + Units.degreesToRadians(45), + Units.degreesToRadians(90))); // Max arm speed and acceleration. private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State(); // The plant holds a state-space model of our arm. This system has the following properties: @@ -125,10 +126,8 @@ public class Robot extends TimedRobot { goal = new TrapezoidProfile.State(kLoweredPosition, 0.0); } // Step our TrapezoidalProfile forward 20ms and set it as our next reference - m_lastProfiledReference = - (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020); + m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference); m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity); - // Correct our Kalman filter's state vector estimate with encoder data. m_loop.correct(VecBuilder.fill(m_encoder.getDistance())); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java index f1aca19c81..20695212cb 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java @@ -43,9 +43,11 @@ public class Robot extends TimedRobot { // the motors, this number should be greater than one. private static final double kElevatorGearing = 6.0; - private final TrapezoidProfile.Constraints m_constraints = - new TrapezoidProfile.Constraints( - Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration. + private final TrapezoidProfile m_profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + Units.feetToMeters(3.0), + Units.feetToMeters(6.0))); // Max elevator speed and acceleration. private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State(); /* The plant holds a state-space model of our elevator. This system has the following properties: @@ -128,8 +130,7 @@ public class Robot extends TimedRobot { goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0); } // Step our TrapezoidalProfile forward 20ms and set it as our next reference - m_lastProfiledReference = - (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020); + m_lastProfiledReference = m_profile.calculate(0.020, goal, m_lastProfiledReference); m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity); // Correct our Kalman filter's state vector estimate with encoder data. diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java index 7637fabe13..6a3862f4fc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java @@ -343,8 +343,8 @@ public class ProfiledPIDController implements Sendable { m_setpoint.position = setpointMinDistance + measurement; } - var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint); - m_setpoint = profile.calculate(getPeriod()); + var profile = new TrapezoidProfile(m_constraints); + m_setpoint = profile.calculate(getPeriod(), m_goal, m_setpoint); return m_controller.calculate(measurement, m_setpoint.position); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java index 3b6559ef7d..fd7494ca25 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java @@ -22,14 +22,14 @@ import java.util.Objects; * new TrapezoidProfile.Constraints(kMaxV, kMaxA); * TrapezoidProfile.State previousProfiledReference = * new TrapezoidProfile.State(initialReference, 0.0); + * TrapezoidProfile profile = new TrapezoidProfile(constraints); * * *

Run on update: * * 


- * TrapezoidProfile profile =
- *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
- * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * previousProfiledReference =
+ * profile.calculate(timeSincePreviousUpdate, unprofiledReference, previousProfiledReference);
  *
* *

where `unprofiledReference` is free to change between calls. Note that when the unprofiled @@ -42,9 +42,10 @@ public class TrapezoidProfile { // The direction of the profile, either 1 for forwards or -1 for inverted private int m_direction; - private Constraints m_constraints; - private State m_initial; - private State m_goal; + private final Constraints m_constraints; + private State m_current; + private State m_goal; // TODO: Remove + private final boolean m_newAPI; // TODO: Remove private double m_endAccel; private double m_endFullSpeed; @@ -96,27 +97,40 @@ public class TrapezoidProfile { } } + /** + * Construct a TrapezoidProfile. + * + * @param constraints The constraints on the profile, like maximum velocity. + */ + public TrapezoidProfile(Constraints constraints) { + m_constraints = constraints; + m_newAPI = true; + } + /** * Construct a TrapezoidProfile. * * @param constraints The constraints on the profile, like maximum velocity. * @param goal The desired state when the profile is complete. * @param initial The initial state (usually the current state). + * @deprecated Pass the desired and current state into calculate instead of constructing a new + * TrapezoidProfile with the desired and current state */ + @Deprecated(since = "2024", forRemoval = true) public TrapezoidProfile(Constraints constraints, State goal, State initial) { m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1; m_constraints = constraints; - m_initial = direct(initial); + m_current = direct(initial); m_goal = direct(goal); - - if (m_initial.velocity > m_constraints.maxVelocity) { - m_initial.velocity = m_constraints.maxVelocity; + m_newAPI = false; + if (m_current.velocity > m_constraints.maxVelocity) { + m_current.velocity = m_constraints.maxVelocity; } // Deal with a possibly truncated motion profile (with nonzero initial or // final velocity) by calculating the parameters as if the profile began and // ended at zero velocity - double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration; + double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration; double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration; @@ -126,7 +140,7 @@ public class TrapezoidProfile { // of a truncated one double fullTrapezoidDist = - cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd; + cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd; double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration; double fullSpeedDist = @@ -148,7 +162,10 @@ public class TrapezoidProfile { * * @param constraints The constraints on the profile, like maximum velocity. * @param goal The desired state when the profile is complete. + * @deprecated Pass the desired and current state into calculate instead of constructing a new + * TrapezoidProfile with the desired and current state */ + @Deprecated(since = "2024", forRemoval = true) public TrapezoidProfile(Constraints constraints, State goal) { this(constraints, goal, new State(0, 0)); } @@ -159,17 +176,23 @@ public class TrapezoidProfile { * * @param t The time since the beginning of the profile. * @return The position and velocity of the profile at time t. + * @deprecated Pass the desired and current state into calculate instead of constructing a new + * TrapezoidProfile with the desired and current state */ + @Deprecated(since = "2024", forRemoval = true) public State calculate(double t) { - State result = new State(m_initial.position, m_initial.velocity); + if (m_newAPI) { + throw new RuntimeException("Cannot use new constructor with deprecated calculate()"); + } + State result = new State(m_current.position, m_current.velocity); if (t < m_endAccel) { result.velocity += t * m_constraints.maxAcceleration; - result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t; + result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; } else if (t < m_endFullSpeed) { result.velocity = m_constraints.maxVelocity; result.position += - (m_initial.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel + (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel + m_constraints.maxVelocity * (t - m_endAccel); } else if (t <= m_endDeccel) { result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration; @@ -184,6 +207,75 @@ public class TrapezoidProfile { return direct(result); } + /** + * Calculate the correct position and velocity for the profile at a time t where the beginning of + * the profile was at time t = 0. + * + * @param t The time since the beginning of the profile. + * @param goal The desired state when the profile is complete. + * @param current The current state. + * @return The position and velocity of the profile at time t. + */ + public State calculate(double t, State goal, State current) { + m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1; + m_current = direct(current); + goal = direct(goal); + + if (m_current.velocity > m_constraints.maxVelocity) { + m_current.velocity = m_constraints.maxVelocity; + } + + // Deal with a possibly truncated motion profile (with nonzero initial or + // final velocity) by calculating the parameters as if the profile began and + // ended at zero velocity + double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration; + double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; + + double cutoffEnd = goal.velocity / m_constraints.maxAcceleration; + double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; + + // Now we can calculate the parameters as if it was a full trapezoid instead + // of a truncated one + + double fullTrapezoidDist = + cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd; + double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration; + + double fullSpeedDist = + fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration; + + // Handle the case where the profile never reaches full speed + if (fullSpeedDist < 0) { + accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration); + fullSpeedDist = 0; + } + + m_endAccel = accelerationTime - cutoffBegin; + m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; + m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd; + State result = new State(m_current.position, m_current.velocity); + + if (t < m_endAccel) { + result.velocity += t * m_constraints.maxAcceleration; + result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; + } else if (t < m_endFullSpeed) { + result.velocity = m_constraints.maxVelocity; + result.position += + (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel + + m_constraints.maxVelocity * (t - m_endAccel); + } else if (t <= m_endDeccel) { + result.velocity = goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration; + double timeLeft = m_endDeccel - t; + result.position = + goal.position + - (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft; + } else { + result = goal; + } + + return direct(result); + } + /** * Returns the time left until a target distance in the profile is reached. * @@ -191,8 +283,8 @@ public class TrapezoidProfile { * @return The time left until a target distance in the profile is reached. */ public double timeLeftUntil(double target) { - double position = m_initial.position * m_direction; - double velocity = m_initial.velocity * m_direction; + double position = m_current.position * m_direction; + double velocity = m_current.velocity * m_direction; double endAccel = m_endAccel * m_direction; double endFullSpeed = m_endFullSpeed * m_direction - endAccel; diff --git a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h index 92d2b72f9c..2db3fa5477 100644 --- a/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h +++ b/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h @@ -317,8 +317,8 @@ class ProfiledPIDController m_setpoint.position = setpointMinDistance + measurement; } - frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint}; - m_setpoint = profile.Calculate(GetPeriod()); + frc::TrapezoidProfile profile{m_constraints}; + m_setpoint = profile.Calculate(GetPeriod(), m_goal, m_setpoint); return m_controller.Calculate(measurement.value(), m_setpoint.position.value()); } diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h index 24a8253f7d..73aab38506 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h @@ -4,6 +4,8 @@ #pragma once +#include + #include "units/time.h" #include "wpimath/MathShared.h" @@ -21,13 +23,14 @@ namespace frc { * @code{.cpp} * TrapezoidProfile::Constraints constraints{kMaxV, kMaxA}; * double previousProfiledReference = initialReference; + * TrapezoidProfile profile{constraints}; * @endcode * * Run on update: * @code{.cpp} - * TrapezoidProfile profile{constraints, unprofiledReference, - * previousProfiledReference}; - * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate); + * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate, + * unprofiledReference, + * previousProfiledReference); * @endcode * * where `unprofiledReference` is free to change between calls. Note that when @@ -71,13 +74,26 @@ class TrapezoidProfile { bool operator==(const State&) const = default; }; + /** + * Construct a TrapezoidProfile. + * + * @param constraints The constraints on the profile, like maximum velocity. + */ + TrapezoidProfile(Constraints constraints); // NOLINT + /** * Construct a TrapezoidProfile. * * @param constraints The constraints on the profile, like maximum velocity. * @param goal The desired state when the profile is complete. * @param initial The initial state (usually the current state). + * @deprecated Pass the desired and current state into calculate instead of + * constructing a new TrapezoidProfile with the desired and current state */ + WPI_DEPRECATED( + "Pass the desired and current state into calculate instead of " + "constructing a new TrapezoidProfile with the desired and current " + "state") TrapezoidProfile(Constraints constraints, State goal, State initial = State{Distance_t{0}, Velocity_t{0}}); @@ -91,9 +107,25 @@ class TrapezoidProfile { * where the beginning of the profile was at time t = 0. * * @param t The time since the beginning of the profile. + * @deprecated Pass the desired and current state into calculate instead of + * constructing a new TrapezoidProfile with the desired and current state */ + [[deprecated( + "Pass the desired and current state into calculate instead of " + "constructing a new TrapezoidProfile with the desired and current " + "state")]] State Calculate(units::second_t t) const; + /** + * Calculate the correct position and velocity for the profile at a time t + * where the beginning of the profile was at time t = 0. + * + * @param t The time since the beginning of the profile. + * @param goal The desired state when the profile is complete. + * @param current The initial state (usually the current state). + */ + State Calculate(units::second_t t, State goal, State current); + /** * Returns the time left until a target distance in the profile is reached. * @@ -141,8 +173,9 @@ class TrapezoidProfile { int m_direction; Constraints m_constraints; - State m_initial; - State m_goal; + State m_current; + State m_goal; // TODO: remove + bool m_newAPI; // TODO: remove units::second_t m_endAccel; units::second_t m_endFullSpeed; diff --git a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc index 19eb1f3b1d..24e0a46b80 100644 --- a/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc +++ b/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc @@ -10,22 +10,27 @@ #include "units/math.h" namespace frc { +template +TrapezoidProfile::TrapezoidProfile(Constraints constraints) + : m_constraints(constraints), m_newAPI(true) {} + template TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal, State initial) : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1}, m_constraints(constraints), - m_initial(Direct(initial)), - m_goal(Direct(goal)) { - if (m_initial.velocity > m_constraints.maxVelocity) { - m_initial.velocity = m_constraints.maxVelocity; + m_current(Direct(initial)), + m_goal(Direct(goal)), + m_newAPI(false) { + if (m_current.velocity > m_constraints.maxVelocity) { + m_current.velocity = m_constraints.maxVelocity; } // Deal with a possibly truncated motion profile (with nonzero initial or // final velocity) by calculating the parameters as if the profile began and // ended at zero velocity units::second_t cutoffBegin = - m_initial.velocity / m_constraints.maxAcceleration; + m_current.velocity / m_constraints.maxAcceleration; Distance_t cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; @@ -37,7 +42,7 @@ TrapezoidProfile::TrapezoidProfile(Constraints constraints, // of a truncated one Distance_t fullTrapezoidDist = - cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd; + cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd; units::second_t accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration; @@ -60,15 +65,19 @@ TrapezoidProfile::TrapezoidProfile(Constraints constraints, template typename TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const { - State result = m_initial; + if (m_newAPI) { + throw std::runtime_error( + "Cannot use new constructor with deprecated Calculate()"); + } + State result = m_current; if (t < m_endAccel) { result.velocity += t * m_constraints.maxAcceleration; result.position += - (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t; + (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; } else if (t < m_endFullSpeed) { result.velocity = m_constraints.maxVelocity; - result.position += (m_initial.velocity + + result.position += (m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel + m_constraints.maxVelocity * (t - m_endAccel); @@ -86,12 +95,83 @@ TrapezoidProfile::Calculate(units::second_t t) const { return Direct(result); } +template +typename TrapezoidProfile::State +TrapezoidProfile::Calculate(units::second_t t, State goal, + State current) { + m_direction = ShouldFlipAcceleration(current, goal) ? -1 : 1; + m_current = Direct(current); + goal = Direct(goal); + if (m_current.velocity > m_constraints.maxVelocity) { + m_current.velocity = m_constraints.maxVelocity; + } + + // Deal with a possibly truncated motion profile (with nonzero initial or + // final velocity) by calculating the parameters as if the profile began and + // ended at zero velocity + units::second_t cutoffBegin = + m_current.velocity / m_constraints.maxAcceleration; + Distance_t cutoffDistBegin = + cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0; + + units::second_t cutoffEnd = goal.velocity / m_constraints.maxAcceleration; + Distance_t cutoffDistEnd = + cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0; + + // Now we can calculate the parameters as if it was a full trapezoid instead + // of a truncated one + + Distance_t fullTrapezoidDist = + cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd; + units::second_t accelerationTime = + m_constraints.maxVelocity / m_constraints.maxAcceleration; + + Distance_t fullSpeedDist = + fullTrapezoidDist - + accelerationTime * accelerationTime * m_constraints.maxAcceleration; + + // Handle the case where the profile never reaches full speed + if (fullSpeedDist < Distance_t{0}) { + accelerationTime = + units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration); + fullSpeedDist = Distance_t{0}; + } + + m_endAccel = accelerationTime - cutoffBegin; + m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity; + m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd; + State result = m_current; + + if (t < m_endAccel) { + result.velocity += t * m_constraints.maxAcceleration; + result.position += + (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t; + } else if (t < m_endFullSpeed) { + result.velocity = m_constraints.maxVelocity; + result.position += (m_current.velocity + + m_endAccel * m_constraints.maxAcceleration / 2.0) * + m_endAccel + + m_constraints.maxVelocity * (t - m_endAccel); + } else if (t <= m_endDeccel) { + result.velocity = + goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration; + units::second_t timeLeft = m_endDeccel - t; + result.position = + goal.position - + (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * + timeLeft; + } else { + result = goal; + } + + return Direct(result); +} template units::second_t TrapezoidProfile::TimeLeftUntil( Distance_t target) const { - Distance_t position = m_initial.position * m_direction; - Velocity_t velocity = m_initial.velocity * m_direction; + Distance_t position = m_current.position * m_direction; + Velocity_t velocity = m_current.velocity * m_direction; units::second_t endAccel = m_endAccel * m_direction; units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel; diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java index 7b8484dcab..7ea8caa715 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java @@ -58,12 +58,12 @@ class LinearSystemLoopTest { TrapezoidProfile profile; TrapezoidProfile.State state; for (int i = 0; i < 1000; i++) { - profile = - new TrapezoidProfile( - constraints, + profile = new TrapezoidProfile(constraints); + state = + profile.calculate( + kDt, new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)), new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))); - state = profile.calculate(kDt); m_loop.setNextR(VecBuilder.fill(state.position, state.velocity)); updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev); diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java index ee6cc8ffb8..fb28b6957f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java @@ -57,9 +57,9 @@ class TrapezoidProfileTest { TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0); TrapezoidProfile.State state = new TrapezoidProfile.State(); + TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 450; ++i) { - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); } assertEquals(state, goal); } @@ -67,21 +67,21 @@ class TrapezoidProfileTest { // Tests that decreasing the maximum velocity in the middle when it is already // moving faster than the new max is handled correctly @Test - void posContinousUnderVelChange() { + void posContinuousUnderVelChange() { TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75); TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0); - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal); - TrapezoidProfile.State state = profile.calculate(kDt); + TrapezoidProfile profile = new TrapezoidProfile(constraints); + TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State()); double lastPos = state.position; for (int i = 0; i < 1600; ++i) { if (i == 400) { constraints = new TrapezoidProfile.Constraints(0.75, 0.75); + profile = new TrapezoidProfile(constraints); } - profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); double estimatedVel = (state.position - lastPos) / kDt; if (i >= 400) { @@ -105,9 +105,9 @@ class TrapezoidProfileTest { TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0); TrapezoidProfile.State state = new TrapezoidProfile.State(); + TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 400; ++i) { - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); } assertEquals(state, goal); } @@ -118,16 +118,16 @@ class TrapezoidProfileTest { TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0); TrapezoidProfile.State state = new TrapezoidProfile.State(); + TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 200; ++i) { - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); } assertNotEquals(state, goal); goal = new TrapezoidProfile.State(0.0, 0.0); + profile = new TrapezoidProfile(constraints); for (int i = 0; i < 550; ++i) { - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); } assertEquals(state, goal); } @@ -139,15 +139,15 @@ class TrapezoidProfileTest { TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0); TrapezoidProfile.State state = new TrapezoidProfile.State(); + TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 200; ++i) { - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); } assertNear(constraints.maxVelocity, state.velocity, 10e-5); + profile = new TrapezoidProfile(constraints); for (int i = 0; i < 2000; ++i) { - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); } assertEquals(state, goal); } @@ -158,9 +158,9 @@ class TrapezoidProfileTest { TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0); TrapezoidProfile.State state = new TrapezoidProfile.State(); + TrapezoidProfile profile = new TrapezoidProfile(constraints); for (int i = 0; i < 400; i++) { - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); assertNear(profile.timeLeftUntil(state.position), 0, 2e-2); } } @@ -170,14 +170,13 @@ class TrapezoidProfileTest { TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75); TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0); - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal); - TrapezoidProfile.State state = profile.calculate(kDt); + TrapezoidProfile profile = new TrapezoidProfile(constraints); + TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State()); double predictedTimeLeft = profile.timeLeftUntil(goal.position); boolean reachedGoal = false; for (int i = 0; i < 400; i++) { - profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); if (!reachedGoal && state.equals(goal)) { // Expected value using for loop index is just an approximation since // the time left in the profile doesn't increase linearly at the @@ -193,14 +192,13 @@ class TrapezoidProfileTest { TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75); TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0); - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal); - TrapezoidProfile.State state = profile.calculate(kDt); + TrapezoidProfile profile = new TrapezoidProfile(constraints); + TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State()); double predictedTimeLeft = profile.timeLeftUntil(1); boolean reachedGoal = false; for (int i = 0; i < 400; i++) { - profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); if (!reachedGoal && Math.abs(state.velocity - 1) < 10e-5) { assertNear(predictedTimeLeft, i / 100.0, 2e-2); reachedGoal = true; @@ -213,14 +211,13 @@ class TrapezoidProfileTest { TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75); TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0); - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal); - TrapezoidProfile.State state = profile.calculate(kDt); + TrapezoidProfile profile = new TrapezoidProfile(constraints); + TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State()); double predictedTimeLeft = profile.timeLeftUntil(goal.position); boolean reachedGoal = false; for (int i = 0; i < 400; i++) { - profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); if (!reachedGoal && state.equals(goal)) { // Expected value using for loop index is just an approximation since // the time left in the profile doesn't increase linearly at the @@ -236,14 +233,13 @@ class TrapezoidProfileTest { TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75); TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0); - TrapezoidProfile profile = new TrapezoidProfile(constraints, goal); - TrapezoidProfile.State state = profile.calculate(kDt); + TrapezoidProfile profile = new TrapezoidProfile(constraints); + TrapezoidProfile.State state = profile.calculate(kDt, goal, new TrapezoidProfile.State()); double predictedTimeLeft = profile.timeLeftUntil(-1); boolean reachedGoal = false; for (int i = 0; i < 400; i++) { - profile = new TrapezoidProfile(constraints, goal, state); - state = profile.calculate(kDt); + state = profile.calculate(kDt, goal, state); if (!reachedGoal && Math.abs(state.velocity + 1) < 10e-5) { assertNear(predictedTimeLeft, i / 100.0, 2e-2); reachedGoal = true; diff --git a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp index 6a35261108..b9a66969d9 100644 --- a/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp @@ -31,9 +31,9 @@ TEST(TrapezoidProfileTest, ReachesGoal) { frc::TrapezoidProfile::State goal{3_m, 0_mps}; frc::TrapezoidProfile::State state; + frc::TrapezoidProfile profile{constraints}; for (int i = 0; i < 450; ++i) { - frc::TrapezoidProfile profile{constraints, goal, state}; - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); } EXPECT_EQ(state, goal); } @@ -45,17 +45,18 @@ TEST(TrapezoidProfileTest, PosContinousUnderVelChange) { 0.75_mps_sq}; frc::TrapezoidProfile::State goal{12_m, 0_mps}; - frc::TrapezoidProfile profile{constraints, goal}; - auto state = profile.Calculate(kDt); + frc::TrapezoidProfile profile{constraints}; + auto state = profile.Calculate(kDt, goal, + frc::TrapezoidProfile::State{}); auto lastPos = state.position; for (int i = 0; i < 1600; ++i) { if (i == 400) { constraints.maxVelocity = 0.75_mps; + profile = frc::TrapezoidProfile{constraints}; } - profile = frc::TrapezoidProfile{constraints, goal, state}; - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); auto estimatedVel = (state.position - lastPos) / kDt; if (i >= 400) { @@ -79,9 +80,9 @@ TEST(TrapezoidProfileTest, Backwards) { frc::TrapezoidProfile::State goal{-2_m, 0_mps}; frc::TrapezoidProfile::State state; + frc::TrapezoidProfile profile{constraints}; for (int i = 0; i < 400; ++i) { - frc::TrapezoidProfile profile{constraints, goal, state}; - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); } EXPECT_EQ(state, goal); } @@ -92,16 +93,16 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) { frc::TrapezoidProfile::State goal{-2_m, 0_mps}; frc::TrapezoidProfile::State state; + frc::TrapezoidProfile profile{constraints}; for (int i = 0; i < 200; ++i) { - frc::TrapezoidProfile profile{constraints, goal, state}; - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); } EXPECT_NE(state, goal); goal = {0.0_m, 0.0_mps}; + profile = frc::TrapezoidProfile{constraints}; for (int i = 0; i < 550; ++i) { - frc::TrapezoidProfile profile{constraints, goal, state}; - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); } EXPECT_EQ(state, goal); } @@ -113,15 +114,15 @@ TEST(TrapezoidProfileTest, TopSpeed) { frc::TrapezoidProfile::State goal{4_m, 0_mps}; frc::TrapezoidProfile::State state; + frc::TrapezoidProfile profile{constraints}; for (int i = 0; i < 200; ++i) { - frc::TrapezoidProfile profile{constraints, goal, state}; - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); } EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps); + profile = frc::TrapezoidProfile{constraints}; for (int i = 0; i < 2000; ++i) { - frc::TrapezoidProfile profile{constraints, goal, state}; - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); } EXPECT_EQ(state, goal); } @@ -132,9 +133,9 @@ TEST(TrapezoidProfileTest, TimingToCurrent) { frc::TrapezoidProfile::State goal{2_m, 0_mps}; frc::TrapezoidProfile::State state; + frc::TrapezoidProfile profile{constraints}; for (int i = 0; i < 400; i++) { - frc::TrapezoidProfile profile{constraints, goal, state}; - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s); } } @@ -146,14 +147,14 @@ TEST(TrapezoidProfileTest, TimingToGoal) { 0.75_mps_sq}; frc::TrapezoidProfile::State goal{2_m, 0_mps}; - frc::TrapezoidProfile profile{constraints, goal}; - auto state = profile.Calculate(kDt); + frc::TrapezoidProfile profile{constraints}; + auto state = profile.Calculate(kDt, goal, + frc::TrapezoidProfile::State{}); auto predictedTimeLeft = profile.TimeLeftUntil(goal.position); bool reachedGoal = false; for (int i = 0; i < 400; i++) { - profile = frc::TrapezoidProfile(constraints, goal, state); - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); if (!reachedGoal && state == goal) { // Expected value using for loop index is just an approximation since the // time left in the profile doesn't increase linearly at the endpoints @@ -170,14 +171,14 @@ TEST(TrapezoidProfileTest, TimingBeforeGoal) { 0.75_mps_sq}; frc::TrapezoidProfile::State goal{2_m, 0_mps}; - frc::TrapezoidProfile profile{constraints, goal}; - auto state = profile.Calculate(kDt); + frc::TrapezoidProfile profile{constraints}; + auto state = profile.Calculate(kDt, goal, + frc::TrapezoidProfile::State{}); auto predictedTimeLeft = profile.TimeLeftUntil(1_m); bool reachedGoal = false; for (int i = 0; i < 400; i++) { - profile = frc::TrapezoidProfile(constraints, goal, state); - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); if (!reachedGoal && (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) { EXPECT_NEAR(unit_cast(predictedTimeLeft), i / 100.0, 2e-2); @@ -193,14 +194,14 @@ TEST(TrapezoidProfileTest, TimingToNegativeGoal) { 0.75_mps_sq}; frc::TrapezoidProfile::State goal{-2_m, 0_mps}; - frc::TrapezoidProfile profile{constraints, goal}; - auto state = profile.Calculate(kDt); + frc::TrapezoidProfile profile{constraints}; + auto state = profile.Calculate(kDt, goal, + frc::TrapezoidProfile::State{}); auto predictedTimeLeft = profile.TimeLeftUntil(goal.position); bool reachedGoal = false; for (int i = 0; i < 400; i++) { - profile = frc::TrapezoidProfile(constraints, goal, state); - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); if (!reachedGoal && state == goal) { // Expected value using for loop index is just an approximation since the // time left in the profile doesn't increase linearly at the endpoints @@ -217,14 +218,14 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) { 0.75_mps_sq}; frc::TrapezoidProfile::State goal{-2_m, 0_mps}; - frc::TrapezoidProfile profile{constraints, goal}; - auto state = profile.Calculate(kDt); + frc::TrapezoidProfile profile{constraints}; + auto state = profile.Calculate(kDt, goal, + frc::TrapezoidProfile::State{}); auto predictedTimeLeft = profile.TimeLeftUntil(-1_m); bool reachedGoal = false; for (int i = 0; i < 400; i++) { - profile = frc::TrapezoidProfile(constraints, goal, state); - state = profile.Calculate(kDt); + state = profile.Calculate(kDt, goal, state); if (!reachedGoal && (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) { EXPECT_NEAR(unit_cast(predictedTimeLeft), i / 100.0, 2e-2);