Merge pull request #60 from flamingchickens1540/motion-profiling

Add interrupted, override annotations to RunMotionProfiles
flamingchickens1540 · Mar 7, 2018 · 1eeaf0e · 1eeaf0e
2 parents fcee0ce + 01ad1ae
commit 1eeaf0e
Showing 1 changed file with 13 additions and 0 deletions.
diff --git 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
    *