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

[wpimath] Refactor TrapezoidProfile API (#5457)

Browse Source
This commit is contained in:
Gold856
2023-07-19 20:25:10 -04:00
committed by GitHub
parent 72a4543493
commit 86e91e6724
24 changed files with 492 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
View File

@@ -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<State> m_output;
private final Supplier<State> m_goal;
private final Supplier<State> 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<State> output,
Supplier<State> goal,
Supplier<State> 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<State> 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

8
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
View File

@@ -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);
}