Minor driving tweaks

GearUp12499-org · Feb 25, 2024 · 9b43a5f · 9b43a5f
1 parent c5b63ab
commit 9b43a5f
Show file tree

Hide file tree

Showing 5 changed files with 48 additions and 20 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Var.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Var.kt
@@ -123,7 +123,7 @@ object Var {
         /**
          * Lift preset height for scoring, encoder ticks
          */
-        const val liftScoringReadyPreset = 450
+        const val liftScoringReadyPreset = 1850
 
         /**
          * Lift preset height for hanging, encoder ticks
@@ -207,6 +207,7 @@ object Var {
         val BlueLeftRightPixel = Pose(24.5.inches + RobotWidth / 2.0, 83.5.inches, (90).degrees)
 
         // Lift scoring position: 1280, 1100
-        const val LiftScoring = 1280
+        const val LiftScoringLow = 1145
+        const val LiftScoringHigh = 1280
     }
 }
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
@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.odo
 
 import android.util.Log
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor
 import com.qualcomm.robotcore.util.ElapsedTime
 import dev.aether.collaborative_multitasking.MultitaskScheduler
 import dev.aether.collaborative_multitasking.Task
@@ -11,6 +12,7 @@ import org.firstinspires.ftc.teamcode.utilities.LengthUnit
 import org.firstinspires.ftc.teamcode.utilities.MotorPowers
 import org.firstinspires.ftc.teamcode.utilities.Move
 import org.firstinspires.ftc.teamcode.utilities.RotationUnit
+import java.util.Optional
 import kotlin.math.PI
 import kotlin.math.abs
 import kotlin.math.min
@@ -86,6 +88,14 @@ class KOdometryDrive(
     private fun distanceRight() = tick2inch(rightOdo.currentPosition)
     private fun distanceStrafe() = -tick2inch(strafeOdo.currentPosition)
 
+    private fun collisionInfo(): Optional<Double> {
+        val readA = collisionA.getDistance(DistanceUnit.INCH);
+        val readB = collisionB.getDistance(DistanceUnit.INCH);
+        if (readA > CollisionDetectionDistance) return Optional.empty()
+        if (readB > CollisionDetectionDistance) return Optional.empty()
+        return Optional.of(CollideRamp.rampDown(min(readA, readB)))
+    }
+
     // 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
@@ -146,12 +156,9 @@ class KOdometryDrive(
 
                 var afterFactor = 1.0
                 if (isCollisionPreventionViable) {
-                    val collider = min(
-                        collisionA.getDistance(DistanceUnit.INCH),
-                        collisionB.getDistance(DistanceUnit.INCH)
-                    )
-                    if (collider < CollisionDetectionDistance) {
-                        afterFactor = min(rampBase, CollideRamp.rampDown(collider - CollisionTargetDistance))
+                    val collideData = collisionInfo();
+                    if (collideData.isPresent) {
+                        afterFactor = collideData.get()
                     }
                 }
                 val speed = rampBase * switcher

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveForward2.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DriveForward2.kt
@@ -21,7 +21,7 @@ class DriveForward2 : LinearOpMode() {
         val reference = DriveForwardPID(robot)
         val rotateReference = TurnPID(robot)
         waitForStart()
-        drive.driveReverse(4.feet);
+        drive.strafeRight(4.feet, -1.0);
         sleep(100)
 
         try {

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
@@ -374,8 +374,10 @@ public void runOpMode() {
             robot.tele(telemetry);
 
             //Updates the average distance traveled forward: positive is right or forward; negative is backward or left
-            telemetry.addData("Distance Driven Forward:",
-                    OdoToInches((para2.getCurrentPosition() + para1.getCurrentPosition()) / 2.0));
+            telemetry.addData("left:",
+                    OdoToInches((para1.getCurrentPosition())));
+            telemetry.addData("right:",
+                    OdoToInches((para2.getCurrentPosition())));
             telemetry.addData("Inches Strafed: ", OdoToInches(robot.intake().getCurrentPosition()));
             telemetry.addData("Left Slide Ticks", robot.liftLeft().getCurrentPosition());
             telemetry.addData("Right Slide Ticks", robot.liftRight().getCurrentPosition());

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
@@ -231,15 +231,18 @@ void unRightLeft() {
         turnPID.TurnRobot(90.0);
     }
 
+    private int yellowLiftTarget = Var.AutoPositions.LiftScoringLow;
+    private boolean isLowBallin = false;
+
     private void scoreYellowPixel(MultitaskScheduler scheduler, RobotConfiguration robot, ApproachObject2 xButton, KOdometryDrive drive) {
         scheduler.task(e -> {
                     e.onStart(() -> {
-                        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoring);
+                        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoringLow);
                         robot.dumperLatch().setPosition(Var.Box.latched);
                         robot.dumperRotate().setPosition(Var.Box.dumpRotate);
                         return kvoid;
                     });
-                    e.isCompleted(() -> robot.liftLeft().getCurrentPosition() >= Var.AutoPositions.LiftScoring - 20);
+                    e.isCompleted(() -> robot.liftLeft().getCurrentPosition() >= yellowLiftTarget - 20);
                     TimingKt.minDuration(e, 100);
                     TimingKt.maxDuration(e, 3000);
                     return kvoid;
@@ -324,6 +327,7 @@ public void run() {
         newOdo = new KOdometryDrive(scheduler, robot);
         why = new SyncFail(scheduler, newOdo, this::panic_button);
         theXButton = new ApproachObject2(scheduler, robot, 18.0);
+        isLowBallin = globalPosition() == FieldGlobalPosition.Backstage;
 
         // Set up robot hardware
         robot.clearEncoders();
@@ -340,6 +344,7 @@ public void run() {
         ElapsedTime dDownRepeat = new ElapsedTime();
         double downNextInterval = buttonRepeatRate;
         boolean lastDDown = false;
+        boolean lastX = false;
 
         // Display current detection results
         while (opModeInInit()) {
@@ -364,6 +369,9 @@ public void run() {
             telemetry.addLine("Configuration (GP1)");
             telemetry.addData("TimeToPartnerCleared", clearDuration);
             telemetry.addLine("dpad +/- 1s");
+            telemetry.addLine();
+            telemetry.addData("Yellow Height", isLowBallin ? "low" : "high");
+            telemetry.addLine("(x button)");
             telemetry.update();
             if (gamepad1.dpad_up && !lastDUp) {
                 clearDuration += 1.0;
@@ -392,8 +400,18 @@ public void run() {
             if (clearDuration < 0) clearDuration = 0;
             if (clearDuration > maxAwait) clearDuration = maxAwait;
 
+            if (gamepad1.x && !lastX) {
+                isLowBallin = !isLowBallin;
+            }
+
             lastDUp = gamepad1.dpad_up;
             lastDDown = gamepad1.dpad_down;
+            lastX = gamepad1.x;
+        }
+        if (isLowBallin) {
+            yellowLiftTarget = Var.AutoPositions.LiftScoringLow;
+        } else {
+            yellowLiftTarget = Var.AutoPositions.LiftScoringHigh;
         }
         waitForStart();
         if (!opModeIsActive()) return;
@@ -587,7 +605,7 @@ private void navBackstage(
         }
 
         if (allianceColor() == AllianceColor.Red) nearMedFar = 2 - nearMedFar;
-        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoring);
+        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoringLow);
 
 //        switch (nearMedFar) {
 //            case 0:
@@ -619,7 +637,7 @@ private void frontstageLeft() {
         sleep(250);
         turnPID.TurnRobot(95);
         why.DriveReverse(48);
-        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoring);
+        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoringLow);
         while (timer.time() < clearDuration) {
             telemetry.addLine(String.format(
                     Locale.getDefault(),
@@ -640,7 +658,7 @@ private void frontstageLeftB() {
         sleep(250);
         turnPID.TurnRobot(-95);
         why.DriveReverse(48);
-        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoring);
+        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoringLow);
         while (timer.time() < clearDuration) {
             telemetry.addLine(String.format(
                     Locale.getDefault(),
@@ -659,7 +677,7 @@ private void frontstageCenter() {
         why.DriveReverse(24);
         turnPID.TurnRobot(95);
         why.DriveReverse(48);
-        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoring);
+        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoringLow);
         while (timer.time() < clearDuration) {
             telemetry.addLine(String.format(
                     Locale.getDefault(),
@@ -679,7 +697,7 @@ private void frontstageCenterB() {
         why.DriveReverse(24);
         turnPID.TurnRobot(-90);
         why.DriveReverse(72);
-        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoring);
+        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoringLow);
         while (timer.time() < clearDuration) {
             telemetry.addLine(String.format(
                     Locale.getDefault(),
@@ -700,7 +718,7 @@ private void frontstageRight() {
         sleep(250);
         turnPID.TurnRobot(90);
         why.DriveReverse(48);
-        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoring);
+        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoringLow);
         while (timer.time() < clearDuration) {
             telemetry.addLine(String.format(
                     Locale.getDefault(),
@@ -721,7 +739,7 @@ private void frontstageRightB() {
         sleep(250);
         turnPID.TurnRobot(-90);
         why.DriveReverse(48);
-        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoring);
+        robot.liftLeft().setTargetPosition(Var.AutoPositions.LiftScoringLow);
         while (timer.time() < clearDuration) {
             telemetry.addLine(String.format(
                     Locale.getDefault(),