From 994fbb729586455f9c0cc4ef5c28fb6780877705 Mon Sep 17 00:00:00 2001 From: sch1afend Date: Tue, 21 Jul 2026 16:02:41 +0300 Subject: [PATCH 1/3] implemented limelight --- .../ftc/teamcode/kronbot/Robot.java | 108 ++++++++++++++++++ .../kronbot/manual/MainDrivingOp.java | 6 +- 2 files changed, 113 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index 882f1e5..fb7c2da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -5,10 +5,13 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.R; import org.firstinspires.ftc.teamcode.kronbot.utils.detection.AprilTagWebcam; import org.opencv.core.Mat; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_CLOSE; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_MAX; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.ANGLE_SERVO_MIN; @@ -49,9 +52,24 @@ import java.util.ArrayList; import java.util.Dictionary; import java.util.Enumeration; +import java.util.List; import java.util.Map; import java.util.TreeMap; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; + public class Robot extends KronBot { // Singleton instance @@ -619,4 +637,94 @@ public void telemetry(Telemetry telemetry) { telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower()); } } + + public static class Limelight { + + private Limelight3A limelight; + private Telemetry telemetry; + private LLResult result; + + // Call this once, from your OpMode's init(), passing in its hardwareMap and telemetry + public void init(HardwareMap hardwareMap, Telemetry telemetry) { + this.telemetry = telemetry; + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + limelight.setPollRateHz(100); // ask Limelight for data 100 times per second + limelight.pipelineSwitch(7); // switch to pipeline 7 + limelight.start(); // start looking + } + + // Call this once per loop() BEFORE calling telemetry(), so 'result' is fresh + public void update() { + result = limelight.getLatestResult(); + } + + public void telemetry() { + if (result == null) { + telemetry.addData("Limelight", "No data yet"); + return; + } + + if (result.isValid()) { + double tx = result.getTx(); // left/right (degrees) + double ty = result.getTy(); // up/down (degrees) + double ta = result.getTa(); // target size (0-100%) + + telemetry.addData("Target X", tx); + telemetry.addData("Target Y", ty); + telemetry.addData("Target Area", ta); + + Pose3D botpose = result.getBotpose(); + if (botpose != null) { + double x = botpose.getPosition().x; + double y = botpose.getPosition().y; + telemetry.addData("MT1 Location", "(" + x + ", " + y + ")"); + } + } else { + telemetry.addData("Limelight", "No Targets"); + } + + List colorTargets = result.getColorResults(); + for (LLResultTypes.ColorResult colorTarget : colorTargets) { + double x = colorTarget.getTargetXDegrees(); + double y = colorTarget.getTargetYDegrees(); + double area = colorTarget.getTargetArea(); + telemetry.addData("Color Target", "x=" + x + " y=" + y + " area=" + area + "%"); + } + + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + int id = fiducial.getFiducialId(); + double x = fiducial.getTargetXDegrees(); + double y = fiducial.getTargetYDegrees(); + Pose3D poseInTargetSpace = fiducial.getRobotPoseTargetSpace(); + double distance = poseInTargetSpace != null ? poseInTargetSpace.getPosition().y : -1; + telemetry.addData("Fiducial " + id, "x=" + x + " y=" + y + " dist=" + distance + "m"); + } + + List barcodes = result.getBarcodeResults(); + for (LLResultTypes.BarcodeResult barcode : barcodes) { + String data = barcode.getData(); + String family = barcode.getFamily(); + telemetry.addData("Barcode", data + " (" + family + ")"); + } + + List classifications = result.getClassifierResults(); + for (LLResultTypes.ClassifierResult classification : classifications) { + String className = classification.getClassName(); + double confidence = classification.getConfidence(); + telemetry.addData("I see a", className + " (" + confidence + "%)"); + } + + long staleness = result.getStaleness(); + if (staleness < 100) { + telemetry.addData("Data", "Good"); + } else { + telemetry.addData("Data", "Old (" + staleness + " ms)"); + } + } + + public LLResult getResult() { + return result; + } + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index d3ef9e4..a152090 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -54,6 +54,8 @@ public class MainDrivingOp extends OpMode { boolean rumbled = false; + Robot.Limelight ll = new Robot.Limelight(); + @Override public void init() { @@ -77,7 +79,7 @@ public void init() { drivingGP = new Controls(gamepad1); utilityGP = new Controls(gamepad2); - + ll.init(hardwareMap, telemetry); } @Override @@ -222,6 +224,7 @@ else if (robot.loader.speed < -0.2) //Update robot systems status robot.follower.setTeleOpDrive(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true); robot.updateAllSystems(); + ll.update(); _telemetry(); //robot.webcam.update(); } @@ -247,6 +250,7 @@ public void _telemetry() { robot.heading.telemetry(telemetry); robot.turret.telemetry(telemetry); drivingGP.telemetry(telemetry); + ll.telemetry(); telemetry.update(); } } \ No newline at end of file From 1d8f66feffb5eaaa65633773d276a6a9d1206a7c Mon Sep 17 00:00:00 2001 From: Cozma Vlad Date: Wed, 29 Jul 2026 14:22:57 +0300 Subject: [PATCH 2/3] Make the autoaim based on limelight with fallback on odometers when apriltag not detected. --- .../ftc/teamcode/kronbot/Robot.java | 358 ++++++++++++------ .../kronbot/manual/MainDrivingOp.java | 11 +- .../ftc/teamcode/kronbot/utils/Constants.java | 4 +- 3 files changed, 251 insertions(+), 122 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index fb7c2da..2a52304 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -21,6 +21,8 @@ import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.DELTA_THRESHOLD; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_CLOSED; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.FLAP_OPEN; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.LIMELIGHT_TURRET_KP; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.LIMELIGHT_TX_DEADBAND; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KD; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KF; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.OUT_MOTOR_KI; @@ -61,14 +63,7 @@ import com.qualcomm.hardware.limelightvision.LLStatus; import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; -import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; -import com.qualcomm.hardware.limelightvision.LLStatus; public class Robot extends KronBot { // Singleton instance @@ -85,6 +80,7 @@ public class Robot extends KronBot { public final Flap flap; public final Shoot shoot; public final Heading heading; + public final Limelight limelight; public boolean Blue_Target = false; @@ -111,6 +107,7 @@ public Robot() { this.shoot = new Shoot(); this.flap = new Flap(); this.heading = new Heading(); + this.limelight = new Limelight(); } // Get the singleton instance @@ -150,6 +147,7 @@ public void initSystems(HardwareMap hardwareMap) { public void updateAllSystems() { double rawHeading = follower.getHeading(); heading.update(rawHeading); + limelight.update(); outtake.update(); intake.update(); @@ -170,6 +168,200 @@ public void updateAllSystems() { // webcam.update(); } + public class Limelight { + + private static final int POLL_RATE_HZ = 30; + private static final int PIPELINE_INDEX = 7; + private static final long STALE_RESULT_MS = 500; + private static final long TARGET_LOST_GRACE_MS = 300; + + private Limelight3A limelight; + private Telemetry telemetry; + private LLResult result; + private long lastFreshTargetTimeMs = 0; + private boolean initialized = false; + private String lastFault = null; + + // Call this once, from your OpMode's init(), passing in its hardwareMap and telemetry + public void init(HardwareMap hardwareMap, Telemetry telemetry) { + this.telemetry = telemetry; + try { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + limelight.setPollRateHz(POLL_RATE_HZ); + limelight.pipelineSwitch(PIPELINE_INDEX); + limelight.start(); + initialized = true; + lastFault = null; + } catch (RuntimeException e) { + initialized = false; + lastFault = e.getClass().getSimpleName() + ": " + e.getMessage(); + } + } + + // Call this once per loop() BEFORE calling telemetry(), so 'result' is fresh + public void update() { + if (!initialized || limelight == null) { + return; + } + + try { + if (!limelight.isConnected()) { + lastFault = "Disconnected"; + result = null; + return; + } + + limelight.updateRobotOrientation(heading.get()); + result = limelight.getLatestResult(); + if (isFreshTarget(result)) { + lastFreshTargetTimeMs = System.currentTimeMillis(); + } + lastFault = null; + } catch (RuntimeException e) { + result = null; + lastFault = e.getClass().getSimpleName() + ": " + e.getMessage(); + } + } + + public void telemetry() { + telemetry.addLine("=== LIMELIGHT STATUS ==="); + + if (!initialized || limelight == null) { + telemetry.addData("Limelight", "Not initialized"); + if (lastFault != null) { + telemetry.addData("Fault", lastFault); + } + return; + } + + try { + telemetry.addData("Connected", limelight.isConnected()); + telemetry.addData("Last Update", limelight.getTimeSinceLastUpdate() + " ms"); + } catch (RuntimeException e) { + lastFault = e.getClass().getSimpleName() + ": " + e.getMessage(); + } + + if (lastFault != null) { + telemetry.addData("Fault", lastFault); + } + + if (result == null) { + telemetry.addData("Limelight", "No data yet"); + return; + } + + long staleness = result.getStaleness(); + if (staleness > STALE_RESULT_MS) { + telemetry.addData("Limelight", "Stale data (" + staleness + " ms)"); + return; + } + + if (result.isValid()) { + double tx = result.getTx(); // left/right (degrees) + double ty = result.getTy(); // up/down (degrees) + double ta = result.getTa(); // target size (0-100%) + + telemetry.addData("Target X", tx); + telemetry.addData("Target Y", ty); + telemetry.addData("Target Area", ta); + + // First, tell Limelight which way your robot is facing + double robotYaw = heading.get(); + limelight.updateRobotOrientation(robotYaw); + if (result != null && result.isValid()) { + Pose3D botpose_mt2 = result.getBotpose_MT2(); + if (botpose_mt2 != null) { + double x = botpose_mt2.getPosition().x; + double y = botpose_mt2.getPosition().y; + telemetry.addData("MT2 Location:", "(" + x + ", " + y + ")"); + } + } + + Pose3D botpose = result.getBotpose(); + if (botpose != null) { + double x = botpose.getPosition().x; + double y = botpose.getPosition().y; + telemetry.addData("MT1 Location", "(" + x + ", " + y + ")"); + } + } else { + telemetry.addData("Limelight", "No Targets"); + return; + } + + List colorTargets = result.getColorResults(); + for (LLResultTypes.ColorResult colorTarget : colorTargets) { + double x = colorTarget.getTargetXDegrees(); + double y = colorTarget.getTargetYDegrees(); + double area = colorTarget.getTargetArea(); + telemetry.addData("Color Target", "x=" + x + " y=" + y + " area=" + area + "%"); + } + + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + int id = fiducial.getFiducialId(); + double x = fiducial.getTargetXDegrees(); + double y = fiducial.getTargetYDegrees(); + Pose3D poseInTargetSpace = fiducial.getRobotPoseTargetSpace(); + double distance = poseInTargetSpace != null ? poseInTargetSpace.getPosition().y : -1; + telemetry.addData("Fiducial " + id, "x=" + x + " y=" + y + " dist=" + distance + "m"); + } + + List barcodes = result.getBarcodeResults(); + for (LLResultTypes.BarcodeResult barcode : barcodes) { + String data = barcode.getData(); + String family = barcode.getFamily(); + telemetry.addData("Barcode", data + " (" + family + ")"); + } + + List classifications = result.getClassifierResults(); + for (LLResultTypes.ClassifierResult classification : classifications) { + String className = classification.getClassName(); + double confidence = classification.getConfidence(); + telemetry.addData("I see a", className + " (" + confidence + "%)"); + } + + if (staleness < 100) { + telemetry.addData("Data", "Good"); + } else { + telemetry.addData("Data", "Old (" + staleness + " ms)"); + } + } + + public LLResult getResult() { + return result; + } + + public LLResult getFreshResult() { + return isFreshTarget(result) ? result : null; + } + + public boolean hasFreshTarget() { + return getFreshResult() != null; + } + + public boolean hasRecentTarget() { + return System.currentTimeMillis() - lastFreshTargetTimeMs <= TARGET_LOST_GRACE_MS; + } + + public long getTimeSinceFreshTargetMs() { + return System.currentTimeMillis() - lastFreshTargetTimeMs; + } + + private boolean isFreshTarget(LLResult result) { + return result != null && result.isValid() && result.getStaleness() <= STALE_RESULT_MS; + } + + public void stop() { + if (limelight != null) { + try { + limelight.stop(); + } catch (RuntimeException e) { + lastFault = e.getClass().getSimpleName() + ": " + e.getMessage(); + } + } + } + } + public class Outtake { public boolean on = false; public RangeConfig activeConfig; @@ -432,6 +624,9 @@ public class Turret { public double angle = 0; public double driverOffset = 0; private double servoPosition; + private String aimSource = "Odometry"; + private double limelightTx = 0; + private double limelightCorrection = 0; public boolean autoAimEnabled = true; @@ -447,19 +642,35 @@ public void update() { if (turretServo == null || follower == null) return; if(autoAimEnabled) { - - //Turret angle - double robot_X = follower.getPose().getX(); - double robot_Y = follower.getPose().getY(); - double robotHeading = heading.get(); - - double dy = (Blue_Target ? BASKET_BLUE_Y : BASKET_Y) - robot_Y; - double dx = BASKET_X - robot_X; - - double targetFieldAngle = Math.atan2(dy, dx); - - //calculate - double robotRelativeAngle = targetFieldAngle - robotHeading + driverOffset; + LLResult limelightResult = limelight.getFreshResult(); + double robotRelativeAngle; + + if (limelightResult != null) { + aimSource = "Limelight"; + limelightTx = limelightResult.getTx(); + if (Math.abs(limelightTx) > LIMELIGHT_TX_DEADBAND) { + limelightCorrection = -Math.toRadians(limelightTx) * LIMELIGHT_TURRET_KP; + } else { + limelightCorrection = 0; + } + robotRelativeAngle = angle + limelightCorrection; + } else if (limelight.hasRecentTarget()) { + aimSource = "Limelight Hold"; + limelightCorrection = 0; + robotRelativeAngle = angle; + } else { + aimSource = "Odometry"; + limelightTx = 0; + limelightCorrection = 0; + double robot_X = follower.getPose().getX(); + double robot_Y = follower.getPose().getY(); + double robotHeading = heading.get(); + + double dy = (Blue_Target ? BASKET_BLUE_Y : BASKET_Y) - robot_Y; + double dx = BASKET_X - robot_X; + double targetFieldAngle = Math.atan2(dy, dx); + robotRelativeAngle = targetFieldAngle - robotHeading + driverOffset; + } //normalize robotRelativeAngle = Math.atan2( @@ -467,17 +678,20 @@ public void update() { Math.cos(robotRelativeAngle) ); - servoPosition = robotRelativeAngle * TURRET_SERVO_UNITS_PER_RAD + 0.5; + angle = robotRelativeAngle; + servoPosition = angle * TURRET_SERVO_UNITS_PER_RAD + 0.5; } else { + aimSource = "Driver Offset"; + angle = driverOffset; servoPosition = driverOffset * TURRET_SERVO_UNITS_PER_RAD + 0.5; } - turretServo.setPosition( - Math.clamp(servoPosition, TURRET_SERVO_MIN, TURRET_SERVO_MAX) - ); + servoPosition = Math.clamp(servoPosition, TURRET_SERVO_MIN, TURRET_SERVO_MAX); + angle = (servoPosition - 0.5) / TURRET_SERVO_UNITS_PER_RAD; + turretServo.setPosition(servoPosition); // if (turretServo != null && follower != null) { @@ -510,6 +724,11 @@ public void update() { public void telemetry(Telemetry telemetry) { telemetry.addLine("=== TURRET STATUS ==="); telemetry.addData("Target Angle", "%.3f", angle); + telemetry.addData("Aim Source", aimSource); + telemetry.addData("Limelight Target", limelight.hasFreshTarget()); + telemetry.addData("Limelight Tx", "%.2f", limelightTx); + telemetry.addData("Limelight Correction", "%.4f", limelightCorrection); + telemetry.addData("Last Limelight Target", limelight.getTimeSinceFreshTargetMs() + " ms"); telemetry.addData("Robot Heading", "%.4f", follower.getHeading()); telemetry.addData("Servo Position", "%.3f", turretServo.getPosition()); telemetry.addData("Servo Range", "%.3f - %.3f", TURRET_SERVO_MIN, TURRET_SERVO_MAX); @@ -638,93 +857,4 @@ public void telemetry(Telemetry telemetry) { } } - public static class Limelight { - - private Limelight3A limelight; - private Telemetry telemetry; - private LLResult result; - - // Call this once, from your OpMode's init(), passing in its hardwareMap and telemetry - public void init(HardwareMap hardwareMap, Telemetry telemetry) { - this.telemetry = telemetry; - limelight = hardwareMap.get(Limelight3A.class, "limelight"); - limelight.setPollRateHz(100); // ask Limelight for data 100 times per second - limelight.pipelineSwitch(7); // switch to pipeline 7 - limelight.start(); // start looking - } - - // Call this once per loop() BEFORE calling telemetry(), so 'result' is fresh - public void update() { - result = limelight.getLatestResult(); - } - - public void telemetry() { - if (result == null) { - telemetry.addData("Limelight", "No data yet"); - return; - } - - if (result.isValid()) { - double tx = result.getTx(); // left/right (degrees) - double ty = result.getTy(); // up/down (degrees) - double ta = result.getTa(); // target size (0-100%) - - telemetry.addData("Target X", tx); - telemetry.addData("Target Y", ty); - telemetry.addData("Target Area", ta); - - Pose3D botpose = result.getBotpose(); - if (botpose != null) { - double x = botpose.getPosition().x; - double y = botpose.getPosition().y; - telemetry.addData("MT1 Location", "(" + x + ", " + y + ")"); - } - } else { - telemetry.addData("Limelight", "No Targets"); - } - - List colorTargets = result.getColorResults(); - for (LLResultTypes.ColorResult colorTarget : colorTargets) { - double x = colorTarget.getTargetXDegrees(); - double y = colorTarget.getTargetYDegrees(); - double area = colorTarget.getTargetArea(); - telemetry.addData("Color Target", "x=" + x + " y=" + y + " area=" + area + "%"); - } - - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - int id = fiducial.getFiducialId(); - double x = fiducial.getTargetXDegrees(); - double y = fiducial.getTargetYDegrees(); - Pose3D poseInTargetSpace = fiducial.getRobotPoseTargetSpace(); - double distance = poseInTargetSpace != null ? poseInTargetSpace.getPosition().y : -1; - telemetry.addData("Fiducial " + id, "x=" + x + " y=" + y + " dist=" + distance + "m"); - } - - List barcodes = result.getBarcodeResults(); - for (LLResultTypes.BarcodeResult barcode : barcodes) { - String data = barcode.getData(); - String family = barcode.getFamily(); - telemetry.addData("Barcode", data + " (" + family + ")"); - } - - List classifications = result.getClassifierResults(); - for (LLResultTypes.ClassifierResult classification : classifications) { - String className = classification.getClassName(); - double confidence = classification.getConfidence(); - telemetry.addData("I see a", className + " (" + confidence + "%)"); - } - - long staleness = result.getStaleness(); - if (staleness < 100) { - telemetry.addData("Data", "Good"); - } else { - telemetry.addData("Data", "Old (" + staleness + " ms)"); - } - } - - public LLResult getResult() { - return result; - } - } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java index a152090..704d94c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/MainDrivingOp.java @@ -54,9 +54,6 @@ public class MainDrivingOp extends OpMode { boolean rumbled = false; - Robot.Limelight ll = new Robot.Limelight(); - - @Override public void init() { lpsCounter = new LpsCounter(); @@ -79,7 +76,7 @@ public void init() { drivingGP = new Controls(gamepad1); utilityGP = new Controls(gamepad2); - ll.init(hardwareMap, telemetry); + robot.limelight.init(hardwareMap, telemetry); } @Override @@ -224,7 +221,6 @@ else if (robot.loader.speed < -0.2) //Update robot systems status robot.follower.setTeleOpDrive(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true); robot.updateAllSystems(); - ll.update(); _telemetry(); //robot.webcam.update(); } @@ -232,6 +228,7 @@ else if (robot.loader.speed < -0.2) @Override public void stop() { + robot.limelight.stop(); robot.webcam.stop(); } @@ -250,7 +247,7 @@ public void _telemetry() { robot.heading.telemetry(telemetry); robot.turret.telemetry(telemetry); drivingGP.telemetry(telemetry); - ll.telemetry(); + robot.limelight.telemetry(); telemetry.update(); } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index aaef1b4..1ad7771 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -108,6 +108,8 @@ public class Constants { public static double ANGLE_TOLERANCE = 2.0; public static double DELTA_THRESHOLD = 0.01; public static double MAX_ROTATION_POWER = 0.5; + public static double LIMELIGHT_TURRET_KP = 0.3; + public static double LIMELIGHT_TX_DEADBAND = 1.4; public static double BASKET_Y = -140; public static double BASKET_BLUE_Y = -20; @@ -117,4 +119,4 @@ public class Constants { public static AutonomousConstants.Coordinates RedTowerCoords = new AutonomousConstants.Coordinates(130, 130, 0); public static AutonomousConstants.Coordinates BlueTowerCoords = new AutonomousConstants.Coordinates(10, 135, 0); -} \ No newline at end of file +} From 1b7ac9b480aad41c76c4126870ec43e80d0b6956 Mon Sep 17 00:00:00 2001 From: Cozma Vlad Date: Wed, 29 Jul 2026 16:06:32 +0300 Subject: [PATCH 3/3] Adjust the constants and configure the code for the reversed camera. --- .../java/org/firstinspires/ftc/teamcode/kronbot/Robot.java | 2 +- .../firstinspires/ftc/teamcode/kronbot/utils/Constants.java | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java index 2a52304..b06026b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java @@ -649,7 +649,7 @@ public void update() { aimSource = "Limelight"; limelightTx = limelightResult.getTx(); if (Math.abs(limelightTx) > LIMELIGHT_TX_DEADBAND) { - limelightCorrection = -Math.toRadians(limelightTx) * LIMELIGHT_TURRET_KP; + limelightCorrection = Math.toRadians(limelightTx) * LIMELflaIGHT_TURRET_KP; } else { limelightCorrection = 0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index 1ad7771..d92e8d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -52,8 +52,8 @@ public class Constants { public static double ANGLE_SERVO_FAR = 0.72; public static double ANGLE_SERVO_MIN = 0; - public static double FLAP_CLOSED = 0.55; - public static double FLAP_OPEN = 1; + public static double FLAP_CLOSED = 0.3; + public static double FLAP_OPEN = 0.6; public static double INTAKE_DRIVER_POWER = 0.55; public static double INTAKE_DRIVER_REVERSE = -0.55; @@ -108,7 +108,7 @@ public class Constants { public static double ANGLE_TOLERANCE = 2.0; public static double DELTA_THRESHOLD = 0.01; public static double MAX_ROTATION_POWER = 0.5; - public static double LIMELIGHT_TURRET_KP = 0.3; + public static double LIMELIGHT_TURRET_KP = 0.25; public static double LIMELIGHT_TX_DEADBAND = 1.4; public static double BASKET_Y = -140;