diff --git a/FissionLib/src/main/java/com/stuyfission/fissionlib/motion/MotionProfiledDcMotor.java b/FissionLib/src/main/java/com/stuyfission/fissionlib/motion/MotionProfiledDcMotor.java index f87b0b0..2c3c15a 100644 --- a/FissionLib/src/main/java/com/stuyfission/fissionlib/motion/MotionProfiledDcMotor.java +++ b/FissionLib/src/main/java/com/stuyfission/fissionlib/motion/MotionProfiledDcMotor.java @@ -185,6 +185,12 @@ public void setTargetPosition(double targetPosition) { profileTimer.reset(); } + public void setTargetPosition(double targetPosition, double retractionMultiplier) { + if (targetPosition < getPosition()) { profile = generateProfile(targetPosition, MAX_VEL * retractionMultiplier, MAX_ACCEL * RETRACTION_MULTIPLIER); } + else { profile = generateProfile(targetPosition, MAX_VEL, MAX_ACCEL); } + profileTimer.reset(); + } + /** * Overrides DcMotor method setTargetPosition(int) to utilize motion profile *