From 3268d5cd020fb5fc335ebadaf1a039317d112413 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 19 Feb 2026 21:19:04 -0600 Subject: [PATCH] front gate auto still in progress --- .../teamcode/autonomous/Auto_LT_Close.java | 147 ++++++++++++++++-- .../autonomous/actions/AutoActions.java | 90 ++++++----- .../ftc/teamcode/constants/Front_Poses.java | 6 + .../teamcode/constants/ServoPositions.java | 4 +- 4 files changed, 187 insertions(+), 60 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 b82fa67..b06233d 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 @@ -40,17 +40,13 @@ import org.firstinspires.ftc.teamcode.utils.Turret; @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93; - public static double velGate0Start = 2700, hoodGate0Start = 0.61; + public static double velGate0Start = 2700, hoodGate0Start = 0.6; - public static double velGate0End = 2700, hoodGate0End = 0.4; + public static double velGate0End = 2700, hoodGate0End = 0.35; + public static double hood0MoveTime = 2; + public static double spindexerSpeedIncrease = 0.02; - public static double spindexerSpeedIncrease = 0.025; - - double obeliskTurrPos1 = 0.0; - double obeliskTurrPos2 = 0.0; - double obeliskTurrPos3 = 0.0; - - public static double shootAllTime = 2; + public static double shootAllTime = 4; public static double intake1Time = 3.3; public static double intake2Time = 3.8; @@ -67,10 +63,16 @@ public class Auto_LT_Close extends LinearOpMode { public static double shoot3Time = 2.5; public static double colorSenseTime = 1.2; public int motif = 0; - public static double waitToShoot0 = 0.6; - public static double waitToPickupGate2 = 0.1; + public static double waitToShoot0 = 0.5; + public static double waitToPickupGate2 = 0.3; public static double pickupStackGateSpeed = 50; - public static double intake2TimeGate = 6; + public static double intake2TimeGate = 3; + public static double shoot2GateTime = 3; + public static double endGateTime = 25; + public static double waitToPickupGateWithPartner = 2; + public static double waitToPickupGateSolo = 1; + public static double intakeGateTime = 5; + public static double shootGateTime = 3; Robot robot; MultipleTelemetry TELE; @@ -97,7 +99,8 @@ public class Auto_LT_Close extends LinearOpMode { double xShoot, yShoot, hShoot; double xShoot0, yShoot0, hShoot0; - double xGate, yGate, hGate; + double pickupGateAX, pickupGateAY, pickupGateAH; + double pickupGateBX, pickupGateBY, pickupGateBH; double xShootGate, yShootGate, hShootGate; double xLeave, yLeave, hLeave; double xLeaveGate, yLeaveGate, hLeaveGate; @@ -105,7 +108,11 @@ public class Auto_LT_Close extends LinearOpMode { int ballCycles = 3; int prevMotif = 0; boolean gateCycle = true; - + boolean withPartner = false; + double obeliskTurrPos1 = 0.0; + double obeliskTurrPos2 = 0.0; + double obeliskTurrPos3 = 0.0; + double waitToPickupGate = 0; // initialize path variables here TrajectoryActionBuilder shoot0 = null; @@ -116,6 +123,8 @@ public class Auto_LT_Close extends LinearOpMode { TrajectoryActionBuilder pickup3 = null; TrajectoryActionBuilder shoot3 = null; TrajectoryActionBuilder shoot0ToPickup2 = null; + TrajectoryActionBuilder gateCyclePickup = null; + TrajectoryActionBuilder gateCycleShoot = null; @Override public void runOpMode() throws InterruptedException { @@ -155,7 +164,12 @@ public class Auto_LT_Close extends LinearOpMode { while (opModeInInit()) { - servos.setHoodPos(shoot0Hood); + if (gateCycle){ + servos.setHoodPos(hoodGate0Start); + } else { + servos.setHoodPos(shoot0Hood); + } + turret.setTurret(turrDefault); if (gamepad2.crossWasPressed()) { @@ -228,6 +242,13 @@ public class Auto_LT_Close extends LinearOpMode { yLeaveGate = rLeaveGateY; hLeaveGate = rLeaveGateH; + pickupGateAX = rPickupGateAX; + pickupGateAY = rPickupGateAY; + pickupGateAH = rPickupGateAH; + pickupGateBX = rPickupGateBX; + pickupGateBY = rPickupGateBY; + pickupGateBH = rPickupGateBH; + obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; obeliskTurrPos3 = turrDefault + redObeliskTurrPos3; @@ -276,6 +297,13 @@ public class Auto_LT_Close extends LinearOpMode { yLeaveGate = bLeaveGateY; hLeaveGate = bLeaveGateH; + pickupGateAX = bPickupGateAX; + pickupGateAY = bPickupGateAY; + pickupGateAH = bPickupGateAH; + pickupGateBX = bPickupGateBX; + pickupGateBY = bPickupGateBY; + pickupGateBH = bPickupGateBH; + obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; @@ -338,6 +366,20 @@ public class Auto_LT_Close extends LinearOpMode { .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), new TranslationalVelConstraint(pickupStackGateSpeed)); + if (withPartner){ + waitToPickupGate = waitToPickupGateWithPartner; + } else { + waitToPickupGate = waitToPickupGateSolo; + } + + gateCyclePickup = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate))) + .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH)) + .waitSeconds(waitToPickupGate) + .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH)); + + gateCycleShoot = drive.actionBuilder(new Pose2d(pickupGateBX, pickupGateBY, Math.toRadians(pickupGateBH))) + .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); + TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Ball Cycles", ballCycles); @@ -350,11 +392,19 @@ public class Auto_LT_Close extends LinearOpMode { if (isStopRequested()) return; if (opModeIsActive()) { + double stamp = getRuntime(); robot.transfer.setPower(1); if (gateCycle){ shoot0Gate(); + cycleStackMiddleGate(); + + while (getRuntime() - stamp < endGateTime){ + cycleGateIntake(); + cycleGateShoot(); + } + } else { startAuto(); shoot(); @@ -574,9 +624,9 @@ public class Auto_LT_Close extends LinearOpMode { shoot0ToPickup2.build(), new SequentialAction( new ParallelAction( - autoActions.Wait(waitToShoot0), autoActions.manageShooterManual( waitToShoot0, + 0.501, velGate0Start, hoodGate0Start, velGate0Start, @@ -584,7 +634,15 @@ public class Auto_LT_Close extends LinearOpMode { 0.501 ) ), - autoActions.shootAllManual(shootAllTime, spindexerSpeedIncrease, velGate0Start, hoodGate0Start, velGate0End, hoodGate0End,0.501), + autoActions.shootAllManual( + shootAllTime, + hood0MoveTime, + spindexerSpeedIncrease, + velGate0Start, + hoodGate0Start, + velGate0End, + hoodGate0End, + 0.501), autoActions.intake( intake2TimeGate, xShootGate, @@ -596,4 +654,59 @@ public class Auto_LT_Close extends LinearOpMode { ); } + void cycleStackMiddleGate(){ + servos.setSpinPos(spinStartPos); + Actions.runBlocking( + new ParallelAction( + shoot2.build(), + new SequentialAction( + new ParallelAction( + autoActions.manageShooterAuto( + shoot2GateTime, + xShootGate, + yShootGate, + hShootGate + ), + autoActions.Wait(shoot2GateTime) + ), + autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + ) + ); + } + + void cycleGateIntake(){ + Actions.runBlocking( + new ParallelAction( + gateCyclePickup.build(), + autoActions.intake( + intakeGateTime, + xShootGate, + yShootGate, + hShootGate + ) + ) + ); + } + + void cycleGateShoot(){ + servos.setSpinPos(spinStartPos); + Actions.runBlocking( + new ParallelAction( + gateCycleShoot.build(), + new SequentialAction( + new ParallelAction( + autoActions.manageShooterAuto( + shootGateTime, + xShootGate, + yShootGate, + hShootGate + ), + autoActions.Wait(shootGateTime) + ), + autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + ) + ); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 3ebba07..de62ba5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.autonomous.actions; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; @@ -32,7 +33,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; import java.util.Objects; @Config -public class AutoActions{ +public class AutoActions { Robot robot; MultipleTelemetry TELE; Servos servos; @@ -47,12 +48,12 @@ public class AutoActions{ private int passengerSlotGreen = 0; private int rearSlotGreen = 0; private int mostGreenSlot = 0; - private double firstSpindexShootPos = spindexer_outtakeBall1; + private double firstSpindexShootPos = spinStartPos; private boolean shootForward = true; public int motif = 0; double spinEndPos = ServoPositions.spinEndPos; - public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig){ + public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig) { this.robot = rob; this.drive = dri; this.TELE = tel; @@ -81,37 +82,38 @@ public class AutoActions{ boolean decideGreenSlot = false; - void spin1PosFirst(){ + void spin1PosFirst() { firstSpindexShootPos = spindexer_outtakeBall1; shootForward = true; spinEndPos = spindexer_outtakeBall3 + 0.1; } - void spin2PosFirst(){ + void spin2PosFirst() { firstSpindexShootPos = spindexer_outtakeBall2; shootForward = false; spinEndPos = spindexer_outtakeBall3b - 0.1; } - void reverseSpin2PosFirst(){ + void reverseSpin2PosFirst() { firstSpindexShootPos = spindexer_outtakeBall2; shootForward = true; spinEndPos = 0.95; } - void spin3PosFirst(){ + void spin3PosFirst() { firstSpindexShootPos = spindexer_outtakeBall3; shootForward = false; spinEndPos = spindexer_outtakeBall1 - 0.1; } - void oddSpin3PosFirst(){ + void oddSpin3PosFirst() { firstSpindexShootPos = spindexer_outtakeBall3b; shootForward = true; spinEndPos = spindexer_outtakeBall2 + 0.1; } Action manageShooter = null; + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { @@ -215,6 +217,7 @@ public class AutoActions{ int shooterTicker = 0; Action manageShooter = null; + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { drive.updatePoseEstimate(); @@ -238,16 +241,15 @@ public class AutoActions{ double prevSpinPos = servos.getSpinCmdPos(); boolean end; - if (shootForward){ + if (shootForward) { end = prevSpinPos > spinEndPos; } else { end = prevSpinPos < spinEndPos; } - if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) { + if (System.currentTimeMillis() - stamp < shootTime * 1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks + 1)) { if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) { - servos.setTransferPos(transferServo_out); servos.setSpinPos(firstSpindexShootPos); } else { servos.setTransferPos(transferServo_in); @@ -255,7 +257,7 @@ public class AutoActions{ Spindexer.whileShooting = true; if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) { servos.setSpinPos(prevSpinPos + spindexSpeed); - } else if (shooterTicker > Spindexer.waitFirstBallTicks){ + } else if (shooterTicker > Spindexer.waitFirstBallTicks) { servos.setSpinPos(prevSpinPos - spindexSpeed); } @@ -276,7 +278,15 @@ public class AutoActions{ }; } - public Action shootAllManual(double shootTime, double spindexSpeed, double velStart, double hoodStart, double velEnd, double hoodEnd, double turr) { + public Action shootAllManual( + double shootTime, + double hoodMoveTime, //Set to 0.501 to show that you are not using, but you must set hoodPoses equal + double spindexSpeed, + double velStart, + double hoodStart, + double velEnd, + double hoodEnd, + double turr) { return new Action() { int ticker = 1; @@ -284,6 +294,7 @@ public class AutoActions{ int shooterTicker = 0; Action manageShooter = null; + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { drive.updatePoseEstimate(); @@ -297,7 +308,7 @@ public class AutoActions{ if (ticker == 1) { stamp = System.currentTimeMillis(); - manageShooter = manageShooterManual(shootTime, velStart, hoodStart, velEnd, hoodEnd, turr); + manageShooter = manageShooterManual(shootTime, hoodMoveTime, velStart, hoodStart, velEnd, hoodEnd, turr); } ticker++; @@ -307,27 +318,20 @@ public class AutoActions{ double prevSpinPos = servos.getSpinCmdPos(); boolean end; - if (shootForward){ + if (shootForward) { end = prevSpinPos > spinEndPos; } else { end = prevSpinPos < spinEndPos; } - if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks+1)) { - - if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) { - servos.setTransferPos(transferServo_out); - servos.setSpinPos(firstSpindexShootPos); - } else { - servos.setTransferPos(transferServo_in); - shooterTicker++; - Spindexer.whileShooting = true; - if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) { - servos.setSpinPos(prevSpinPos + spindexSpeed); - } else if (shooterTicker > Spindexer.waitFirstBallTicks){ - servos.setSpinPos(prevSpinPos - spindexSpeed); - } - + if (System.currentTimeMillis() - stamp < shootTime * 1000 && !end) { + servos.setTransferPos(transferServo_in); + shooterTicker++; + Spindexer.whileShooting = true; + if (shootForward && shooterTicker > Spindexer.waitFirstBallTicks) { + servos.setSpinPos(prevSpinPos + spindexSpeed); + } else if (shooterTicker > Spindexer.waitFirstBallTicks) { + servos.setSpinPos(prevSpinPos - spindexSpeed); } return true; @@ -355,6 +359,7 @@ public class AutoActions{ double stamp = 0.0; int ticker = 0; Action manageShooter = null; + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { @@ -374,7 +379,7 @@ public class AutoActions{ manageShooter.run(telemetryPacket); - if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()){ + if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) { spindexer.setIntakePower(-0.1); return false; } else { @@ -385,6 +390,7 @@ public class AutoActions{ } private boolean detectingObelisk = false; + public Action detectObelisk( double time, double posX, @@ -427,8 +433,8 @@ public class AutoActions{ teleStart = currentPose; - if (shouldFinish){ - if (redAlliance){ + if (shouldFinish) { + if (redAlliance) { turret.pipelineSwitch(4); } else { turret.pipelineSwitch(2); @@ -514,13 +520,14 @@ public class AutoActions{ }; } - public Action Wait(double time){ + public Action Wait(double time) { return new Action() { boolean ticker = false; double stamp = 0.0; + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (!ticker){ + if (!ticker) { stamp = System.currentTimeMillis(); ticker = true; } @@ -532,19 +539,20 @@ public class AutoActions{ } public Action manageShooterManual( - double time, + double maxTime, + double hoodMoveTime, //Set to 0.501 to show that you are not using, but you must set hoodPoses equal double velStart, double hoodStart, double velEnd, double hoodEnd, double turr - ){ + ) { return new Action() { double stamp = 0.0; int ticker = 0; - final boolean timeFallback = (time != 0.501); + final boolean timeFallback = (maxTime != 0.501); @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { @@ -578,14 +586,14 @@ public class AutoActions{ turret.setTurret(turr); } - servos.setHoodPos(hoodStart + (hoodEnd-hoodStart) * ((System.currentTimeMillis() - stamp)/(time*1000))); - double vel = velStart + (velEnd-velStart) * ((System.currentTimeMillis() - stamp)/(time*1000)); + servos.setHoodPos(hoodStart + ((hoodEnd - hoodStart) * Math.min(((System.currentTimeMillis() - stamp) / (hoodMoveTime * 1000)), 1))); + double vel = velStart + (velEnd - velStart) * Math.min(((System.currentTimeMillis() - stamp) / (hoodMoveTime * 1000)), 1); double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > maxTime * 1000; teleStart = currentPose; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index 7ebb565..c3bd77b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -51,5 +51,11 @@ public class Front_Poses { public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55; public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55; + public static double rPickupGateAX = 36, rPickupGateAY = 50, rPickupGateAH = 140; + public static double bPickupGateAX = 36, bPickupGateAY = -50, bPickupGateAH = -140; + + public static double rPickupGateBX = 46, rPickupGateBY = 60, rPickupGateBH = 140; + public static double bPickupGateBX = 46, bPickupGateBY = -60, bPickupGateBH = -140; + public static Pose2d teleStart = new Pose2d(0, 0, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 61edb26..7e8e651 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -16,8 +16,8 @@ public class ServoPositions { public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6; public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4; - public static double spinStartPos = 0.25; - public static double spinEndPos = 0.85; + public static double spinStartPos = 0.05; + public static double spinEndPos = 0.95; public static double shootAllSpindexerSpeedIncrease = 0.014;