From 2b9b0a140b8fee1571a775904adef2bac56bac16 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sun, 15 Feb 2026 17:03:49 -0600 Subject: [PATCH] lights added to auto --- .../ftc/teamcode/autonomous/Auto_LT_Close.java | 8 +++++++- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 10 ++++++++-- .../teamcode/autonomous/actions/AutoActions.java | 14 ++++++++++++-- .../firstinspires/ftc/teamcode/utils/Light.java | 2 +- 4 files changed, 28 insertions(+), 6 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 92dc1e4..6b004c1 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 @@ -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); 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 26ca9a9..fb08f5a 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 @@ -35,6 +35,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; @@ -62,6 +63,7 @@ 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; @@ -117,6 +119,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 +165,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; @@ -192,7 +198,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; 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 e950604..be1ab39 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 @@ -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(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java index dc77b07..6a5f09d 100644 --- 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;