Merge auto-aim based on limelight#6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
vladuzzul merged 5 commits into from
Aug 3, 2026
Merged

Merge auto-aim based on limelight #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
994fbb7
implemented limelight
sch1afend Jul 21, 2026
4b2501a
Merge pull request #4 from kronbot/main
vladuzzul Jul 29, 2026
1d8f66f
Make the autoaim based on limelight with fallback on odometers when a…
vladuzzul Jul 29, 2026
1b7ac9b
Adjust the constants and configure the code for the reversed camera.
vladuzzul Jul 29, 2026
afa081b
Merge branch 'main' into Cosmin
vladuzzul Aug 3, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
274 changes: 256 additions & 18 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java
View file
Open in desktop
Original file line numberDiff line numberDiff line change
Expand Up@@ -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;
Comment on lines +13 to +14
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;
Expand All@@ -18,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;
Expand DownExpand Up@@ -49,9 +54,17 @@
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.util.ElapsedTime;

public class Robot extends KronBot {
// Singleton instance

Expand All@@ -67,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;

Expand All@@ -93,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
Expand DownExpand Up@@ -132,6 +147,7 @@ public void initSystems(HardwareMap hardwareMap) {
public void updateAllSystems() {
double rawHeading = follower.getHeading();
heading.update(rawHeading);
limelight.update();

outtake.update();
intake.update();
Expand All@@ -152,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<LLResultTypes.ColorResult> 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<LLResultTypes.FiducialResult> 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<LLResultTypes.BarcodeResult> barcodes = result.getBarcodeResults();
for (LLResultTypes.BarcodeResult barcode : barcodes) {
String data = barcode.getData();
String family = barcode.getFamily();
telemetry.addData("Barcode", data + " (" + family + ")");
}

List<LLResultTypes.ClassifierResult> 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;
Expand DownExpand Up@@ -414,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;
Expand All@@ -429,43 +642,67 @@ 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) * LIMELflaIGHT_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(
Math.sin(robotRelativeAngle),
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);

}

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);
Expand DownExpand Up@@ -593,4 +830,5 @@ public void telemetry(Telemetry telemetry) {
telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower());
}
}
}

}
Loading
, 'i'); if (__m === '*' || __re.test(location.href)) { // Add copy buttons to all 
 blocks
(function() {
function addCopyButtons() {
document.querySelectorAll('pre code').forEach(function(codeBlock) {
if (codeBlock.parentElement.hasAttribute('data-copy-added')) return;
codeBlock.parentElement.setAttribute('data-copy-added', 'true');
var btn = document.createElement('button');
btn.textContent = 'Copy';
btn.style.cssText = 'position:absolute;top:4px;right:4px;padding:2px 8px;font-size:11px;background:#4ecdc4;border:none;border-radius:4px;color:#1a1a2e;cursor:pointer;opacity:0.7;transition:opacity 0.2s;';
btn.onmouseover = function() { this.style.opacity = '1'; };
btn.onmouseout = function() { this.style.opacity = '0.7'; };
btn.onclick = function() {
navigator.clipboard.writeText(codeBlock.textContent).then(function() {
btn.textContent = 'Copied!';
setTimeout(function() { btn.textContent = 'Copy'; }, 1500);
});
};
codeBlock.parentElement.style.position = 'relative';
codeBlock.parentElement.appendChild(btn);
});
}
addCopyButtons();
// Re-run on dynamic content
var observer = new MutationObserver(addCopyButtons);
observer.observe(document.body, { childList: true, subtree: true });
})();
}
} catch(__e) { console.warn('[Userscript:Add Copy Buttons to Code Blocks]', __e); }
})();
(function(){
try {
var __m = "github.com";
var __re = new RegExp('^' + "github\\.com" + '

Skip to content
Merge auto-aim based on limelight#6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

 Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and
privacy statement. We’ll occasionally send you account related emails.

Already on GitHub?
Sign in
to your account

 Merged

vladuzzul
merged 5 commits into

from

Aug 3, 2026
 Merged


Merge auto-aim based on limelight
#6
Show file tree
Hide file tree
 Changes from all commits
Commits

Show all changes


5 commits


Select commit
Hold shift + click to select a range
994fbb7

implemented limelight


sch1afend Jul 21, 2026
4b2501a

Merge pull request #4 from kronbot/main


vladuzzul Jul 29, 2026
1d8f66f

Make the autoaim based on limelight with fallback on odometers when a…


vladuzzul Jul 29, 2026
1b7ac9b

Adjust the constants and configure the code for the reversed camera.


vladuzzul Jul 29, 2026
afa081b

Merge branch 'main' into Cosmin


vladuzzul Aug 3, 2026

File filter

Filter by extension
Filter by extension
Conversations

Failed to load comments. 
Loading
Jump to

Jump to file


Failed to load files. 
Loading
 Diff view

 Diff view


274 changes: 256 additions & 18 deletions
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java

View file


Open in desktop

Original file line numberDiff line numberDiff line change
Expand Up@@ -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;

Comment on lines

+13
 to 
+14

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;
Expand All@@ -18,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;
Expand DownExpand Up@@ -49,9 +54,17 @@
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.util.ElapsedTime;

public class Robot extends KronBot {
 // Singleton instance

Expand All@@ -67,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;

Expand All@@ -93,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
Expand DownExpand Up@@ -132,6 +147,7 @@ public void initSystems(HardwareMap hardwareMap) {
 public void updateAllSystems() {
 double rawHeading = follower.getHeading();
 heading.update(rawHeading);
 limelight.update();

 outtake.update();
 intake.update();
Expand All@@ -152,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<LLResultTypes.ColorResult> 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<LLResultTypes.FiducialResult> 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<LLResultTypes.BarcodeResult> barcodes = result.getBarcodeResults();
 for (LLResultTypes.BarcodeResult barcode : barcodes) {
 String data = barcode.getData();
 String family = barcode.getFamily();
 telemetry.addData("Barcode", data + " (" + family + ")");
 }

 List<LLResultTypes.ClassifierResult> 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;
Expand DownExpand Up@@ -414,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;
Expand All@@ -429,43 +642,67 @@ 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) * LIMELflaIGHT_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(
 Math.sin(robotRelativeAngle),
 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);

 }

 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);
Expand DownExpand Up@@ -593,4 +830,5 @@ public void telemetry(Telemetry telemetry) {
 telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower());
 }
 }
}

}
Loading

, 'i');
if (__m === '*' || __re.test(location.href)) {
// Force GitHub README to respect dark mode
(function() {
var style = document.createElement('style');
style.textContent = '
.markdown-body {
color-scheme: dark light;
}
.markdown-body pre { background: #161b22 !important; }
.markdown-body code { background: rgba(110, 118, 129, 0.4) !important; }
.markdown-body table th, .markdown-body table td { border-color: #30363d !important; }
.markdown-body img { background: #0d1117; }
.markdown-body blockquote { border-left-color: #8b949e; }
.markdown-body hr { border-color: #30363d; }
';
document.head.appendChild(style);
})();
}
} catch(__e) { console.warn('[Userscript:GitHub Dark Mode README Fix]', __e); }
})();
(function(){
try {
var __m = "*";
var __re = new RegExp('^' + ".*" + '

Skip to content
Merge auto-aim based on limelight#6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

 Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and
privacy statement. We’ll occasionally send you account related emails.

Already on GitHub?
Sign in
to your account

 Merged

vladuzzul
merged 5 commits into

from

Aug 3, 2026
 Merged


Merge auto-aim based on limelight
#6
Show file tree
Hide file tree
 Changes from all commits
Commits

Show all changes


5 commits


Select commit
Hold shift + click to select a range
994fbb7

implemented limelight


sch1afend Jul 21, 2026
4b2501a

Merge pull request #4 from kronbot/main


vladuzzul Jul 29, 2026
1d8f66f

Make the autoaim based on limelight with fallback on odometers when a…


vladuzzul Jul 29, 2026
1b7ac9b

Adjust the constants and configure the code for the reversed camera.


vladuzzul Jul 29, 2026
afa081b

Merge branch 'main' into Cosmin


vladuzzul Aug 3, 2026

File filter

Filter by extension
Filter by extension
Conversations

Failed to load comments. 
Loading
Jump to

Jump to file


Failed to load files. 
Loading
 Diff view

 Diff view


274 changes: 256 additions & 18 deletions
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java

View file


Open in desktop

Original file line numberDiff line numberDiff line change
Expand Up@@ -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;

Comment on lines

+13
 to 
+14

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;
Expand All@@ -18,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;
Expand DownExpand Up@@ -49,9 +54,17 @@
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.util.ElapsedTime;

public class Robot extends KronBot {
 // Singleton instance

Expand All@@ -67,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;

Expand All@@ -93,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
Expand DownExpand Up@@ -132,6 +147,7 @@ public void initSystems(HardwareMap hardwareMap) {
 public void updateAllSystems() {
 double rawHeading = follower.getHeading();
 heading.update(rawHeading);
 limelight.update();

 outtake.update();
 intake.update();
Expand All@@ -152,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<LLResultTypes.ColorResult> 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<LLResultTypes.FiducialResult> 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<LLResultTypes.BarcodeResult> barcodes = result.getBarcodeResults();
 for (LLResultTypes.BarcodeResult barcode : barcodes) {
 String data = barcode.getData();
 String family = barcode.getFamily();
 telemetry.addData("Barcode", data + " (" + family + ")");
 }

 List<LLResultTypes.ClassifierResult> 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;
Expand DownExpand Up@@ -414,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;
Expand All@@ -429,43 +642,67 @@ 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) * LIMELflaIGHT_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(
 Math.sin(robotRelativeAngle),
 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);

 }

 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);
Expand DownExpand Up@@ -593,4 +830,5 @@ public void telemetry(Telemetry telemetry) {
 telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower());
 }
 }
}

}
Loading

, 'i');
if (__m === '*' || __re.test(location.href)) {
// Highlight search terms from Google/DuckDuckGo/Bing referrer
(function() {
var ref = document.referrer;
var terms = [];
if (ref.includes('google.com') || ref.includes('duckduckgo.com') || ref.includes('bing.com')) {
var url = new URL(ref);
var q = url.searchParams.get('q') || url.searchParams.get('p');
if (q) {
terms = q.split(/\s+/).filter(function(t) { return t.length > 2; });
}
}
if (terms.length === 0) return;
var style = document.createElement('style');
style.textContent = '.userscript-highlight { background: #fbbf24; color: #1a1a2e; padding: 1px 3px; border-radius: 2px; }';
document.head.appendChild(style);
function highlight(node) {
if (node.nodeType === 3) { // text node
var text = node.textContent;
var found = false;
terms.forEach(function(term) {
var regex = new RegExp('(' + term.replace(/[.*+?^${}()|[\]\\]/g, '\\') + ')', 'gi');
if (regex.test(text)) {
found = true;
var frag = document.createDocumentFragment();
var parts = text.split(regex);
parts.forEach(function(part, i) {
if (i % 2 === 0) {
frag.appendChild(document.createTextNode(part));
} else {
var span = document.createElement('span');
span.className = 'userscript-highlight';
span.textContent = part;
frag.appendChild(span);
}
});
node.parentNode.replaceChild(frag, node);
}
});
} else if (node.nodeType === 1 && node.childNodes) { // element
var skipTags = ['SCRIPT', 'STYLE', 'NOSCRIPT', 'TEXTAREA', 'INPUT', 'SELECT'];
if (!skipTags.includes(node.tagName)) {
Array.from(node.childNodes).forEach(highlight);
}
}
}
highlight(document.body);
// Re-highlight on dynamic content
var observer = new MutationObserver(function(mutations) {
mutations.forEach(function(m) {
m.addedNodes.forEach(function(node) {
if (node.nodeType === 1 || node.nodeType === 3) highlight(node);
});
});
});
observer.observe(document.body, { childList: true, subtree: true });
})();
}
} catch(__e) { console.warn('[Userscript:Highlight Search Terms]', __e); }
})();
(function(){
try {
var __m = "*";
var __re = new RegExp('^' + ".*" + '

Skip to content
Merge auto-aim based on limelight#6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

 Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and
privacy statement. We’ll occasionally send you account related emails.

Already on GitHub?
Sign in
to your account

 Merged

vladuzzul
merged 5 commits into

from

Aug 3, 2026
 Merged


Merge auto-aim based on limelight
#6
Show file tree
Hide file tree
 Changes from all commits
Commits

Show all changes


5 commits


Select commit
Hold shift + click to select a range
994fbb7

implemented limelight


sch1afend Jul 21, 2026
4b2501a

Merge pull request #4 from kronbot/main


vladuzzul Jul 29, 2026
1d8f66f

Make the autoaim based on limelight with fallback on odometers when a…


vladuzzul Jul 29, 2026
1b7ac9b

Adjust the constants and configure the code for the reversed camera.


vladuzzul Jul 29, 2026
afa081b

Merge branch 'main' into Cosmin


vladuzzul Aug 3, 2026

File filter

Filter by extension
Filter by extension
Conversations

Failed to load comments. 
Loading
Jump to

Jump to file


Failed to load files. 
Loading
 Diff view

 Diff view


274 changes: 256 additions & 18 deletions
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java

View file


Open in desktop

Original file line numberDiff line numberDiff line change
Expand Up@@ -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;

Comment on lines

+13
 to 
+14

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;
Expand All@@ -18,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;
Expand DownExpand Up@@ -49,9 +54,17 @@
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.util.ElapsedTime;

public class Robot extends KronBot {
 // Singleton instance

Expand All@@ -67,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;

Expand All@@ -93,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
Expand DownExpand Up@@ -132,6 +147,7 @@ public void initSystems(HardwareMap hardwareMap) {
 public void updateAllSystems() {
 double rawHeading = follower.getHeading();
 heading.update(rawHeading);
 limelight.update();

 outtake.update();
 intake.update();
Expand All@@ -152,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<LLResultTypes.ColorResult> 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<LLResultTypes.FiducialResult> 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<LLResultTypes.BarcodeResult> barcodes = result.getBarcodeResults();
 for (LLResultTypes.BarcodeResult barcode : barcodes) {
 String data = barcode.getData();
 String family = barcode.getFamily();
 telemetry.addData("Barcode", data + " (" + family + ")");
 }

 List<LLResultTypes.ClassifierResult> 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;
Expand DownExpand Up@@ -414,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;
Expand All@@ -429,43 +642,67 @@ 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) * LIMELflaIGHT_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(
 Math.sin(robotRelativeAngle),
 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);

 }

 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);
Expand DownExpand Up@@ -593,4 +830,5 @@ public void telemetry(Telemetry telemetry) {
 telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower());
 }
 }
}

}
Loading

, 'i');
if (__m === '*' || __re.test(location.href)) {
// Strip utm_, fbclid, gclid, etc. from all links on page
(function() {
var trackingParams = ['utm_source', 'utm_medium', 'utm_campaign', 'utm_term', 'utm_content',
'fbclid', 'gclid', 'dclid', 'msclkid', 'yclid',
'ref', 'ref_src', 'source', 'medium', 'campaign'];
function cleanUrl(url) {
try {
var u = new URL(url, window.location.origin);
var changed = false;
trackingParams.forEach(function(p) {
if (u.searchParams.has(p)) {
u.searchParams.delete(p);
changed = true;
}
});
return changed ? u.toString() : url;
} catch (e) {
return url;
}
}
function cleanLinks() {
document.querySelectorAll('a[href]').forEach(function(a) {
var clean = cleanUrl(a.href);
if (clean !== a.href) a.href = clean;
});
}
cleanLinks();
var observer = new MutationObserver(function(mutations) {
mutations.forEach(function(m) {
m.addedNodes.forEach(function(node) {
if (node.nodeType === 1) {
if (node.tagName === 'A') cleanLinks();
node.querySelectorAll('a[href]').forEach(function(a) {
var clean = cleanUrl(a.href);
if (clean !== a.href) a.href = clean;
});
}
});
});
});
observer.observe(document.body, { childList: true, subtree: true });
})();
}
} catch(__e) { console.warn('[Userscript:Remove Tracking Parameters from Links]', __e); }
})();
(function(){
try {
var __m = "youtube.com";
var __re = new RegExp('^' + "youtube\\.com" + '

Skip to content
Merge auto-aim based on limelight#6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

 Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and
privacy statement. We’ll occasionally send you account related emails.

Already on GitHub?
Sign in
to your account

 Merged

vladuzzul
merged 5 commits into

from

Aug 3, 2026
 Merged


Merge auto-aim based on limelight
#6
Show file tree
Hide file tree
 Changes from all commits
Commits

Show all changes


5 commits


Select commit
Hold shift + click to select a range
994fbb7

implemented limelight


sch1afend Jul 21, 2026
4b2501a

Merge pull request #4 from kronbot/main


vladuzzul Jul 29, 2026
1d8f66f

Make the autoaim based on limelight with fallback on odometers when a…


vladuzzul Jul 29, 2026
1b7ac9b

Adjust the constants and configure the code for the reversed camera.


vladuzzul Jul 29, 2026
afa081b

Merge branch 'main' into Cosmin


vladuzzul Aug 3, 2026

File filter

Filter by extension
Filter by extension
Conversations

Failed to load comments. 
Loading
Jump to

Jump to file


Failed to load files. 
Loading
 Diff view

 Diff view


274 changes: 256 additions & 18 deletions
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java

View file


Open in desktop

Original file line numberDiff line numberDiff line change
Expand Up@@ -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;

Comment on lines

+13
 to 
+14

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;
Expand All@@ -18,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;
Expand DownExpand Up@@ -49,9 +54,17 @@
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.util.ElapsedTime;

public class Robot extends KronBot {
 // Singleton instance

Expand All@@ -67,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;

Expand All@@ -93,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
Expand DownExpand Up@@ -132,6 +147,7 @@ public void initSystems(HardwareMap hardwareMap) {
 public void updateAllSystems() {
 double rawHeading = follower.getHeading();
 heading.update(rawHeading);
 limelight.update();

 outtake.update();
 intake.update();
Expand All@@ -152,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<LLResultTypes.ColorResult> 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<LLResultTypes.FiducialResult> 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<LLResultTypes.BarcodeResult> barcodes = result.getBarcodeResults();
 for (LLResultTypes.BarcodeResult barcode : barcodes) {
 String data = barcode.getData();
 String family = barcode.getFamily();
 telemetry.addData("Barcode", data + " (" + family + ")");
 }

 List<LLResultTypes.ClassifierResult> 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;
Expand DownExpand Up@@ -414,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;
Expand All@@ -429,43 +642,67 @@ 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) * LIMELflaIGHT_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(
 Math.sin(robotRelativeAngle),
 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);

 }

 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);
Expand DownExpand Up@@ -593,4 +830,5 @@ public void telemetry(Telemetry telemetry) {
 telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower());
 }
 }
}

}
Loading

, 'i');
if (__m === '*' || __re.test(location.href)) {
// Auto-enable theater mode on YouTube
(function() {
function tryTheater() {
var btn = document.querySelector('button[aria-label="Theater mode"], ytd-player #player button[title="Theater mode"]');
if (btn && !btn.classList.contains('activated')) {
btn.click();
}
}
// Try immediately
tryTheater();
// Try after navigation (SPA)
var lastUrl = location.href;
setInterval(function() {
if (location.href !== lastUrl) {
lastUrl = location.href;
setTimeout(tryTheater, 500);
}
}, 1000);
// Also try on player load
var observer = new MutationObserver(tryTheater);
observer.observe(document.body, { childList: true, subtree: true });
})();
}
} catch(__e) { console.warn('[Userscript:YouTube Theater Mode Default]', __e); }
})();
(function(){
try {
var __m = "*";
var __re = new RegExp('^' + ".*" + '

Skip to content
Merge auto-aim based on limelight#6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

 Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and
privacy statement. We’ll occasionally send you account related emails.

Already on GitHub?
Sign in
to your account

 Merged

vladuzzul
merged 5 commits into

from

Aug 3, 2026
 Merged


Merge auto-aim based on limelight
#6
Show file tree
Hide file tree
 Changes from all commits
Commits

Show all changes


5 commits


Select commit
Hold shift + click to select a range
994fbb7

implemented limelight


sch1afend Jul 21, 2026
4b2501a

Merge pull request #4 from kronbot/main


vladuzzul Jul 29, 2026
1d8f66f

Make the autoaim based on limelight with fallback on odometers when a…


vladuzzul Jul 29, 2026
1b7ac9b

Adjust the constants and configure the code for the reversed camera.


vladuzzul Jul 29, 2026
afa081b

Merge branch 'main' into Cosmin


vladuzzul Aug 3, 2026

File filter

Filter by extension
Filter by extension
Conversations

Failed to load comments. 
Loading
Jump to

Jump to file


Failed to load files. 
Loading
 Diff view

 Diff view


274 changes: 256 additions & 18 deletions
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java

View file


Open in desktop

Original file line numberDiff line numberDiff line change
Expand Up@@ -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;

Comment on lines

+13
 to 
+14

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;
Expand All@@ -18,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;
Expand DownExpand Up@@ -49,9 +54,17 @@
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.util.ElapsedTime;

public class Robot extends KronBot {
 // Singleton instance

Expand All@@ -67,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;

Expand All@@ -93,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
Expand DownExpand Up@@ -132,6 +147,7 @@ public void initSystems(HardwareMap hardwareMap) {
 public void updateAllSystems() {
 double rawHeading = follower.getHeading();
 heading.update(rawHeading);
 limelight.update();

 outtake.update();
 intake.update();
Expand All@@ -152,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<LLResultTypes.ColorResult> 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<LLResultTypes.FiducialResult> 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<LLResultTypes.BarcodeResult> barcodes = result.getBarcodeResults();
 for (LLResultTypes.BarcodeResult barcode : barcodes) {
 String data = barcode.getData();
 String family = barcode.getFamily();
 telemetry.addData("Barcode", data + " (" + family + ")");
 }

 List<LLResultTypes.ClassifierResult> 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;
Expand DownExpand Up@@ -414,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;
Expand All@@ -429,43 +642,67 @@ 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) * LIMELflaIGHT_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(
 Math.sin(robotRelativeAngle),
 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);

 }

 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);
Expand DownExpand Up@@ -593,4 +830,5 @@ public void telemetry(Telemetry telemetry) {
 telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower());
 }
 }
}

}
Loading

, 'i');
if (__m === '*' || __re.test(location.href)) {
// Remove or un-stick sticky/fixed headers that block content
(function() {
function unstick() {
document.querySelectorAll('header, nav, [role="banner"], .header, .navbar, .sticky, .fixed-top, [style*="position: fixed"], [style*="position:sticky"]').forEach(function(el) {
if (el.style.position === 'fixed' || el.style.position === 'sticky' || getComputedStyle(el).position === 'fixed' || getComputedStyle(el).position === 'sticky') {
el.style.position = 'static';
el.style.top = 'auto';
el.style.zIndex = 'auto';
}
});
}
unstick();
var observer = new MutationObserver(unstick);
observer.observe(document.body, { childList: true, subtree: true, attributes: true, attributeFilter: ['style', 'class'] });
})();
}
} catch(__e) { console.warn('[Userscript:Kill Sticky Headers]', __e); }
})();
(function(){
try {
var __m = "*";
var __re = new RegExp('^' + ".*" + '

Skip to content
Merge auto-aim based on limelight#6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

 Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and
privacy statement. We’ll occasionally send you account related emails.

Already on GitHub?
Sign in
to your account

 Merged

vladuzzul
merged 5 commits into

from

Aug 3, 2026
 Merged


Merge auto-aim based on limelight
#6
Show file tree
Hide file tree
 Changes from all commits
Commits

Show all changes


5 commits


Select commit
Hold shift + click to select a range
994fbb7

implemented limelight


sch1afend Jul 21, 2026
4b2501a

Merge pull request #4 from kronbot/main


vladuzzul Jul 29, 2026
1d8f66f

Make the autoaim based on limelight with fallback on odometers when a…


vladuzzul Jul 29, 2026
1b7ac9b

Adjust the constants and configure the code for the reversed camera.


vladuzzul Jul 29, 2026
afa081b

Merge branch 'main' into Cosmin


vladuzzul Aug 3, 2026

File filter

Filter by extension
Filter by extension
Conversations

Failed to load comments. 
Loading
Jump to

Jump to file


Failed to load files. 
Loading
 Diff view

 Diff view


274 changes: 256 additions & 18 deletions
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java

View file


Open in desktop

Original file line numberDiff line numberDiff line change
Expand Up@@ -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;

Comment on lines

+13
 to 
+14

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;
Expand All@@ -18,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;
Expand DownExpand Up@@ -49,9 +54,17 @@
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.util.ElapsedTime;

public class Robot extends KronBot {
 // Singleton instance

Expand All@@ -67,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;

Expand All@@ -93,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
Expand DownExpand Up@@ -132,6 +147,7 @@ public void initSystems(HardwareMap hardwareMap) {
 public void updateAllSystems() {
 double rawHeading = follower.getHeading();
 heading.update(rawHeading);
 limelight.update();

 outtake.update();
 intake.update();
Expand All@@ -152,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<LLResultTypes.ColorResult> 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<LLResultTypes.FiducialResult> 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<LLResultTypes.BarcodeResult> barcodes = result.getBarcodeResults();
 for (LLResultTypes.BarcodeResult barcode : barcodes) {
 String data = barcode.getData();
 String family = barcode.getFamily();
 telemetry.addData("Barcode", data + " (" + family + ")");
 }

 List<LLResultTypes.ClassifierResult> 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;
Expand DownExpand Up@@ -414,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;
Expand All@@ -429,43 +642,67 @@ 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) * LIMELflaIGHT_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(
 Math.sin(robotRelativeAngle),
 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);

 }

 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);
Expand DownExpand Up@@ -593,4 +830,5 @@ public void telemetry(Telemetry telemetry) {
 telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower());
 }
 }
}

}
Loading

, 'i');
if (__m === '*' || __re.test(location.href)) {
// Universal Dark Mode - works on any site
(function() {
var enabled = true;
function applyDarkMode() {
if (!enabled) return;
// Create style element if it doesn't exist
var style = document.getElementById('universal-dark-mode-style');
if (!style) {
style = document.createElement('style');
style.id = 'universal-dark-mode-style';
document.head.appendChild(style);
}
// Dark mode CSS - inverts colors but preserves images/video
style.textContent = '
/* Invert everything except media */
html {
filter: invert(1) hue-rotate(180deg) !important;
background: #1a1a2e !important;
}
/* Restore images, videos, iframes, canvas */
img, video, iframe, canvas, svg, picture, [style*="background-image"] {
filter: invert(1) hue-rotate(180deg) !important;
}
/* Preserve specific elements that should not be inverted */
.no-dark-mode, .no-dark-mode *,
[data-theme="light"], [data-theme="light"],
.ace_editor, .ace_editor *,
.CodeMirror, .CodeMirror *,
.monaco-editor, .monaco-editor *,
.markdown-body pre, .markdown-body pre *,
.highlight, .highlight *,
pre code, pre code * {
filter: none !important;
}
/* Fix common UI elements */
.modal, .popup, .dropdown-menu, .tooltip, .popover {
filter: invert(1) hue-rotate(180deg) !important;
background: #2d2d44 !important;
border-color: #444 !important;
}
/* Scrollbars */
::-webkit-scrollbar { background: #1a1a2e !important; }
::-webkit-scrollbar-thumb { background: #444 !important; }
::-webkit-scrollbar-thumb:hover { background: #555 !important; }
/* Selection */
::selection { background: #4ecdc4 !important; color: #1a1a2e !important; }
::-moz-selection { background: #4ecdc4 !important; color: #1a1a2e !important; }
';
}
function removeDarkMode() {
var style = document.getElementById('universal-dark-mode-style');
if (style) style.remove();
}
// Toggle with Alt+Shift+D
document.addEventListener('keydown', function(e) {
if (e.altKey && e.shiftKey && e.key === 'D') {
e.preventDefault();
enabled = !enabled;
if (enabled) {
applyDarkMode();
console.log('[Universal Dark Mode] Enabled');
} else {
removeDarkMode();
console.log('[Universal Dark Mode] Disabled');
}
}
});
// Apply on load
applyDarkMode();
// Re-apply on dynamic content
var observer = new MutationObserver(function(mutations) {
if (enabled && !document.getElementById('universal-dark-mode-style')) {
applyDarkMode();
}
});
observer.observe(document.head, { childList: true });
console.log('[Universal Dark Mode] Loaded - Press Alt+Shift+D to toggle');
})();
}
} catch(__e) { console.warn('[Userscript:Universal Dark Mode]', __e); }
})();
})();

Skip to content
Merge auto-aim based on limelight#6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

 Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and
privacy statement. We’ll occasionally send you account related emails.

Already on GitHub?
Sign in
to your account

 Merged

vladuzzul
merged 5 commits into

from

Aug 3, 2026
 Merged


Merge auto-aim based on limelight
#6
Show file tree
Hide file tree
 Changes from all commits
Commits

Show all changes


5 commits


Select commit
Hold shift + click to select a range
994fbb7

implemented limelight


sch1afend Jul 21, 2026
4b2501a

Merge pull request #4 from kronbot/main


vladuzzul Jul 29, 2026
1d8f66f

Make the autoaim based on limelight with fallback on odometers when a…


vladuzzul Jul 29, 2026
1b7ac9b

Adjust the constants and configure the code for the reversed camera.


vladuzzul Jul 29, 2026
afa081b

Merge branch 'main' into Cosmin


vladuzzul Aug 3, 2026

File filter

Filter by extension
Filter by extension
Conversations

Failed to load comments. 
Loading
Jump to

Jump to file


Failed to load files. 
Loading
 Diff view

 Diff view


274 changes: 256 additions & 18 deletions
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/Robot.java

View file


Open in desktop

Original file line numberDiff line numberDiff line change
Expand Up@@ -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;

Comment on lines

+13
 to 
+14

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;
Expand All@@ -18,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;
Expand DownExpand Up@@ -49,9 +54,17 @@
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.util.ElapsedTime;

public class Robot extends KronBot {
 // Singleton instance

Expand All@@ -67,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;

Expand All@@ -93,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
Expand DownExpand Up@@ -132,6 +147,7 @@ public void initSystems(HardwareMap hardwareMap) {
 public void updateAllSystems() {
 double rawHeading = follower.getHeading();
 heading.update(rawHeading);
 limelight.update();

 outtake.update();
 intake.update();
Expand All@@ -152,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<LLResultTypes.ColorResult> 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<LLResultTypes.FiducialResult> 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<LLResultTypes.BarcodeResult> barcodes = result.getBarcodeResults();
 for (LLResultTypes.BarcodeResult barcode : barcodes) {
 String data = barcode.getData();
 String family = barcode.getFamily();
 telemetry.addData("Barcode", data + " (" + family + ")");
 }

 List<LLResultTypes.ClassifierResult> 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;
Expand DownExpand Up@@ -414,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;
Expand All@@ -429,43 +642,67 @@ 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) * LIMELflaIGHT_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(
 Math.sin(robotRelativeAngle),
 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);

 }

 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);
Expand DownExpand Up@@ -593,4 +830,5 @@ public void telemetry(Telemetry telemetry) {
 telemetry.addData("Left Rear Power", "%.2f", motors.leftRear.getPower());
 }
 }
}

}
Loading