compettiotn

GearUp12499-org · Feb 12, 2024 · b2d051a · b2d051a
1 parent 8e719d6
commit b2d051a
Show file tree

Hide file tree

Showing 3 changed files with 50 additions and 10 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
@@ -72,7 +72,7 @@ class KOdometryDrive(
     private fun distanceStrafe() = -tick2inch(strafeOdo.currentPosition)
 
     @JvmOverloads
-    fun driveForward(target: LengthUnit, timeout: Double = -1.0): Task {
+    fun driveForward(target: LengthUnit, timeout: Double = 4.0): Task {
         val distInch = abs(target.to.inches.value)
         val switcher = target.value.sign
 

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/TeleOp.java
@@ -316,8 +316,22 @@ public void runOpMode() {
                 });
             }
 
-            if (gamepad1.options) {
-                robot.imu().resetYaw();
+            if (gamepad2.options) {
+                // something broke
+                // freeze the robot
+                // fuck it we block who cares anymore
+                robot.driveMotors().setAll(0.0);
+                robot.liftLeft().setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                robot.liftLeft().setPower(-0.25);
+                while (gamepad2.options) {
+                    sleep(20);
+                }
+                robot.liftLeft().setPower(0.0);
+                robot.liftLeft().setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+                targetLeft.set(0);
+                robot.liftLeft().setTargetPosition(targetLeft.get());
+                robot.liftLeft().setMode(DcMotor.RunMode.RUN_TO_POSITION);
+                robot.liftLeft().setPower(1.0);
             }
 
             if (gamepad2.left_trigger > Var.TeleOp.triggerPct) {

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
@@ -85,6 +85,24 @@ public abstract class TwentyAuto extends LinearOpMode {
     protected abstract AllianceColor allianceColor();
 
     protected abstract FieldGlobalPosition globalPosition();
+
+    private ElapsedTime timer2 = new ElapsedTime();
+    private MultitaskScheduler scheduler = null;
+
+
+    private boolean panic_button() {
+        if (timer2.time() >= 28.5) {
+            scheduler.filteredStop(e -> true);
+            scheduler.panic();
+            robot.dumperRotate().setPosition(Var.Box.idleRotate);
+            robot.liftLeft().setTargetPosition(0);
+            while (opModeIsActive()) {
+                sleep(10);
+            }
+            throw new Panic();
+        }
+        return opModeIsActive();
+    }
 
     /**
      * Robot starting position
@@ -234,7 +252,7 @@ private void scoreYellowPixel(MultitaskScheduler scheduler, RobotConfiguration r
                     return kvoid;
                 })
                 .then(drive.driveForward(new InchUnit(5.0)));
-        scheduler.runToCompletion(this::opModeIsActive);
+        scheduler.runToCompletion(this::panic_button);
     }
 
     /**
@@ -283,14 +301,21 @@ private void alignTag(AprilTagProcessor aprilTag, int targetID) {
 
     @Override
     public void runOpMode() throws InterruptedException {
+        try {
+            run();
+        } catch (Panic e) {
+            Log.e("20auto", "panic failed");
+        }
+    }
+    public void run() throws InterruptedException {
         // Get robot hardware configs
-        MultitaskScheduler scheduler = new MultitaskScheduler();
+        scheduler = new MultitaskScheduler();
         setupVision(hardwareMap.get(CameraName.class, "Webcam 1"));
         telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
         robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap);
         turnPID = new TurnPID(robot);
         KOdometryDrive newOdo = new KOdometryDrive(scheduler, robot);
-        why = new SyncFail(scheduler, newOdo, this::opModeIsActive);
+        why = new SyncFail(scheduler, newOdo, this::panic_button);
         ApproachObject2 theXButton = new ApproachObject2(scheduler, robot, 18.0);
         Dumper dumper = new Dumper(scheduler, robot);
 
@@ -373,6 +398,7 @@ public void runOpMode() throws InterruptedException {
             clearDuration -= RedArrivalTime; // TODO
         }
         timer.reset();
+        timer2.reset();
 
         // Capture the result
         AdvSphereProcess.Result result = sphere.getResult();
@@ -446,7 +472,7 @@ public void runOpMode() throws InterruptedException {
                     .minus(new InchUnit(2));
 
             toWallWithTimeout.accept(strafeMotion, 3.0);
-            scheduler.runToCompletion(this::opModeIsActive);
+            scheduler.runToCompletion(this::panic_button);
 
             /*
              * 1. go to the target position within the acceptError
@@ -474,7 +500,7 @@ public void runOpMode() throws InterruptedException {
                 return kvoid;
             });
 
-            scheduler.runToCompletion(this::opModeIsActive);
+            scheduler.runToCompletion(this::panic_button);
         } else { // Frontstage scripts
             Consumer<Double> rightOnBlue = allianceColor() == AllianceColor.Blue ? why::StrafeRight : why::StrafeLeft;
             navFrontstage(result, rightOnBlue);
@@ -506,7 +532,7 @@ public void runOpMode() throws InterruptedException {
                         return kvoid;
                     });
 
-            scheduler.runToCompletion(this::opModeIsActive);
+            scheduler.runToCompletion(this::panic_button);
         }
 
         // Stop motors just in case
@@ -563,7 +589,7 @@ private void navBackstage(
                 );
                 break;
         }
-        scheduler.runToCompletion(this::opModeIsActive);
+        scheduler.runToCompletion(this::panic_button);
     }
 
     public static final double[] frontstageTravelDistances = {18.5, 20.0, 27.0};