Add timeouts to lift operations

GearUp12499-org · Feb 19, 2024 · bbfa092 · bbfa092
1 parent 5d6d1df
commit bbfa092
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 11 deletions.
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
@@ -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
@@ -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;
                     });