KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Merge with LimelightTesting.

Browse Source
This commit is contained in:
Matt9150
2026-01-25 11:39:26 -06:00
parent 7ffc51f60a fefeeb1f2e
commit 8dc03adfd3
6 changed files with 104 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -19,6 +19,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -26,7 +27,6 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
@@ -396,8 +396,6 @@ public class TeleopV3 extends LinearOpMode {
turret.trackGoal(deltaPose);
//VELOCITY AUTOMATIC
if (targetingVel) {
vel = targetingSettings.flywheelRPM;
@@ -634,7 +632,6 @@ public class TeleopV3 extends LinearOpMode {
}
}
//
// if (shootAll) {
//
@@ -806,7 +803,6 @@ public class TeleopV3 extends LinearOpMode {
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
//
TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
@@ -833,7 +829,7 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("shootall commanded", shootAll);
// Targeting Debug
TELE.addData("robotX", robotX);
TELE.addData( "robotY", robotY);
TELE.addData("robotY", robotY);
TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData( "robotInchesY", targeting.robotInchesY);
TELE.addData("Targeting Interpolate", turretInterpolate);

72
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java
View File

@@ -4,32 +4,35 @@ import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import java.util.List;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Turret;
@Config
@TeleOp
//TODO: fix to get the apriltag that it is reading
public class LimelightTest extends LinearOpMode {
MultipleTelemetry TELE;
Turret turret;
Robot robot;
public static int pipeline = 0; //0 is for test; 1 for obelisk; 2 is for blue track; 3 is for red track
public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
public static boolean turretMode = false;
public static double turretPos = 0.501;
@Override
public void runOpMode() throws InterruptedException {
Limelight3A limelight = hardwareMap.get(Limelight3A.class, "limelight");
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
limelight.pipelineSwitch(pipeline);
robot = new Robot(hardwareMap);
turret = new Turret(robot, TELE, robot.limelight);
robot.limelight.pipelineSwitch(pipeline);
waitForStart();
if (isStopRequested()) return;
limelight.start();
while (opModeIsActive()){
if (mode == 0){
limelight.pipelineSwitch(pipeline);
LLResult result = limelight.getLatestResult();
robot.limelight.pipelineSwitch(pipeline);
LLResult result = robot.limelight.getLatestResult();
if (result != null) {
if (result.isValid()) {
TELE.addData("tx", result.getTx());
@@ -38,40 +41,29 @@ public class LimelightTest extends LinearOpMode {
}
}
} else if (mode == 1){
limelight.pipelineSwitch(1);
LLResult result = limelight.getLatestResult();
if (result != null && result.isValid()) {
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
for (LLResultTypes.FiducialResult fiducial : fiducials) {
int id = fiducial.getFiducialId();
TELE.addData("ID", id);
TELE.update();
}
}
} else if (mode == 2){
limelight.pipelineSwitch(4);
LLResult result = limelight.getLatestResult();
if (result != null) {
if (result.isValid()) {
TELE.addData("tx", result.getTx());
TELE.addData("ty", result.getTy());
TELE.update();
}
}
} else if (mode == 3){
limelight.pipelineSwitch(5);
LLResult result = limelight.getLatestResult();
if (result != null) {
if (result.isValid()) {
TELE.addData("tx", result.getTx());
TELE.addData("ty", result.getTy());
TELE.update();
}
}
int obeliskID = turret.detectObelisk();
TELE.addData("Limelight ID", obeliskID);
TELE.update();
} else if (mode == 2 || mode == 3){ // Use redAlliance variable to switch between red and blue
double tx = turret.getBearing();
double ty = turret.getTy();
double x = turret.getLimelightX();
double y = turret.getLimelightY();
TELE.addData("tx", tx);
TELE.addData("ty", ty);
TELE.addData("x", x);
TELE.addData("y", y);
TELE.update();
} else {
limelight.pipelineSwitch(0);
robot.limelight.pipelineSwitch(0);
}
if (turretMode){
if (turretPos != 0.501){
turret.manualSetTurret(turretPos);
}
}
}
}
}

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java
View File

@@ -8,15 +8,12 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Turret;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@Autonomous
@Config
public class TurretTest extends LinearOpMode {
public static boolean zeroTurr = false;
@Override
public void runOpMode() throws InterruptedException {
@@ -26,11 +23,9 @@ public class TurretTest extends LinearOpMode {
telemetry, FtcDashboard.getInstance().getTelemetry()
);
Turret turret = new Turret(robot, TELE, robot.limelight);
waitForStart();
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0));
while(opModeIsActive()){
@@ -38,16 +33,16 @@ public class TurretTest extends LinearOpMode {
drive.updatePoseEstimate();
turret.trackGoal(drive.localizer.getPose());
TELE.addData("tpos", turret.getTurrPos());
TELE.addData("Limelight tx", turret.getBearing());
TELE.addData("Limelight ty", turret.getTy());
TELE.addData("Limelight X", turret.getLimelightX());
TELE.addData("Limelight Y", turret.getLimelightY());
if(zeroTurr){
turret.zeroTurretEncoder();
}
TELE.update();
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -21,7 +21,7 @@ public class Robot {
//Initialize Public Components
public static boolean usingLimelight = true;
public static boolean usingCamera = false;
public static boolean usingCamera = true;
public DcMotorEx frontLeft;
public DcMotorEx frontRight;
public DcMotorEx backLeft;

140
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -1,3 +1,4 @@
package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
@@ -5,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
@@ -16,52 +18,50 @@ import java.util.List;
@Config
public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 0.00011264432;
public static double turret180Range = 0.4;
public static double turrDefault = 0.4;
// TODO: tune these values for limelight
// At the top with other static variables:
public static double kP = 0.015; // Proportional gain - tune this first
public static double kI = 0.0005; // Integral gain - add slowly if needed
public static double kD = 0.002; // Derivative gain - helps prevent overshoot
public static double kF = 0.002; // Derivative gain - helps prevent overshoot
public static double maxOffset = 10; // degrees - safety limit
// Add these as instance variables:
private double lastTagBearing = 0.0;
private double offsetIntegral = 0.0;
public static double cameraBearingEqual = 1;
public static double errorLearningRate = -0.15;
public static double turrMin = 0.2;
public static double turrMax = 0.8;
public static double mult = 0.0;
public static double staticOffsetRate = -0.15;
public static double deltaAngleThreshold = 0.02;
public static double angleMultiplier = 0.0;
public static double fastSeekThreshold = 5.0; // Switch to medium mode below this
public static double mediumSeekThreshold = 2.0; // Switch to fine mode below this
public static double fastCorrectionGain = 0.75; // Correction gain for large errors
public static double mediumCorrectionGain = 0.4; // Correction gain for medium errors
public static double fineCorrectionGain = 0.2; // Correction gain for small errors
public static double maxOffsetChangePerCycle = 0.3; // Max offset change per cycle (degrees)
public static double finalInterpolation = 0.1; // Final position interpolation factor
// TODO: tune these values for limelight
public static double clampTolerance = 0.03;
private boolean lockOffset = false;
Robot robot;
MultipleTelemetry TELE;
Limelight3A webcam;
private int obeliskID = 0;
private double offset = 0.0;
private PIDFController controller = new PIDFController(kP, kI, kD, kF);
double tx = 0.0;
double ty = 0.0;
double limelightPosX = 0.0;
double limelightPosY = 0.0;
private boolean lockOffset = false;
private int obeliskID = 0;
private double offset = 0.0;
public static double clampTolerance = 0.03;
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
this.TELE = tele;
this.robot = rob;
this.webcam = cam;
webcam.start();
if (redAlliance) {
if (redAlliance){
webcam.pipelineSwitch(3);
} else {
webcam.pipelineSwitch(2);
@@ -78,17 +78,17 @@ public class Turret {
}
public void manualSetTurret(double pos) {
public void manualSetTurret(double pos){
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
robot.turr2.setPosition(1-pos);
}
public boolean turretEqual(double pos) {
return Math.abs(pos - this.getTurrPos()) < turretTolerance;
}
private void limelightRead() { // only for tracking purposes, not general reads
if (redAlliance) {
private void limelightRead(){ // only for tracking purposes, not general reads
if (redAlliance){
webcam.pipelineSwitch(3);
} else {
webcam.pipelineSwitch(2);
@@ -101,7 +101,7 @@ public class Turret {
ty = result.getTy();
// MegaTag1 code for receiving position
Pose3D botpose = result.getBotpose();
if (botpose != null) {
if (botpose != null){
limelightPosX = botpose.getPosition().x;
limelightPosY = botpose.getPosition().y;
}
@@ -116,17 +116,17 @@ public class Turret {
return tx;
}
public double getTy() {
public double getTy(){
limelightRead();
return ty;
}
public double getLimelightX() {
public double getLimelightX(){
limelightRead();
return limelightPosX;
}
public double getLimelightY() {
public double getLimelightY(){
limelightRead();
return limelightPosY;
}
@@ -158,9 +158,9 @@ public class Turret {
/*
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
*/
public void trackGoal(Pose2d deltaPos) {
controller.setPIDF(kP, kI, kD, kF);
/* ---------------- FIELD → TURRET GEOMETRY ---------------- */
// Angle from robot to goal in robot frame
@@ -173,75 +173,55 @@ public class Turret {
// Turret angle needed relative to robot
double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
turretAngleDeg = -turretAngleDeg;
// Normalize to [-180, 180]
while (turretAngleDeg > 180) turretAngleDeg -= 360;
while (turretAngleDeg < -180) turretAngleDeg += 360;
/* ---------------- LIMELIGHT VISION CORRECTION ---------------- */
/* ---------------- APRILTAG CORRECTION ---------------- */
//
double tagBearingDeg = getBearing(); // + = target is to the left
boolean hasValidTarget = (tagBearingDeg != 1000.0);
// Apply persistent offset from previous corrections
turretAngleDeg += offset;
// Active correction if we see the target
if (hasValidTarget && !lockOffset) {
double bearingError = Math.abs(tagBearingDeg);
/* ---------------- ANGLE → SERVO ---------------- */
if (bearingError > cameraBearingEqual) {
// Dual-mode correction: fast when far, gentle when close
double correctionGain;
if (bearingError > fastSeekThreshold) {
correctionGain = fastCorrectionGain;
} else if (bearingError > mediumSeekThreshold) {
correctionGain = mediumCorrectionGain;
} else {
correctionGain = fineCorrectionGain;
}
double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
// Clamp to servo range
double currentEncoderPos = this.getTurrPos();
if (!turretEqual(turretPos)) {
double diff = turretPos - currentEncoderPos;
turretPos = turretPos + diff * mult;
}
if (currentEncoderPos < (turrMin + clampTolerance) || currentEncoderPos > (turrMax - clampTolerance)) {
// Clamp to servo range
turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
} else { // TODO: add so it only adds error when standstill
if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual && !lockOffset) {
// PID-based offset correction for faster, smoother tracking
// Proportional: respond to current error
offset = -controller.calculate(tagBearingDeg);
// Immediate correction to turret angle
turretAngleDeg -= tagBearingDeg * correctionGain;
// Learn offset slowly for persistent calibration
double offsetChange = -tagBearingDeg * errorLearningRate;
// Rate limit to prevent oscillation
offsetChange = Math.max(-maxOffsetChangePerCycle, Math.min(offsetChange, maxOffsetChangePerCycle));
offset += offsetChange;
TELE.addData("Correction Mode", bearingError > fastSeekThreshold ? "FAST" :
bearingError > mediumSeekThreshold ? "MEDIUM" : "FINE");
}
}
/* ---------------- ANGLE → SERVO POSITION ---------------- */
double targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
// Clamp to physical servo limits
targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax));
// Interpolate towards target position
double currentPos = getTurrPos();
double turretPos = currentPos + (targetTurretPos - currentPos) * finalInterpolation;
// Set servo positions
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1.0 - turretPos);
/* ---------------- TELEMETRY ---------------- */
TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg);
TELE.addData("Target Pos", "%.3f", targetTurretPos);
TELE.addData("Current Pos", "%.3f", currentPos);
TELE.addData("Commanded Pos", "%.3f", turretPos);
TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET");
TELE.addData("Learned Offset", "%.2f", offset);
TELE.addData("Turret Angle", turretAngleDeg);
TELE.addData("Bearing", tagBearingDeg);
TELE.addData("Offset", offset);
}
}