From 0a81eb60d3a5b9798729e44866bad12e5b3cd3fb Mon Sep 17 00:00:00 2001 From: abhiramtx Date: Tue, 13 Jan 2026 18:47:04 -0600 Subject: [PATCH 1/3] moved ll to utils and got rid of spaghetti code for easier debugging --- .../ftc/teamcode/autonomous/AutoClose_V3.java | 305 +++++------------- .../ftc/teamcode/utils/LimelightManager.java | 103 ++++++ 2 files changed, 183 insertions(+), 225 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/LimelightManager.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java index b6ab1c0..8841c64 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java @@ -18,19 +18,16 @@ import com.acmerobotics.roadrunner.SequentialAction; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.FlywheelV2; +import org.firstinspires.ftc.teamcode.utils.LimelightManager; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; -import java.util.List; - @Config @Autonomous(preselectTeleOp = "TeleopV3") public class AutoClose_V3 extends LinearOpMode { @@ -39,20 +36,19 @@ public class AutoClose_V3 extends LinearOpMode { MecanumDrive drive; FlywheelV2 flywheel; Servos servo; + LimelightManager limelightManager; double velo = 0.0; public static double intake1Time = 2.7; public static double intake2Time = 3.0; public static double colorDetect = 3.0; - boolean gpp = false; - boolean pgp = false; - boolean ppg = false; + public static double holdTurrPow = 0.1; + + // Ball color detection: 0 = no ball, 1 = green, 2 = purple + int b1 = 0, b2 = 0, b3 = 0; + boolean gpp = false, pgp = false, ppg = false; double powPID = 0.0; double bearing = 0.0; - int b1 = 0; // 0 = no ball, 1 = green, 2 = purple - int b2 = 0;// 0 = no ball, 1 = green, 2 = purple - int b3 = 0;// 0 = no ball, 1 = green, 2 = purple - public static double holdTurrPow = 0.1; // power to hold turret in place public Action initShooter(int vel) { return new Action() { @@ -72,52 +68,34 @@ public class AutoClose_V3 extends LinearOpMode { public Action Obelisk() { return new Action() { - int id = 0; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { - LLResult result = robot.limelight.getLatestResult(); - if (result != null && result.isValid()) { - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - id = fiducial.getFiducialId(); - TELE.addData("ID", id); - TELE.update(); - } + limelightManager.update(); + int id = limelightManager.detectFiducial(); - } + if (id == 21) gpp = true; + else if (id == 22) pgp = true; + else if (id == 23) ppg = true; - if (id == 21){ - gpp = true; - } else if (id == 22){ - pgp = true; - } else if (id == 23){ - ppg = true; - } - - TELE.addData("Velocity", velo); + TELE.addData("Fiducial ID", id); TELE.addData("21", gpp); TELE.addData("22", pgp); TELE.addData("23", ppg); TELE.update(); if (gpp || pgp || ppg) { - if (redAlliance){ - robot.limelight.pipelineSwitch(3); - double turretPID = servo.setTurrPos(turret_redClose); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_redClose); + LimelightManager.LimelightMode mode = redAlliance ? + LimelightManager.LimelightMode.RED_GOAL : + LimelightManager.LimelightMode.BLUE_GOAL; + limelightManager.switchMode(mode); - } else { - robot.limelight.pipelineSwitch(2); - double turretPID = servo.setTurrPos(turret_blueClose); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_blueClose); - } - } else { - return true; + double turretTarget = redAlliance ? turret_redClose : turret_blueClose; + double turretPID = servo.setTurrPos(turretTarget); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turretTarget); } + return true; } }; } @@ -361,21 +339,14 @@ public class AutoClose_V3 extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); - flywheel = new FlywheelV2(); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - drive = new MecanumDrive(hardwareMap, new Pose2d( - 0, 0, 0 - )); - - robot.limelight.pipelineSwitch(1); - robot.limelight.start(); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + servo = new Servos(hardwareMap); + limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight); + limelightManager.init(); + limelightManager.switchMode(LimelightManager.LimelightMode.OBELISK_DETECTION); TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); @@ -559,12 +530,8 @@ public class AutoClose_V3 extends LinearOpMode { } //TODO: adjust this according to Teleop numbers public void detectTag() { - LLResult result = robot.limelight.getLatestResult(); - if (result != null) { - if (result.isValid()) { - bearing = result.getTx(); - } - } + limelightManager.update(); + bearing = limelightManager.getBearing(); double turretPos = servo.getTurrPos() - (bearing / 1300); double turretPID = servo.setTurrPos(turretPos); robot.turr1.setPower(turretPID); @@ -573,179 +540,67 @@ public class AutoClose_V3 extends LinearOpMode { public void shootingSequence() { TELE.addData("Velocity", velo); + + // Define sequences based on obelisk configuration + double[][] sequences = { + {1, 2, 3}, {1, 3, 2}, {2, 1, 3}, {2, 3, 1}, {3, 1, 2}, {3, 2, 1} + }; + + double[] sequence = null; + if (gpp) { if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } + if (b1 == 2 && b2 == b3) sequence = sequences[0]; // 1,2,3 + else if (b2 == 2 && b1 == b3) sequence = sequences[2]; // 2,1,3 + else if (b3 == 2 && b1 == b2) sequence = sequences[4]; // 3,1,2 + else sequence = sequences[0]; } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } + if (b1 == 2) sequence = sequences[0]; + else if (b2 == 2) sequence = sequences[2]; + else if (b3 == 2) sequence = sequences[4]; + } else sequence = sequences[0]; } else if (pgp) { if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence4(); - TELE.addLine("sequence4"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } + if (b1 == 2 && b2 == b3) sequence = sequences[2]; // 2,1,3 + else if (b2 == 2 && b1 == b3) sequence = sequences[0]; // 1,2,3 + else if (b3 == 2 && b1 == b2) sequence = sequences[3]; // 2,3,1 + else sequence = sequences[0]; } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2) { - sequence4(); - TELE.addLine("sequence4"); - } - } else { - sequence3(); - TELE.addLine("sequence3"); - } + if (b1 == 2) sequence = sequences[2]; + else if (b2 == 2) sequence = sequences[0]; + else if (b3 == 2) sequence = sequences[3]; + } else sequence = sequences[2]; } else if (ppg) { if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } + if (b1 == 2 && b2 == b3) sequence = sequences[4]; // 3,1,2 + else if (b2 == 2 && b1 == b3) sequence = sequences[5]; // 3,2,1 + else if (b3 == 2 && b1 == b2) sequence = sequences[0]; // 1,2,3 + else sequence = sequences[0]; } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } - } else { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } + if (b1 == 2) sequence = sequences[4]; + else if (b2 == 2) sequence = sequences[5]; + else if (b3 == 2) sequence = sequences[0]; + } else sequence = sequences[4]; + } else sequence = sequences[0]; + + executeShootingSequence(sequence); TELE.update(); } - public void sequence1() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } + private void executeShootingSequence(double[] sequence) { + double[] ballPositions = { + spindexer_outtakeBall1, + spindexer_outtakeBall2, + spindexer_outtakeBall3 + }; - public void sequence2() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence3() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence4() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence5() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence6() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); + for (double ball : sequence) { + Actions.runBlocking( + new SequentialAction( + spindex(ballPositions[(int) ball - 1], AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/LimelightManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/LimelightManager.java new file mode 100644 index 0000000..bc42434 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/LimelightManager.java @@ -0,0 +1,103 @@ +package org.firstinspires.ftc.teamcode.utils; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.List; + +@Config +public class LimelightManager { + private Limelight3A limelight; + private LLResult lastResult; + private int lastFiducialId = -1; + private double lastBearing = 0.0; + + public static final int PIPELINE_DEFAULT = 1; + public static final int PIPELINE_BLUE_DETECTION = 2; + public static final int PIPELINE_RED_DETECTION = 3; + + public enum LimelightMode { + OBELISK_DETECTION(PIPELINE_DEFAULT), + BLUE_GOAL(PIPELINE_BLUE_DETECTION), + RED_GOAL(PIPELINE_RED_DETECTION); + + public final int pipeline; + + LimelightMode(int pipeline) { + this.pipeline = pipeline; + } + } + + public LimelightManager(HardwareMap hardwareMap, boolean enabled) { + if (enabled) { + this.limelight = hardwareMap.get(Limelight3A.class, "limelight"); + } + } + + public void init() { + if (limelight != null) { + limelight.start(); + } + } + + public void switchMode(LimelightMode mode) { + if (limelight != null) { + limelight.pipelineSwitch(mode.pipeline); + } + } + + public void setPipeline(int pipeline) { + if (limelight != null) { + limelight.pipelineSwitch(pipeline); + } + } + + public void update() { + if (limelight != null) { + lastResult = limelight.getLatestResult(); + if (lastResult != null && lastResult.isValid()) { + lastBearing = lastResult.getTx(); + } + } + } + + public double getBearing() { + return lastBearing; + } + + public int detectFiducial() { + if (lastResult != null && lastResult.isValid()) { + List fiducials = lastResult.getFiducialResults(); + if (!fiducials.isEmpty()) { + lastFiducialId = fiducials.get(0).getFiducialId(); + return lastFiducialId; + } + } + return -1; + } + + public int getLastFiducialId() { + return lastFiducialId; + } + + public boolean isFiducialDetected(int id) { + return lastFiducialId == id; + } + + public LLResult getLatestResult() { + return lastResult; + } + + public boolean isConnected() { + return limelight != null; + } + + public Limelight3A getLimelight() { + return limelight; + } +} -- 2.43.0 From 73f804bd2ff3d616cbe2843fff8c17de13cf83de Mon Sep 17 00:00:00 2001 From: abhiramtx Date: Tue, 13 Jan 2026 18:57:49 -0600 Subject: [PATCH 2/3] updated all to use LLManager --- .../ftc/teamcode/autonomous/AutoFar_V1.java | 94 ++++++------------- 1 file changed, 31 insertions(+), 63 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java index 88dd86e..415cf1c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java @@ -18,19 +18,16 @@ import com.acmerobotics.roadrunner.SequentialAction; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.FlywheelV2; +import org.firstinspires.ftc.teamcode.utils.LimelightManager; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; -import java.util.List; - @Config @Autonomous(preselectTeleOp = "TeleopV3") public class AutoFar_V1 extends LinearOpMode { @@ -39,20 +36,19 @@ public class AutoFar_V1 extends LinearOpMode { MecanumDrive drive; FlywheelV2 flywheel; Servos servo; + LimelightManager limelightManager; double velo = 0.0; public static double intake1Time = 2.7; public static double intake2Time = 3.0; public static double colorDetect = 3.0; - boolean gpp = false; - boolean pgp = false; - boolean ppg = false; + public static double holdTurrPow = 0.1; + + // Ball color detection: 0 = no ball, 1 = green, 2 = purple + int b1 = 0, b2 = 0, b3 = 0; + boolean gpp = false, pgp = false, ppg = false; double powPID = 0.0; double bearing = 0.0; - int b1 = 0; // 0 = no ball, 1 = green, 2 = purple - int b2 = 0;// 0 = no ball, 1 = green, 2 = purple - int b3 = 0;// 0 = no ball, 1 = green, 2 = purple - public static double holdTurrPow = 0.1; // power to hold turret in place public Action initShooter(int vel) { return new Action() { @@ -72,52 +68,34 @@ public class AutoFar_V1 extends LinearOpMode { public Action Obelisk() { return new Action() { - int id = 0; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { - LLResult result = robot.limelight.getLatestResult(); - if (result != null && result.isValid()) { - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - id = fiducial.getFiducialId(); - TELE.addData("ID", id); - TELE.update(); - } + limelightManager.update(); + int id = limelightManager.detectFiducial(); - } + if (id == 21) gpp = true; + else if (id == 22) pgp = true; + else if (id == 23) ppg = true; - if (id == 21){ - gpp = true; - } else if (id == 22){ - pgp = true; - } else if (id == 23){ - ppg = true; - } - - TELE.addData("Velocity", velo); + TELE.addData("Fiducial ID", id); TELE.addData("21", gpp); TELE.addData("22", pgp); TELE.addData("23", ppg); TELE.update(); if (gpp || pgp || ppg) { - if (redAlliance){ - robot.limelight.pipelineSwitch(3); - double turretPID = servo.setTurrPos(turret_redFar); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_redFar); + LimelightManager.LimelightMode mode = redAlliance ? + LimelightManager.LimelightMode.RED_GOAL : + LimelightManager.LimelightMode.BLUE_GOAL; + limelightManager.switchMode(mode); - } else { - robot.limelight.pipelineSwitch(2); - double turretPID = servo.setTurrPos(turret_blueFar); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_blueFar); - } - } else { - return true; + double turretTarget = redAlliance ? turret_redFar : turret_blueFar; + double turretPID = servo.setTurrPos(turretTarget); + robot.turr1.setPower(turretPID); + robot.turr2.setPower(-turretPID); + return !servo.turretEqual(turretTarget); } + return true; } }; } @@ -363,19 +341,13 @@ public class AutoFar_V1 extends LinearOpMode { public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); - flywheel = new FlywheelV2(); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - drive = new MecanumDrive(hardwareMap, new Pose2d( - 0, 0, 0 - )); - - robot.limelight.pipelineSwitch(1); - robot.limelight.start(); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + servo = new Servos(hardwareMap); + limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight); + limelightManager.init(); + limelightManager.switchMode(LimelightManager.LimelightMode.OBELISK_DETECTION); //TODO: add positions to develop auto @@ -460,12 +432,8 @@ public class AutoFar_V1 extends LinearOpMode { } //TODO: adjust this according to Teleop numbers public void detectTag() { - LLResult result = robot.limelight.getLatestResult(); - if (result != null) { - if (result.isValid()) { - bearing = result.getTx(); - } - } + limelightManager.update(); + bearing = limelightManager.getBearing(); double turretPos = servo.getTurrPos() - (bearing / 1300); double turretPID = servo.setTurrPos(turretPos); robot.turr1.setPower(turretPID); -- 2.43.0 From 5931b2e7faa134a5945fd1791f03caca33c1422d Mon Sep 17 00:00:00 2001 From: abhiramtx Date: Tue, 13 Jan 2026 19:40:03 -0600 Subject: [PATCH 3/3] moved teleop LL stuff to LLManager - 3 instances were being created lol --- .../ftc/teamcode/teleop/TeleopV3.java | 47 +++++++++---------- 1 file changed, 22 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 3381753..d97e229 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -9,7 +9,6 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; -import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -17,6 +16,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.LimelightManager; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; @@ -31,6 +31,7 @@ public class TeleopV3 extends LinearOpMode { Servos servo; Flywheel flywheel; MecanumDrive drive; + LimelightManager limelightManager; public static double manualVel = 3000; public static double hoodDefaultPos = 0.5; @@ -101,15 +102,15 @@ public class TeleopV3 extends LinearOpMode { servo = new Servos(hardwareMap); flywheel = new Flywheel(); drive = new MecanumDrive(hardwareMap, teleStart); + limelightManager = new LimelightManager(hardwareMap, Robot.usingLimelight); + limelightManager.init(); if (redAlliance) { - robot.limelight.pipelineSwitch(3); + limelightManager.switchMode(LimelightManager.LimelightMode.RED_GOAL); } else { - robot.limelight.pipelineSwitch(2); + limelightManager.switchMode(LimelightManager.LimelightMode.BLUE_GOAL); } - robot.limelight.start(); - waitForStart(); if (isStopRequested()) return; while (opModeIsActive()) { @@ -304,30 +305,26 @@ public class TeleopV3 extends LinearOpMode { } if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1) { //not moving - double bearing; + limelightManager.update(); + double bearing = limelightManager.getBearing(); + + if (limelightManager.getLatestResult() != null && limelightManager.getLatestResult().isValid()) { + overrideTurr = true; + turretPos = servo.getTurrPos() - (bearing / 1300); - LLResult result = robot.limelight.getLatestResult(); - if (result != null) { - if (result.isValid()) { - bearing = result.getTx(); - overrideTurr = true; - turretPos = servo.getTurrPos() - (bearing / 1300); + double bearingCorrection = bearing / 1300; - double bearingCorrection = bearing / 1300; - - // deadband: ignore tiny noise - if (Math.abs(bearing) > 0.3 && camTicker < 8) { - - // only accumulate if bearing direction is consistent - if (Math.signum(bearingCorrection) == Math.signum(error) || - error == 0) { - error += bearingCorrection; - } + // deadband: ignore tiny noise + if (Math.abs(bearing) > 0.3 && camTicker < 8) { + // only accumulate if bearing direction is consistent + if (Math.signum(bearingCorrection) == Math.signum(error) || + error == 0) { + error += bearingCorrection; } - camTicker++; - TELE.addData("tx", bearing); - TELE.addData("ty", result.getTy()); } + camTicker++; + TELE.addData("tx", bearing); + TELE.addData("ty", limelightManager.getLatestResult().getTy()); } } else { -- 2.43.0