From 9b43a5f26c08d50ba011d94fdf7ccdd70edd4a2e Mon Sep 17 00:00:00 2001 From: penguinencounter <49845522+penguinencounter@users.noreply.github.com> Date: Sat, 24 Feb 2024 20:43:04 -0800 Subject: [PATCH] Minor driving tweaks --- .../org/firstinspires/ftc/teamcode/Var.kt | 5 +-- .../ftc/teamcode/odo/KOdometryDrive.kt | 19 ++++++---- .../ftc/teamcode/opmodes/DriveForward2.kt | 2 +- .../ftc/teamcode/opmodes/TeleOp.java | 6 ++-- .../ftc/teamcode/opmodes/TwentyAuto.java | 36 ++++++++++++++----- 5 files changed, 48 insertions(+), 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 index 00d4994edeef..e6798ef9c02d 100644 --- 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 index 589b3945ada7..6c62af2d1a98 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 @@ -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 { + 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 index 20e56f313106..29f51fba1bf1 100644 --- 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 index 7104b6e76be0..db7d9ea768ca 100644 --- 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 index 95579af24007..7dc1dfc839af 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 @@ -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(),