diff --git a/src/main/java/org/team1540/base/motionprofiling/RunMotionProfiles.java b/src/main/java/org/team1540/base/motionprofiling/RunMotionProfiles.java index 4eff0c3b..6844206d 100644 --- a/src/main/java/org/team1540/base/motionprofiling/RunMotionProfiles.java +++ b/src/main/java/org/team1540/base/motionprofiling/RunMotionProfiles.java @@ -47,12 +47,14 @@ public void setMotionProfiles( this.motionProfiles = motionProfiles; } + @Override protected void initialize() { timer.start(); lastTime = timer.get(); isFinished = false; } + @Override protected void execute() { for (MotionProfilingProperties currentProperty : motionProfiles) { // Each controller's setpoint is calculated at a slightly different time, but this doesn't @@ -84,6 +86,17 @@ private double getVelocitySetpoint(MotionProfilingProperties currentProperty, do return thisTrajectory.segments[index].velocity / encoderMultiplier * 0.1; } + /** + * Sets the velocity to 0 for all properties and sets isFinished to true. + */ + @Override + protected void interrupted() { + for (MotionProfilingProperties currentProperty : motionProfiles) { + currentProperty.getSetMotorVelocityFunction().accept(0); + } + isFinished = true; + } + /** * Calculate if a number is between (inclusive) two other numbers *