From c42fce2e78e80848a44d7be8e1f957b85427343a Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 27 Feb 2026 19:01:33 -0600 Subject: [PATCH] untested edits of autos --- .../teamcode/autonomous/Auto_LT_Close.java | 25 ++- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 210 ++++++++++++++---- .../ftc/teamcode/constants/Back_Poses.java | 12 +- 3 files changed, 185 insertions(+), 62 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 34d7bef..36050fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -134,6 +134,7 @@ public class Auto_LT_Close extends LinearOpMode { double obeliskTurrPos3 = 0.0; double waitToPickupGate = 0; double obeliskTurrPosAutoStart = 0; + boolean limelightStart = false; // initialize path variables here TrajectoryActionBuilder shoot0 = null; @@ -185,7 +186,7 @@ public class Auto_LT_Close extends LinearOpMode { hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU(); while (opModeInInit()) { - if (limelightUsed && !gateCycle){ + if (limelightUsed && !gateCycle && limelightStart){ Actions.runBlocking( autoActions.detectObelisk( 0.1, @@ -207,14 +208,6 @@ public class Auto_LT_Close extends LinearOpMode { } } - if (!gateCycle) { - turret.pipelineSwitch(1); - } else if (redAlliance) { - turret.pipelineSwitch(4); - } else { - turret.pipelineSwitch(2); - } - if (gateCycle) { servos.setHoodPos(hoodGate0Start); } else { @@ -249,8 +242,10 @@ public class Auto_LT_Close extends LinearOpMode { if (gamepad2.squareWasPressed()) { drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); + sleep(100); robot.limelight.start(); limelightUsed = true; + limelightStart = true; gamepad2.rumble(500); } @@ -258,6 +253,12 @@ public class Auto_LT_Close extends LinearOpMode { if (redAlliance) { robot.light.setPosition(0.28); + if (gateCycle){ + turret.pipelineSwitch(1); + } else { + turret.pipelineSwitch(4); + } + // ---- FIRST SHOT ---- x1 = rx1; y1 = ry1; @@ -314,6 +315,12 @@ public class Auto_LT_Close extends LinearOpMode { } else { robot.light.setPosition(0.6); + if (gateCycle){ + turret.pipelineSwitch(5); + } else { + turret.pipelineSwitch(2); + } + // ---- FIRST SHOT ---- x1 = bx1; y1 = by1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index fbb7d67..eb7dd17 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -12,6 +12,9 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObelisk import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; @@ -57,12 +60,17 @@ public class Auto_LT_Far extends LinearOpMode { AutoActions autoActions; Light light; double xShoot, yShoot, hShoot; - double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; + double pickupGateXA = 0, pickupGateYA = 0, pickupGateHA = 0; + double pickupGateXB = 0, pickupGateYB = 0, pickupGateHB = 0; + double pickupGateXC = 0, pickupGateYC = 0, pickupGateHC = 0; public static double flywheel0Time = 1.5; boolean gatePickup = true; boolean stack3 = true; - double xStackPickupA, yStackPickupA, hStackPickupA; - double xStackPickupB, yStackPickupB, hStackPickupB; + boolean stack2 = true; + double xStackPickupFarA, yStackPickupFarA, hStackPickupFarA; + double xStackPickupFarB, yStackPickupFarB, hStackPickupFarB; + double xStackPickupMiddleA, yStackPickupMiddleA, hStackPickupMiddleA; + double xStackPickupMiddleB, yStackPickupMiddleB, hStackPickupMiddleB; public static int pickupStackSpeed = 17; public static int pickupGateSpeed = 25; int prevMotif = 0; @@ -88,6 +96,7 @@ public class Auto_LT_Far extends LinearOpMode { TrajectoryActionBuilder shoot3 = null; TrajectoryActionBuilder pickupGate = null; TrajectoryActionBuilder shootGate = null; + TrajectoryActionBuilder pickup2 = null; Pose2d autoStart = new Pose2d(0,0,0); @Override @@ -124,14 +133,40 @@ public class Auto_LT_Far extends LinearOpMode { servos.setTransferPos(transferServo_out); - while (opModeInInit()) { + limelightUsed = false; - if (gamepad2.leftBumperWasPressed()){ + while (opModeInInit()) { + if (limelightUsed){ + Actions.runBlocking( + autoActions.detectObelisk( + 0.1, + 0.501, + 0.501, + 0.501, + 0.501, + turrDefault + ) + ); + motif = turret.getObeliskID(); + + if (motif == 21){ + AutoActions.firstSpindexShootPos = spindexer_outtakeBall1; + } else if (motif == 22){ + AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b; + } else { + AutoActions.firstSpindexShootPos = spindexer_outtakeBall2; + } + } + + if (gamepad2.triangleWasPressed()){ gatePickup = !gatePickup; } if (gamepad2.rightBumperWasPressed()){ stack3 = !stack3; } + if (gamepad2.leftBumperWasPressed()){ + stack2 = !stack2; + } turret.setTurret(turretShootPos); @@ -164,17 +199,33 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = rShootY; hShoot = rShootH; - xStackPickupA = rStackPickupAX; - yStackPickupA = rStackPickupAY; - hStackPickupA = rStackPickupAH; + xStackPickupFarA = rStackPickupAX; + yStackPickupFarA = rStackPickupAY; + hStackPickupFarA = rStackPickupAH; - xStackPickupB = rStackPickupBX; - yStackPickupB = rStackPickupBY; - hStackPickupB = rStackPickupBH; + xStackPickupFarB = rStackPickupBX; + yStackPickupFarB = rStackPickupBY; + hStackPickupFarB = rStackPickupBH; - pickupGateX = rPickupGateX; - pickupGateY = rPickupGateY; - pickupGateH = rPickupGateH; + xStackPickupMiddleA = rStackPickupAX; + yStackPickupMiddleA = rStackPickupAY; + hStackPickupMiddleA = rStackPickupAH; + + xStackPickupMiddleB = rStackPickupBX; + yStackPickupMiddleB = rStackPickupBY; + hStackPickupMiddleB = rStackPickupBH; + + pickupGateXA = rPickupGateXA; + pickupGateYA = rPickupGateYA; + pickupGateHA = rPickupGateHA; + + pickupGateXB = rPickupGateXB; + pickupGateYB = rPickupGateYB; + pickupGateHB = rPickupGateHB; + + pickupGateXC = rPickupGateXC; + pickupGateYC = rPickupGateYC; + pickupGateHC = rPickupGateHC; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -182,10 +233,9 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + redTurretShootPos; if (gamepad2.squareWasPressed()){ - turret.pipelineSwitch(4); + turret.pipelineSwitch(1); robot.limelight.start(); - drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); - + limelightUsed = true; gamepad2.rumble(500); } } else { @@ -203,17 +253,33 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = bShootY; hShoot = bShootH; - xStackPickupA = bStackPickupAX; - yStackPickupA = bStackPickupAY; - hStackPickupA = bStackPickupAH; + xStackPickupFarA = bStackPickupAX; + yStackPickupFarA = bStackPickupAY; + hStackPickupFarA = bStackPickupAH; - xStackPickupB = bStackPickupBX; - yStackPickupB = bStackPickupBY; - hStackPickupB = bStackPickupBH; + xStackPickupFarB = bStackPickupBX; + yStackPickupFarB = bStackPickupBY; + hStackPickupFarB = bStackPickupBH; - pickupGateX = bPickupGateX; - pickupGateY = bPickupGateY; - pickupGateH = bPickupGateH; + xStackPickupMiddleA = bStackPickupAX; + yStackPickupMiddleA = bStackPickupAY; + hStackPickupMiddleA = bStackPickupAH; + + xStackPickupMiddleB = bStackPickupBX; + yStackPickupMiddleB = bStackPickupBY; + hStackPickupMiddleB = bStackPickupBH; + + pickupGateXA = bPickupGateXA; + pickupGateYA = bPickupGateYA; + pickupGateHA = bPickupGateHA; + + pickupGateXB = bPickupGateXB; + pickupGateYB = bPickupGateYB; + pickupGateHB = bPickupGateHB; + + pickupGateXC = bPickupGateXC; + pickupGateYC = bPickupGateYC; + pickupGateHC = bPickupGateHC; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; @@ -221,10 +287,9 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + blueTurretShootPos; if (gamepad2.squareWasPressed()){ - turret.pipelineSwitch(2); + turret.pipelineSwitch(5); robot.limelight.start(); - drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); - + limelightUsed = true; gamepad2.rumble(500); } } @@ -236,29 +301,33 @@ public class Auto_LT_Far extends LinearOpMode { .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA)) - .strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB), + .strafeToLinearHeading(new Vector2d(xStackPickupFarA, yStackPickupFarA), Math.toRadians(hStackPickupFarA)) + .strafeToLinearHeading(new Vector2d(xStackPickupFarB, yStackPickupFarB), Math.toRadians(hStackPickupFarB), new TranslationalVelConstraint(pickupStackSpeed)); - shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB))) + pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(xStackPickupMiddleA, yStackPickupMiddleA), Math.toRadians(hStackPickupMiddleA)) + .strafeToLinearHeading(new Vector2d(xStackPickupMiddleB, yStackPickupMiddleB), Math.toRadians(hStackPickupMiddleB), + new TranslationalVelConstraint(pickupStackSpeed)); + + shoot3 = drive.actionBuilder(new Pose2d(xStackPickupFarB, yStackPickupFarB, Math.toRadians(hStackPickupFarB))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH)) + .strafeToLinearHeading(new Vector2d(pickupGateXA, pickupGateYA), Math.toRadians(pickupGateHA)) .waitSeconds(0.2) .strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB)) .strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC), new TranslationalVelConstraint(pickupGateSpeed)); - shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) + shootGate = drive.actionBuilder(new Pose2d(pickupGateXC, pickupGateYC, Math.toRadians(pickupGateHC))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); - limelightUsed = true; - TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Gate Cycle?", gatePickup); - TELE.addData("Pickup Stack?", stack3); + TELE.addData("Pickup Stack Far?", stack3); + TELE.addData("Pickup Stack Middle?", stack2); TELE.addData("Start Position", autoStart); TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift TELE.update(); @@ -282,6 +351,11 @@ public class Auto_LT_Far extends LinearOpMode { shoot(); } + if (stack2){ + cycleStackMiddle(); + shoot(); + } + while (gatePickup && getRuntime() - stamp < endAutoTime){ cycleGatePickupBalls(); if (getRuntime() - stamp > endAutoTime){ @@ -358,14 +432,14 @@ public class Auto_LT_Far extends LinearOpMode { pickup3.build(), autoActions.intake( intakeStackTime, - xStackPickupB, - yStackPickupB, - hStackPickupB + xStackPickupFarB, + yStackPickupFarB, + hStackPickupFarB ), autoActions.detectObelisk( intakeStackTime, - xStackPickupB, - yStackPickupB, + xStackPickupFarB, + yStackPickupFarB, posXTolerance, posYTolerance, obeliskTurrPos3 @@ -393,20 +467,62 @@ public class Auto_LT_Far extends LinearOpMode { ); } + + void cycleStackMiddle(){ + Actions.runBlocking( + new ParallelAction( + pickup2.build(), + autoActions.intake( + intakeStackTime, + xStackPickupMiddleB, + yStackPickupMiddleB, + hStackPickupMiddleB + ), + autoActions.detectObelisk( + intakeStackTime, + xStackPickupMiddleB, + yStackPickupMiddleB, + posXTolerance, + posYTolerance, + obeliskTurrPos2 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + shoot3.build(), + autoActions.prepareShootAll( + colorSenseTime, + shootStackTime, + motif, + xShoot, + yShoot, + hShoot) + ) + ); + } + void cycleGatePickupBalls(){ Actions.runBlocking( new ParallelAction( pickupGate.build(), autoActions.intake( intakeStackTime, - pickupGateX, - pickupGateY, - pickupGateH + pickupGateXC, + pickupGateYC, + pickupGateHC ), autoActions.detectObelisk( intakeGateTime, - pickupGateX, - pickupGateY, + pickupGateXC, + pickupGateYC, posXTolerance, posYTolerance, obeliskTurrPos3 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index c5fcfd3..1b1af7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -16,12 +16,12 @@ public class Back_Poses { public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1; public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1; - public static double rPickupGateX = 50, rPickupGateY = 83, rPickupGateH = 140; - public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140; - public static double pickupGateXB = 84, pickupGateYB = 76, pickupGateHB = 140; - public static double pickupGateXC = 50, pickupGateYC = 83, pickupGateHC = 190; - - + public static double rPickupGateXA = 50, rPickupGateYA = 83, rPickupGateHA = 140; + public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -140; + public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 140; + public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -140; + public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190; + public static double bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190; public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;