diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/KOdometryDrive.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/KOdometryDrive.kt index 919f8ef3839c..1ec1a7f9eb6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/KOdometryDrive.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/KOdometryDrive.kt @@ -140,7 +140,7 @@ class KOdometryDrive( val correction = (kpFwd * pError + kiFwd * sumError) * switcher // FIXME timeoutT.time() - val rampBase = ForwardingCurve.ramp(average, distInch - average) + val rampBase = ForwardingCurve.ramp(timeoutT.time(), distInch - average) var afterFactor = 1.0 if (isCollisionPreventionViable) { @@ -165,16 +165,6 @@ class KOdometryDrive( val powers = speeds.map(Move::rampSpeedToPower).normalNoStretch() lastPower = powers powers.apply(driveMotors) - - /* Log.i( - "KOD", - "SumOfError ${ - String.format( - "%+.8f", - sumError - ) - } in*sec ki=$kiFwd, Error ${String.format("%+.8f", pError)} kp=$kpFwd" - ) */ } isCompleted { -> complete || (timeout > 0 && timeoutT.time() >= timeout) } onFinish { -> diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java index a744a9fdd414..eaa9b8f50700 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TwentyAuto.java @@ -235,6 +235,7 @@ private void scoreYellowPixel(MultitaskScheduler scheduler, RobotConfiguration r }); e.isCompleted(() -> robot.liftLeft().getCurrentPosition() >= Var.AutoPositions.LiftScoring - 20); TimingKt.minDuration(e, 100); + TimingKt.maxDuration(e, 3000); return kvoid; }) .then(xButton.approach(new InchUnit(3.0))) @@ -495,6 +496,7 @@ public void run() { }); e.isCompleted(() -> !(robot.liftLeft().isBusy() || robot.liftRight().isBusy())); + TimingKt.maxDuration(e, 3000); return kvoid; }); @@ -527,6 +529,7 @@ public void run() { }); e.isCompleted(() -> !(robot.liftLeft().isBusy() || robot.liftRight().isBusy())); + TimingKt.maxDuration(e, 3000); return kvoid; });