From 5317405f86806fb8c6230d9cb1b8c947c0e27344 Mon Sep 17 00:00:00 2001 From: ImMihai689 Date: Fri, 20 Feb 2026 19:06:04 +0200 Subject: [PATCH 1/8] Added a driver class for the 2004 LCD module with a PCF8574 I2C adapter. --- .../kronbot/utils/components/I2cLcd.java | 196 ++++++++++++++++++ 1 file changed, 196 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/I2cLcd.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/I2cLcd.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/I2cLcd.java new file mode 100644 index 0000000..60beea4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/I2cLcd.java @@ -0,0 +1,196 @@ +package org.firstinspires.ftc.teamcode.kronbot.utils.components; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.I2cAddr; + +import org.firstinspires.ftc.teamcode.kronbot.utils.devices.PCF8574; + + +/** + * Driver class that uses a PCF8574 device to control a 2004 LCD module. + */ +public class I2cLcd { + + private static final byte RS_BIT = 1; + private static final byte RW_BIT = 2; + private static final byte EN_BIT = 4; + private static final byte BL_BIT = 8; + + private final PCF8574 pcf; + private byte backlight = 0; + + public I2cLcd(HardwareMap hardwareMap, String pcfName) { + pcf = hardwareMap.get(PCF8574.class, pcfName); + } + + /** + * Print a string to the display. Only call this after calling setCursor(). + * @param text The string. Make sure it doesn't overflow the row. + */ + public void print(String text) { + for(int i = 0; i < text.length(); i++) { + writeData((byte)text.charAt(i)); + } + } + + /** + * Set the cursor on the screen. Call this when writing on a new row, unless you want more advanced behavior + * @param row The row [0,3] + * @param col The column [0, 19] + */ + public void setCursor(int row, int col) { + int address = col; + switch (row) { + case 1: + address += 40; + break; + case 2: + address += 20; + break; + case 3: + address += 60; + break; + default: + break; + } + writeCommand((byte)(0b10000000 | (address & 0x7F))); + } + + /** + * Initialize the LCD. Can take up to 15ms.
+ * It is preferred to call this only once in init, + * if you want to clear the screen, use clear(). + * @param cursor If true, will show a cursor on the LCD where the next character written will appear + * @param cursorBlink If true, and if cursor is true, the cursor will blink + * @param backlight If true, the backlight will be turned on from init + */ + public void initLcd(boolean cursor, boolean cursorBlink, boolean backlight) { + sendInitCmd(); + try { + Thread.sleep(4); + } catch (InterruptedException ie) { + Thread.currentThread().interrupt(); + } + sendInitCmd(); + sendInitCmd(); + send4bitInit(); + + setBacklight(backlight); + + writeCommand((byte)0b00101000); // Set 4 bit mode, 2 lines and 5x8 font + if(cursor && cursorBlink) // Set display on, with optional cursor and blinking + writeCommand((byte)0b00001111); + else if(cursor) + writeCommand((byte)0b00001110); + else + writeCommand((byte)0b00001100); + writeCommand((byte)0b00000001); // Clear the display + writeCommand((byte)0b00000010); // Return the cursor home (top left) + writeCommand((byte)0b00000110); // Set the cursor to increment with each character + } + + /** + * Initialize the LCD. Can take up to 15ms.
+ * It is preferred to call this only once in init, + * if you want to clear the screen, use clear(). + */ + public void initLcd() { + initLcd(false, false, true); + } + + /** + * Write a byte of data to the LCD + * @param data Refer to the HD44780 datasheet for more details + */ + protected void writeData(byte data) { + /* Wait for the lcd to not be busy */ + while(busy()); + + byte snd = (byte)((data & 0xF0 | RS_BIT | backlight)); + pcf.writeByte(snd | EN_BIT); + + snd = (byte)((data << 4) | RS_BIT | backlight); + pcf.writeByte(snd); + pcf.writeByte(snd | EN_BIT); + pcf.writeByte(snd); + } + + /** + * Write a command to the LCD + * @param cmd Refer to the HD44780 datasheet for more details + */ + protected void writeCommand(byte cmd) { + /* Wait for the lcd to not be busy */ + while(busy()); + + byte snd = (byte)((cmd & 0xF0) | backlight); + pcf.writeByte(snd | EN_BIT); + + snd = (byte)((cmd << 4) | backlight); + pcf.writeByte(snd); + pcf.writeByte(snd | EN_BIT); + pcf.writeByte(snd); + } + + protected boolean busy() { + /* Extract the busy flag from the read data */ + return (read() & 0x80) != 0; + } + + /** + * Read the busy flag and the address counter from the LCD + * @return The read data (BF[7] and AC[6:0]) + */ + protected byte read() { + byte snd = (byte)(0b11110000 | RW_BIT | backlight); + byte readData = 0; + + pcf.writeByte(snd); + pcf.writeByte(snd | EN_BIT); + readData = (byte)(pcf.readByte() & 0xF0); + pcf.writeByte(snd); + pcf.writeByte(snd | EN_BIT); + readData = (byte)(pcf.readByte() >> 4); + pcf.writeByte(snd); + + return readData; + } + + /** + * Sets the backlight value. Will update on the next call that writes to the screen, unless immediate is true. + * @param backlightOn If true, the backlight will turn on, and vice versa. + * @param immediate If true, will send a command that turns on the backlight. + * Do not use immediate while communication is undergoing. + */ + public void setBacklight(boolean backlightOn, boolean immediate) { + if(backlightOn) + backlight = BL_BIT; + if(immediate) + pcf.writeByte(BL_BIT); + } + + /** + * Sets the backlight value. Will update on the next call that writes to the screen. + * @param backlightOn If true, the backlight will turn on, and vice versa. + */ + public void setBacklight(boolean backlightOn) { + setBacklight(backlightOn, false); + } + + private void sendInitCmd() { + pcf.writeByte(0b00110000); + pcf.writeByte(0b00110000 | EN_BIT); + pcf.writeByte(0b00110000); + } + + private void send4bitInit() { + pcf.writeByte(0b00100000); + pcf.writeByte(0b00100000 | EN_BIT); + pcf.writeByte(0b00100000); + } + + /** Changes the address of the device. Call if the address is not the default (0x27) */ + public void changeI2cAddress(byte new7bitAddress) { + pcf.getDeviceClient().setI2cAddress(I2cAddr.create7bit(new7bitAddress)); + } +} From 680dd0f194f2480526297a5e3d381ee14097a3b7 Mon Sep 17 00:00:00 2001 From: ImMihai689 Date: Fri, 20 Feb 2026 19:06:27 +0200 Subject: [PATCH 2/8] Moved the PCF8574.java file to a more reasonable place. --- .../ftc/teamcode/kronbot/utils/components/I2cLedBar.java | 2 +- .../teamcode/kronbot/utils/devices/{PCF8574 => }/PCF8574.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/devices/{PCF8574 => }/PCF8574.java (96%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/I2cLedBar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/I2cLedBar.java index f1499fe..1aa74f8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/I2cLedBar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/components/I2cLedBar.java @@ -3,7 +3,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.I2cAddr; -import org.firstinspires.ftc.teamcode.kronbot.utils.devices.PCF8574.PCF8574; +import org.firstinspires.ftc.teamcode.kronbot.utils.devices.PCF8574; /** * Uses two PCF8574 I/O expanders to control an LED bar diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/devices/PCF8574/PCF8574.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/devices/PCF8574.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/devices/PCF8574/PCF8574.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/devices/PCF8574.java index b9e9002..44e3fd9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/devices/PCF8574/PCF8574.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/devices/PCF8574.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.kronbot.utils.devices.PCF8574; +package org.firstinspires.ftc.teamcode.kronbot.utils.devices; import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch; import com.qualcomm.robotcore.hardware.I2cAddr; From 994fbb729586455f9c0cc4ef5c28fb6780877705 Mon Sep 17 00:00:00 2001 From: sch1afend Date: Tue, 21 Jul 2026 16:02:41 +0300 Subject: [PATCH 3/8] 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 1080b76ec9457b85479296db7435a41517e81ab5 Mon Sep 17 00:00:00 2001 From: Cozma Vlad Date: Thu, 23 Jul 2026 16:45:31 +0300 Subject: [PATCH 4/8] Update android app version --- FtcRobotController/src/main/AndroidManifest.xml | 4 ++-- build.common.gradle | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index c873221..143c1f1 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="62" + android:versionName="11.2"> diff --git a/build.common.gradle b/build.common.gradle index d8099a7..f586b4f 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -21,7 +21,7 @@ apply plugin: 'com.android.application' android { - compileSdkVersion 30 + compileSdkVersion 36 signingConfigs { release { From 1d8f66feffb5eaaa65633773d276a6a9d1206a7c Mon Sep 17 00:00:00 2001 From: Cozma Vlad Date: Wed, 29 Jul 2026 14:22:57 +0300 Subject: [PATCH 5/8] 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 6/8] 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; From 0d09556addefda1bd6814261b89ca3124e69b059 Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Mon, 3 Aug 2026 16:38:42 +0300 Subject: [PATCH 7/8] added blue and red pipelines --- .../ftc/teamcode/kronbot/Robot.java | 36 ++++++++++++++----- .../kronbot/manual/MainDrivingOp.java | 13 +++++-- 2 files changed, 38 insertions(+), 11 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 c17260c..63b30e8 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 @@ -1,18 +1,11 @@ package org.firstinspires.ftc.teamcode.kronbot; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Gamepad; 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; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.BASKET_BLUE_Y; @@ -173,8 +166,12 @@ 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 int PIPELINE_RED = 8; + private static final int PIPELINE_BLUE = 7; private static final long TARGET_LOST_GRACE_MS = 300; + private boolean usingBluePipeline = false; + private Limelight3A limelight; private Telemetry telemetry; private LLResult result; @@ -188,7 +185,8 @@ public void init(HardwareMap hardwareMap, Telemetry telemetry) { try { limelight = hardwareMap.get(Limelight3A.class, "limelight"); limelight.setPollRateHz(POLL_RATE_HZ); - limelight.pipelineSwitch(PIPELINE_INDEX); + limelight.pipelineSwitch(PIPELINE_RED); // default to red at init + usingBluePipeline = false; limelight.start(); initialized = true; lastFault = null; @@ -198,6 +196,26 @@ public void init(HardwareMap hardwareMap, Telemetry telemetry) { } } + public void switchPipeline(boolean blue) { + if (!initialized || limelight == null) return; + try { + limelight.pipelineSwitch(blue ? PIPELINE_BLUE : PIPELINE_RED); + usingBluePipeline = blue; + lastFault = null; + } catch (RuntimeException e) { + lastFault = e.getClass().getSimpleName() + ": " + e.getMessage(); + } + } + + public void togglePipeline() { + switchPipeline(!usingBluePipeline); + } + public boolean isUsingBluePipeline() { + return usingBluePipeline; + } + + + // Call this once per loop() BEFORE calling telemetry(), so 'result' is fresh public void update() { if (!initialized || limelight == null) { @@ -649,7 +667,7 @@ public void update() { aimSource = "Limelight"; limelightTx = limelightResult.getTx(); if (Math.abs(limelightTx) > LIMELIGHT_TX_DEADBAND) { - limelightCorrection = Math.toRadians(limelightTx) * LIMELflaIGHT_TURRET_KP; + limelightCorrection = Math.toRadians(limelightTx) * LIMELIGHT_TURRET_KP; } else { limelightCorrection = 0; } 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 6d9494d..7458802 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 @@ -107,6 +107,14 @@ public void loop() { robot.intake.speed = utilityGP.rightStick.y; robot.intake.reversed = INTAKE_REVERSE; +// if (drivingGP.leftStick.button.justPressed()) +// robot.limelight.togglePipeline(); + + if(drivingGP.rightStick.button.justPressed()) { + robot.Blue_Target = !robot.Blue_Target; + robot.limelight.switchPipeline(robot.Blue_Target); + } + //Loader if (!drivingGP.rightBumper.pressed()) { robot.loader.speed = utilityGP.leftStick.y; @@ -114,10 +122,11 @@ public void loop() { } else { robot.loader.speed = (drivingGP.rightTrigger - drivingGP.leftTrigger) * 0.8; robot.flap.open = true; + if (robot.loader.speed > 0.1) - robot.intake.speed = INTAKE_DRIVER_POWER; + robot.intake.speed = INTAKE_DRIVER_REVERSE; // swapped else if (robot.loader.speed < -0.2) - robot.intake.speed = INTAKE_DRIVER_REVERSE; + robot.intake.speed = INTAKE_DRIVER_POWER; // swapped else robot.intake.speed = 0; } From 0bac2298619bea045424d14f9c9cb025d20dafe2 Mon Sep 17 00:00:00 2001 From: ChiriacIoana Date: Mon, 3 Aug 2026 17:01:05 +0300 Subject: [PATCH 8/8] changed constants for basket --- .../firstinspires/ftc/teamcode/kronbot/utils/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 d983985..2be8b47 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 @@ -111,8 +111,8 @@ public class Constants { public static double LIMELIGHT_TURRET_KP = 0.25; public static double LIMELIGHT_TX_DEADBAND = 1.4; - public static double BASKET_Y = -140; - public static double BASKET_BLUE_Y = -20; + public static double BASKET_Y = 140; + public static double BASKET_BLUE_Y = 20; public static double BASKET_X = 130;