From 56b61ee88b800a381e10b7662758e87b0aff1017 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 20 Feb 2026 22:13:52 -0600 Subject: [PATCH] gate auto in progress - 30% done --- .../teamcode/autonomous/Auto_LT_Close.java | 58 ++++++++++--------- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 3 +- .../autonomous/actions/AutoActions.java | 13 +++-- .../ftc/teamcode/constants/Front_Poses.java | 8 +-- .../teamcode/constants/ServoPositions.java | 2 +- .../ftc/teamcode/tests/ShooterTest.java | 5 +- .../ftc/teamcode/utils/Turret.java | 4 +- 7 files changed, 52 insertions(+), 41 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 5f89a54..3201ad3 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 @@ -44,7 +44,7 @@ public class Auto_LT_Close extends LinearOpMode { public static double velGate0End = 2700, hoodGate0End = 0.35; public static double hood0MoveTime = 2; - public static double spindexerSpeedIncrease = 0.02; + public static double spindexerSpeedIncrease = 0.016; public static double shootAllTime = 4; public static double intake1Time = 3.3; @@ -64,17 +64,16 @@ public class Auto_LT_Close extends LinearOpMode { public static double colorSenseTime = 1.2; public static double waitToShoot0 = 0.5; public static double waitToPickupGate2 = 0.3; - public static double pickupStackGateSpeed = 50; + public static double pickupStackGateSpeed = 30; public static double intake2TimeGate = 3; - public static double shoot2GateTime = 3; + public static double shoot2GateTime = 1.7; public static double endGateTime = 22; - public static double waitToPickupGateWithPartner = 2; - public static double waitToPickupGateSolo = 1; - public static double intakeGateTime = 5; - public static double shootGateTime = 3; - public static double shoot1GateTime = 3; + public static double waitToPickupGateWithPartner = 1; + public static double waitToPickupGateSolo = 0.2; + public static double intakeGateTime = 3; + public static double shootGateTime = 1.7; + public static double shoot1GateTime = 1.7; public static double intake1GateTime = 3.3; - public static double lastIntakeTime = 27; public static double lastShootTime = 27; Robot robot; @@ -162,7 +161,7 @@ public class Auto_LT_Close extends LinearOpMode { servos.setTransferPos(transferServo_out); - limelightUsed = false; + limelightUsed = true; robot.light.setPosition(1); @@ -196,7 +195,13 @@ public class Auto_LT_Close extends LinearOpMode { } if (gamepad2.squareWasPressed()){ - turret.pipelineSwitch(1); + if (!gateCycle){ + turret.pipelineSwitch(1); + } else if (redAlliance){ + turret.pipelineSwitch(4); + } else { + turret.pipelineSwitch(2); + } robot.limelight.start(); gamepad2.rumble(500); } @@ -396,6 +401,7 @@ public class Auto_LT_Close extends LinearOpMode { if (isStopRequested()) return; if (opModeIsActive()) { + double stamp = getRuntime(); robot.transfer.setPower(1); @@ -406,12 +412,11 @@ public class Auto_LT_Close extends LinearOpMode { while (getRuntime() - stamp < endGateTime){ cycleGateIntake(); - cycleGateShoot(); - } - - if (getRuntime() - stamp < lastIntakeTime){ - cycleStackCloseIntakeGate(); + if (getRuntime() - stamp < lastShootTime){ + cycleGateShoot(); + } } + cycleStackCloseIntakeGate(); if (getRuntime() - stamp < lastShootTime){ cycleStackCloseShootGate(); @@ -473,7 +478,8 @@ public class Auto_LT_Close extends LinearOpMode { flywheel0Time, x1, y1, - h1 + h1, + false ) ) ); @@ -677,9 +683,9 @@ public class Auto_LT_Close extends LinearOpMode { shoot2GateTime, xShootGate, yShootGate, - hShootGate - ), - autoActions.Wait(shoot2GateTime) + hShootGate, + false + ) ), autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) ) @@ -712,9 +718,9 @@ public class Auto_LT_Close extends LinearOpMode { shootGateTime, xShootGate, yShootGate, - hShootGate - ), - autoActions.Wait(shootGateTime) + hShootGate, + false + ) ), autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) ) @@ -747,9 +753,9 @@ public class Auto_LT_Close extends LinearOpMode { shoot1GateTime, xLeaveGate, yLeaveGate, - hLeaveGate - ), - autoActions.Wait(shoot1GateTime) + hLeaveGate, + false + ) ), autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) ) 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 e5b964c..68ebeec 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 @@ -327,7 +327,8 @@ public class Auto_LT_Far extends LinearOpMode { flywheel0Time, 0.501, 0.501, - 0.501 + 0.501, + true ) ) 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 de62ba5..9d1cf0a 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 @@ -118,7 +118,7 @@ public class AutoActions { public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { stamp = System.currentTimeMillis(); - manageShooter = manageShooterAuto(time, posX, posY, posH); + manageShooter = manageShooterAuto(time, posX, posY, posH, false); } ticker++; servos.setTransferPos(transferServo_out); @@ -231,7 +231,7 @@ public class AutoActions { if (ticker == 1) { stamp = System.currentTimeMillis(); - manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501); + manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501, false); } ticker++; @@ -364,7 +364,7 @@ public class AutoActions { public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { stamp = System.currentTimeMillis(); - manageShooter = manageShooterAuto(time, posX, posY, posH); + manageShooter = manageShooterAuto(time, posX, posY, posH, false); } ticker++; @@ -453,7 +453,8 @@ public class AutoActions { double time, double posX, double posY, - double posH + double posH, + boolean flywheelSensor ) { return new Action() { @@ -488,8 +489,10 @@ public class AutoActions { Pose2d deltaPose; if (posX != 0.501) { deltaPose = new Pose2d(posX, posY, Math.toRadians(posH)); + Turret.limelightUsed = false; } else { deltaPose = new Pose2d(dx, dy, robotHeading); + Turret.limelightUsed = true; } // double distanceToGoal = Math.sqrt(dx * dx + dy * dy); @@ -508,7 +511,7 @@ public class AutoActions { flywheel.manageFlywheel(targetingSettings.flywheelRPM); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean shouldFinish = timeDone || flywheel.getSteady(); + boolean shouldFinish = timeDone || (flywheel.getSteady() && flywheelSensor); 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 45186cf..2d6dbc4 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 @@ -45,16 +45,16 @@ public class Front_Poses { public static double rShoot0X = 40, rShoot0Y = 0.1, rShoot0H = 0.1; public static double bShoot0X = 40, bShoot0Y = -0.1, bShoot0H = -0.1; - public static double rShootGateX = 40, rShootGateY = 0.2, rShootGateH = 90; - public static double bShootGateX = 40, bShootGateY = -0.2, bShootGateH = -90; + public static double rShootGateX = 50, rShootGateY = 10, rShootGateH = 90; + public static double bShootGateX = 40, bShootGateY = 1, bShootGateH = -90; 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 rPickupGateAX = 24, rPickupGateAY = 50, rPickupGateAH = 140; public static double bPickupGateAX = 36, bPickupGateAY = -50, bPickupGateAH = -140; - public static double rPickupGateBX = 46, rPickupGateBY = 60, rPickupGateBH = 180; + public static double rPickupGateBX = 38, rPickupGateBY = 68, rPickupGateBH = 180; public static double bPickupGateBX = 46, bPickupGateBY = -60, bPickupGateBH = -180; 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 7e8e651..b7f5dbd 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 @@ -27,7 +27,7 @@ public class ServoPositions { public static double hoodAuto = 0.27; - public static double hoodOffset = -0.05; + public static double hoodOffset = 0.05; // offset from 0.93 public static double turret_redClose = 0; public static double turret_blueClose = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 3769e09..12a3c8b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.tests; 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.hoodOffset; 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; @@ -128,9 +129,9 @@ public class ShooterTest extends LinearOpMode { if (hoodPos != 0.501) { if (enableHoodAutoOpen) { - robot.hood.setPosition(hoodPos+(hoodAdjustFactor*(flywheel.getVelo()/Velocity))); + robot.hood.setPosition(hoodPos+(hoodAdjustFactor*(flywheel.getVelo()/Velocity)) + hoodOffset); } else { - robot.hood.setPosition(hoodPos); + robot.hood.setPosition(hoodPos + hoodOffset); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 0b5a945..06e532a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -24,8 +24,8 @@ public class Turret { public static double turrPosScalar = 0.00011264432; public static double turret180Range = 0.4; public static double turrDefault = 0.37; - public static double turrMin = 0.15; - public static double turrMax = 0.8; + public static double turrMin = 0.05; + public static double turrMax = 0.7; public static boolean limelightUsed = true; public static double manualOffset = 0.0;