New auto paths blue left

GearUp12499-org · Feb 19, 2024 · 8f22f33 · 8f22f33
1 parent a67177e
commit 8f22f33
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 26 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
@@ -86,12 +86,14 @@ class KOdometryDrive(
     private fun distanceRight() = tick2inch(rightOdo.currentPosition)
     private fun distanceStrafe() = -tick2inch(strafeOdo.currentPosition)
 
+    // DO NOT RENAME collide2 to collide -- this will cause a method signature conflict
+    fun driveForward(target: LengthUnit, collide2: Boolean) = driveForward(target=target, collide=collide2)
     @JvmOverloads
-    fun driveForward(target: LengthUnit, timeout: Double = 4.0): Task {
+    fun driveForward(target: LengthUnit, timeout: Double = 4.0, collide: Boolean = false): Task {
         val distInch = abs(target.to.inches.value)
         val switcher = target.value.sign
 
-        val isCollisionPreventionViable = target.value < 0
+        val isCollisionPreventionViable = target.value < 0 && collide
 
         return scheduler.task {
             +dmLock
@@ -173,8 +175,10 @@ class KOdometryDrive(
         }
     }
 
+    fun driveReverse(target: LengthUnit, collide2: Boolean) = driveReverse(target=target, collide=collide2)
+
     @JvmOverloads
-    fun driveReverse(target: LengthUnit, timeout: Double = -1.0): Task {
+    fun driveReverse(target: LengthUnit, timeout: Double = 4.0, collide: Boolean = false): Task {
         return driveForward(-target, timeout)
     }
 
@@ -263,7 +267,7 @@ class KOdometryDrive(
     }
 
     @JvmOverloads
-    fun strafeLeft(target: LengthUnit, timeout: Double = -1.0) = strafeRight(-target, timeout)
+    fun strafeLeft(target: LengthUnit, timeout: Double = 3.0) = strafeRight(-target, timeout)
 
     // !!! UNTESTED DO NOT USE AAAAAAAAAAAAAAAAAA
     /**

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/SyncFail.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/odo/SyncFail.kt
@@ -8,11 +8,19 @@ class SyncFail(val scheduler: MultitaskScheduler, val kOdometryDrive: KOdometryD
         kOdometryDrive.driveForward(distance.inches)
         scheduler.runToCompletion(crasher)
     }
+    fun DriveForwardWithCollision(distance: Double) {
+        kOdometryDrive.driveForward(distance.inches, true)
+        scheduler.runToCompletion(crasher)
+    }
 
     fun DriveReverse(distance: Double, vararg whoTFCares: Any?) {
         kOdometryDrive.driveReverse(distance.inches)
         scheduler.runToCompletion(crasher)
     }
+    fun DriveReverseWithCollision(distance: Double) {
+        kOdometryDrive.driveReverse(distance.inches, true)
+        scheduler.runToCompletion(crasher)
+    }
 
     fun StrafeRight(distance: Double, vararg oopsie: Any?) {
         kOdometryDrive.strafeRight(distance.inches)

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
@@ -196,7 +196,7 @@ void leftLeft() {
     void unLeftLeft() {
         RobotLog.ii("TwentyAuto", "LEFT");
         turnPID.TurnRobot(-5.0);
-        why.DriveForward(13.0, telemetry);
+        why.DriveForward(1.0, telemetry);
     }
 
     void centerLeft() {
@@ -209,7 +209,7 @@ void centerLeft() {
     void unCenterLeft() {
         RobotLog.ii("TwentyAuto", "LEFT");
         turnPID.TurnRobot(20.0);
-        why.DriveForward(18.0, telemetry);
+        why.DriveForward(6.0, telemetry);
     }
 
     void rightLeft() {
@@ -222,9 +222,8 @@ void rightLeft() {
 
     void unRightLeft() {
         RobotLog.ii("TwentyAuto", "RIGHT");
-        why.DriveForward(2.0, telemetry);
+        why.DriveForward(4.0, telemetry);
         turnPID.TurnRobot(90.0);
-        why.DriveForward(16.0, telemetry);
     }
 
     private void scoreYellowPixel(MultitaskScheduler scheduler, RobotConfiguration robot, ApproachObject2 xButton, KOdometryDrive drive) {
@@ -585,24 +584,24 @@ private void navBackstage(
         if (allianceColor() == AllianceColor.Red) nearMedFar = 2 - nearMedFar;
         robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoring);
 
-        switch (nearMedFar) {
-            case 0:
-                doAwayFromWall.accept(
-                        new InchUnit(11.0).plus(Var.AutoPositions.RobotWidth.div(2))
-                );
-                break;
-            case 1:
-            default:
-                doAwayFromWall.accept(
-                        new InchUnit(16.5).plus(Var.AutoPositions.RobotWidth.div(2))
-                );
-                break;
-            case 2:
-                doAwayFromWall.accept(
-                        new InchUnit(23.0).plus(Var.AutoPositions.RobotWidth.div(2))
-                );
-                break;
-        }
+//        switch (nearMedFar) {
+//            case 0:
+//                doAwayFromWall.accept(
+//                        new InchUnit(-1).plus(Var.AutoPositions.RobotWidth.div(2))
+//                );
+//                break;
+//            case 1:
+//            default:
+//                doAwayFromWall.accept(
+//                        new InchUnit(4.5).plus(Var.AutoPositions.RobotWidth.div(2))
+//                );
+//                break;
+//            case 2:
+//                doAwayFromWall.accept(
+//                        new InchUnit(11.0).plus(Var.AutoPositions.RobotWidth.div(2))
+//                );
+//                break;
+//        }
         scheduler.runToCompletion(this::panic_button);
     }