From b342c9814937928eb2ca982089a2b7dd3c57d7e6 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 28 Feb 2026 12:02:38 -0600 Subject: [PATCH] gate auto coded --- .../teamcode/autonomous/Auto_LT_Close.java | 76 +++++++++---------- .../ftc/teamcode/constants/Front_Poses.java | 6 ++ 2 files changed, 44 insertions(+), 38 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 cc5af40..4aef555 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 @@ -90,10 +90,6 @@ public class Auto_LT_Close extends LinearOpMode { public static double intake1GateTime = 3.3; public static double lastShootTime = 27; - public static double openGateX = 26; - public static double openGateY = 48; - public static double openGateH = Math.toRadians(155); - Robot robot; MultipleTelemetry TELE; MecanumDrive drive; @@ -125,6 +121,8 @@ public class Auto_LT_Close extends LinearOpMode { double xShootGate, yShootGate, hShootGate; double xLeave, yLeave, hLeave; double xLeaveGate, yLeaveGate, hLeaveGate; + double openGateCloseX = 0, openGateCloseY = 0, openGateCloseH = 0; + double openGateMiddleX = 0, openGateMiddleY = 0, openGateMiddleH = 0; int ballCycles = 3; int prevMotif = 0; @@ -310,6 +308,14 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = rPickupGateBY; pickupGateBH = rPickupGateBH; + openGateCloseX = rOpenGateCloseX; + openGateCloseY = rOpenGateCloseY; + openGateCloseH = rOpenGateCloseH; + + openGateMiddleX = rOpenGateMiddleX; + openGateMiddleY = rOpenGateMiddleY; + openGateMiddleH = rOpenGateMiddleH; + obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -372,25 +378,34 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = bPickupGateBY; pickupGateBH = bPickupGateBH; + openGateCloseX = bOpenGateCloseX; + openGateCloseY = bOpenGateCloseY; + openGateCloseH = bOpenGateCloseH; + + openGateMiddleX = bOpenGateMiddleX; + openGateMiddleY = bOpenGateMiddleY; + openGateMiddleH = bOpenGateMiddleH; + obeliskTurrPosAutoStart = turrDefault + blueObeliskTurrPos0; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; } - if (gateCycle) { - shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0)); - } else { +// if (gateCycle) { +// shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) +// .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0)); +// } else { shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); - } +// } if (gateCycle) { pickup2 = shoot0.endTrajectory().fresh() .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), - new TranslationalVelConstraint(pickupStackGateSpeed)); + new TranslationalVelConstraint(pickupStackGateSpeed)) + .strafeToLinearHeading(new Vector2d(openGateMiddleX, openGateMiddleY), Math.toRadians(openGateMiddleH)); } else { pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) @@ -398,13 +413,14 @@ public class Auto_LT_Close extends LinearOpMode { new TranslationalVelConstraint(pickup1Speed)); } - if (gateCycle&& withPartner) { +// if (gateCycle && withPartner) { +// shoot2 = pickup2.endTrajectory().fresh() +// .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH)) +// .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH)); +// } else + if (gateCycle) { shoot2 = pickup2.endTrajectory().fresh() - .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH)) - .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH)); - } else if (gateCycle) { - shoot2 = pickup2.endTrajectory().fresh() - .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); } else if (ballCycles < 3) { shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); @@ -426,10 +442,11 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { - pickup1 = gateCycleShoot.endTrajectory().fresh() + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), - new TranslationalVelConstraint(pickupStackGateSpeed)); + new TranslationalVelConstraint(pickupStackGateSpeed)) + .strafeToLinearHeading(new Vector2d(openGateCloseX, openGateCloseY), Math.toRadians(openGateCloseH)); } else { pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) @@ -440,7 +457,7 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { shoot1 = pickup1.endTrajectory().fresh() - .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate)); + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); } else if (ballCycles < 2) { shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); @@ -488,28 +505,11 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { startAutoGate(); shoot(0.501, 0.501, 0.501); - cycleStackMiddleGate(); + cycleStackClose(); shoot(0.501,0.501, 0.501); - - while (getRuntime() - stamp < endGateTime) { - cycleGateIntake(); - if (getRuntime() - stamp < lastShootTime) { - cycleGateShoot(); - shoot(0.501, 0.501, 0.501); - } - } - cycleStackCloseIntakeGate(); - - if (getRuntime() - stamp < lastShootTime) { - cycleStackCloseShootGate(); - } - + cycleStackMiddle(); shoot(0.501, 0.501, 0.501); - } else { - - - startAuto(); shoot(0.501, 0.501,0.501); 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 ccfb8d3..51d4117 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 @@ -58,5 +58,11 @@ public class Front_Poses { public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180; public static double pickupGateCX = 34, pickupGateCY = 58, pickupGateCH = 220; + public static double rOpenGateCloseX = 20, rOpenGateCloseY = 35, rOpenGateCloseH = 230; + public static double bOpenGateCloseX = 20, bOpenGateCloseY = -35, bOpenGateCloseH = -230; + + public static double rOpenGateMiddleX = 36, rOpenGateMiddleY = 59, rOpenGateMiddleH = 50; + public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -500; + public static Pose2d teleStart = new Pose2d(0, 0, 0); }