back auto gate cycling in progress

lights added to auto
2026-02-15 18:03:37 -06:00 · 2026-02-15 17:03:49 -06:00
5 changed files with 106 additions and 26 deletions
--- 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
@@ -22,6 +22,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Light;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -78,6 +79,7 @@ public class Auto_LT_Close extends LinearOpMode {
    Targeting targeting;
    Targeting.Settings targetingSettings;
    AutoActions autoActions;
+    Light light;
    double x1, y1, h1;

    double x2a, y2a, h2a, t2a;
@@ -132,7 +134,11 @@ public class Auto_LT_Close extends LinearOpMode {

        drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));

-        autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
+        light = Light.getInstance();
+
+        light.init(robot.light, spindexer, turret);
+
+        autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);

        servos.setSpinPos(spinStartPos);

--- 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
@@ -3,26 +3,14 @@ package org.firstinspires.ftc.teamcode.autonomous;
 import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*;
 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.spinEndPos;
 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;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
-import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;

-import androidx.annotation.NonNull;
-
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.ParallelAction;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
@@ -35,14 +23,13 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Light;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
 import org.firstinspires.ftc.teamcode.utils.Turret;

-import java.util.Objects;
-
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Far extends LinearOpMode {
@@ -62,15 +49,16 @@ public class Auto_LT_Far extends LinearOpMode {
    Targeting targeting;
    Targeting.Settings targetingSettings;
    AutoActions autoActions;
+    Light light;
    double xShoot, yShoot, hShoot;
    double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
-    double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
    public static double flywheel0Time = 1.5;
-    boolean gatePickup = false;
+    boolean gatePickup = true;
    boolean stack3 = true;
    double xStackPickupA, yStackPickupA, hStackPickupA;
    double xStackPickupB, yStackPickupB, hStackPickupB;
    public static int pickupStackSpeed = 12;
+    public static int pickupGateSpeed = 25;
    int prevMotif = 0;
    public static double spindexerSpeedIncrease = 0.008;
    public static double shootAllTime = 2;
@@ -80,8 +68,8 @@ public class Auto_LT_Far extends LinearOpMode {
    public static double shootStackTime = 2;
    public static double shootGateTime = 2;
    public static double colorSenseTime = 1;
-    public static double intakeStackTime = 3.3;
-    public static double intakeGateTime = 3;
+    public static double intakeStackTime = 3;
+    public static double intakeGateTime = 2.5;
    public static double redObeliskTurrPos1 = 0.12;
    public static double redObeliskTurrPos2 = 0.13;
    public static double redObeliskTurrPos3 = 0.14;
@@ -91,12 +79,15 @@ public class Auto_LT_Far extends LinearOpMode {
    double obeliskTurrPos1 = 0.0;
    double obeliskTurrPos2 = 0.0;
    double obeliskTurrPos3 = 0.0;
+    public static double endAutoTime = 25;

    // initialize path variables here
    TrajectoryActionBuilder leave3Ball = null;
    TrajectoryActionBuilder leaveFromShoot = null;
    TrajectoryActionBuilder pickup3 = null;
    TrajectoryActionBuilder shoot3 = null;
+    TrajectoryActionBuilder pickupGate = null;
+    TrajectoryActionBuilder shootGate = null;
    Pose2d autoStart = new Pose2d(0,0,0);

    @Override
@@ -117,6 +108,10 @@ public class Auto_LT_Far extends LinearOpMode {

        servos = new Servos(hardwareMap);

+        light = Light.getInstance();
+
+        light.init(robot.light, spindexer, turret);
+
        robot.limelight.start();

        robot.limelight.pipelineSwitch(1);
@@ -159,7 +154,7 @@ public class Auto_LT_Far extends LinearOpMode {

                autoStart = new Pose2d(autoStartRX, autoStartRY, Math.toRadians(autoStartRH));
                drive = new MecanumDrive(hardwareMap, autoStart);
-                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
+                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);

                xLeave = rLeaveX;
                yLeave = rLeaveY;
@@ -177,6 +172,10 @@ public class Auto_LT_Far extends LinearOpMode {
                yStackPickupB = rStackPickupBY;
                hStackPickupB = rStackPickupBH;

+                pickupGateX = rPickupGateX;
+                pickupGateY = rPickupGateY;
+                pickupGateH = rPickupGateH;
+
                obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
                obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
                obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
@@ -192,7 +191,7 @@ public class Auto_LT_Far extends LinearOpMode {

                autoStart = new Pose2d(autoStartBX, autoStartBY, Math.toRadians(autoStartBH));
                drive = new MecanumDrive(hardwareMap, autoStart);
-                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret);
+                autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);

                xLeave = bLeaveX;
                yLeave = bLeaveY;
@@ -210,6 +209,10 @@ public class Auto_LT_Far extends LinearOpMode {
                yStackPickupB = bStackPickupBY;
                hStackPickupB = bStackPickupBH;

+                pickupGateX = bPickupGateX;
+                pickupGateY = bPickupGateY;
+                pickupGateH = bPickupGateH;
+
                obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
                obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
                obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
@@ -236,6 +239,13 @@ public class Auto_LT_Far extends LinearOpMode {
            shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB)))
                    .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),
+                            new TranslationalVelConstraint(pickupGateSpeed));
+
+            shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH)))
+                    .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
+
            limelightUsed = true;

            TELE.addData("Red?", redAlliance);
@@ -253,6 +263,7 @@ public class Auto_LT_Far extends LinearOpMode {

        // Currently only shoots; keep this start and modify times and then add extra paths
        if (opModeIsActive()) {
+            double stamp = getRuntime();

            robot.transfer.setPower(1);

@@ -264,7 +275,10 @@ public class Auto_LT_Far extends LinearOpMode {
                shoot();
            }

-            //TODO: insert code here for gate auto
+            while (gatePickup && getRuntime() - stamp < endAutoTime){
+                cycleGatePickup();
+                shoot();
+            }

            if (gatePickup || stack3){
                leave();
@@ -382,4 +396,51 @@ public class Auto_LT_Far extends LinearOpMode {
                )
        );
    }
+
+    void cycleGatePickup(){
+        Actions.runBlocking(
+                new ParallelAction(
+                        pickupGate.build(),
+                        autoActions.manageShooterAuto(
+                                intakeGateTime,
+                                pickupGateX,
+                                pickupGateY,
+                                posXTolerance,
+                                posYTolerance,
+                                pickupGateH,
+                                true
+                        ),
+                        autoActions.intake(intakeStackTime),
+                        autoActions.detectObelisk(
+                                intakeGateTime,
+                                pickupGateX,
+                                pickupGateY,
+                                posXTolerance,
+                                posYTolerance,
+                                obeliskTurrPos3
+                        )
+                )
+        );
+
+        motif = turret.getObeliskID();
+
+        if (motif == 0) motif = prevMotif;
+        prevMotif = motif;
+
+        Actions.runBlocking(
+                new ParallelAction(
+                        autoActions.manageShooterAuto(
+                                shootGateTime,
+                                xShoot,
+                                yShoot,
+                                posXTolerance,
+                                posYTolerance,
+                                hShoot,
+                                false
+                        ),
+                        shootGate.build(),
+                        autoActions.prepareShootAll(colorSenseTime, shootGateTime, motif)
+                )
+        );
+    }
 }
--- 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
@@ -21,8 +21,10 @@ import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.Pose2d;

 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.constants.StateEnums;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
+import org.firstinspires.ftc.teamcode.utils.Light;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.teamcode.utils.Spindexer;
@@ -41,6 +43,7 @@ public class AutoActions{
    Spindexer spindexer;
    Targeting targeting;
    Targeting.Settings targetingSettings;
+    Light light;
    Turret turret;
    private int driverSlotGreen = 0;
    private int passengerSlotGreen = 0;
@@ -52,7 +55,7 @@ public class AutoActions{
    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){
+    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;
@@ -62,6 +65,7 @@ public class AutoActions{
        this.targeting = tar;
        this.targetingSettings = tS;
        this.turret = tur;
+        this.light = lig;
    }

    public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
@@ -112,6 +116,8 @@ public class AutoActions{
                servos.setTransferPos(transferServo_out);
                drive.updatePoseEstimate();

+                light.setState(StateEnums.LightState.GOAL_LOCK);
+
                teleStart = drive.localizer.getPose();

                if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
@@ -215,6 +221,9 @@ public class AutoActions{

                spindexer.setIntakePower(-0.1);

+                light.setState(StateEnums.LightState.BALL_COLOR);
+                light.update();
+
                if (ticker == 1) {
                    stamp = System.currentTimeMillis();
                }
@@ -276,8 +285,9 @@ public class AutoActions{

                spindexer.processIntake();
                spindexer.setIntakePower(1);
+                light.setState(StateEnums.LightState.BALL_COUNT);
+                light.update();

-                spindexer.ballCounterLight();
                drive.updatePoseEstimate();

                teleStart = drive.localizer.getPose();
--- 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
@@ -14,9 +14,12 @@ public class Back_Poses {
    public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
    public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;

-    public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1;
+    public static double rStackPickupBX = 55, rStackPickupBY = 83, rStackPickupBH = 140.1;
    public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;

+    public static double rPickupGateX = 65, rPickupGateY = 80, rPickupGateH = 140;
+    public static double bPickupGateX = 60, bPickupGateY = -88, bPickupGateH = -140;
+
    public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;
    public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;

--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java
@@ -12,7 +12,7 @@ public final class Light {

    private static Light instance;
    public static  double ballColorCycleTime = 1000; //in ms
-    public static double restingTime = 150; //in ms
+    public static double restingTime = 125; //in ms

    private Servo lightServo;
    private LightState state = LightState.DISABLED;
Author	SHA1	Message	Date
DanTheMan-byte	85989d54b9	back auto gate cycling in progress	2026-02-15 18:03:37 -06:00
DanTheMan-byte	2b9b0a140b	lights added to auto	2026-02-15 17:03:49 -06:00