diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index 5171e4d..72e33fb 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java @@ -563,6 +563,11 @@ public class Robot { return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180; } + public double lerp(double min, double max, double t) + { + return min + (max - min) * t; + } + public Status getStatus() { return status; } public double getRadius() { return radius; } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java index e19c613..f54934d 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java @@ -1,5 +1,7 @@ package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; +import com.qualcomm.robotcore.util.Range; + import org.cyberarm.engine.V2.CyberarmState; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.minibots.cyberarm.chiron.Robot; @@ -72,8 +74,7 @@ public class Move extends CyberarmState { } private void moveDirectional(double travelledDistance) { - double velocity = targetVelocity; - double easeVelocity; + double velocity; double ratio = 1.0; if (Math.abs(travelledDistance) < easeInDistance) { @@ -82,10 +83,9 @@ public class Move extends CyberarmState { ratio = (targetDistance - Math.abs(travelledDistance)) / easeOutDistance; } - easeVelocity = targetVelocity * ratio; + ratio = Range.clip(ratio, 0.0, 1.0); - if (easeVelocity < minimumVelocity) { easeVelocity = minimumVelocity; } - if (easeVelocity < targetVelocity) { velocity = easeVelocity; } + velocity = robot.lerp(minimumVelocity, targetVelocity, ratio); if (targetDistance > travelledDistance) { robot.frontRightDrive.setVelocity(-velocity);