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

continous ll tracking, TEST

Browse Source
This commit is contained in:
abhiramtx
2026-01-19 10:38:34 -06:00
parent d42af20447
commit f1d4bb9d24
1 changed files with 78 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -48,6 +48,8 @@ public class TeleopV3 extends LinearOpMode {
public static double spindexPos = spindexer_intakePos1; public static double spindexPos = spindexer_intakePos1;
public static double spinPow = 0.09; public static double spinPow = 0.09;
public static double bMult = 1, bDiv = 2200; public static double bMult = 1, bDiv = 2200;
public static double limelightKp = 0.001; // Proportional gain for limelight auto-aim
public static double limelightDeadband = 0.5; // Ignore tx values smaller than this
public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
public static boolean manualTurret = true; public static boolean manualTurret = true;
public double vel = 3000; public double vel = 3000;
@@ -431,46 +433,100 @@ public class TeleopV3 extends LinearOpMode {
//TODO: test the camera teleop code //TODO: test the camera teleop code
// TODO: TEST THIS CODE
TELE.addData("posS2", pos); TELE.addData("posS2", pos);
if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving
double bearing;
LLResult result = robot.limelight.getLatestResult(); LLResult result = robot.limelight.getLatestResult();
if (result != null) { boolean limelightActive = false;
if (result.isValid()) {
bearing = result.getTx() * bMult;
double bearingCorrection = bearing / bDiv; double turretMin = 0.13;
double turretMax = 0.83;
error += bearingCorrection; if (result != null && result.isValid()) {
double tx = result.getTx();
double ty = result.getTy();
if (Math.abs(tx) > limelightDeadband) {
limelightActive = true;
overrideTurr = true;
double currentTurretPos = servo.getTurrPos();
// + tx means tag is right, so rotate right
double adjustment = -tx * limelightKp;
// calculate new position
double newTurretPos = currentTurretPos + adjustment;
if (newTurretPos < turretMin) {
double forwardDist = turretMin - newTurretPos;
double backwardDist = (currentTurretPos - turretMin) + (turretMax - newTurretPos);
// check path distance
if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) {
newTurretPos = turretMax - (turretMin - newTurretPos);
} else {
newTurretPos = turretMin;
}
} else if (newTurretPos > turretMax) {
double forwardDist = newTurretPos - turretMax;
double backwardDist = (turretMax - currentTurretPos) + (newTurretPos - turretMin);
if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) {
newTurretPos = turretMin + (newTurretPos - turretMax);
} else {
newTurretPos = turretMax;
}
}
// Final clamp
if (newTurretPos < turretMin) {
newTurretPos = turretMin;
} else if (newTurretPos > turretMax) {
newTurretPos = turretMax;
}
pos = newTurretPos;
turretPos = pos;
camTicker++; camTicker++;
TELE.addData("tx", bearingCorrection); TELE.addData("tx", tx);
TELE.addData("ty", result.getTy()); TELE.addData("ty", ty);
TELE.addData("limelightAdjustment", adjustment);
TELE.addData("limelightActive", true);
} else {
limelightActive = true;
overrideTurr = true;
TELE.addData("tx", tx);
TELE.addData("ty", ty);
TELE.addData("limelightActive", true);
TELE.addData("limelightStatus", "Centered");
} }
} } else {
if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) {
TELE.addData("limelightActive", false);
TELE.addData("limelightStatus", "No target");
} else { } else {
camTicker = 0; camTicker = 0;
overrideTurr = false; overrideTurr = false;
limelightActive = false;
}
} }
if (!overrideTurr) { if (!limelightActive && !overrideTurr) {
turretPos = pos; turretPos = pos;
} }
TELE.addData("posS3", turretPos); TELE.addData("posS3", turretPos);
if (manualTurret) { if (manualTurret && !limelightActive) {
pos = turrDefault + (manualOffset / 100) + error; pos = turrDefault + (manualOffset / 100) + error;
} }
if (!overrideTurr) { if (!overrideTurr && !limelightActive) {
turretPos = pos; turretPos = pos;
} }
if (Math.abs(gamepad2.left_stick_x)>0.2) { if (Math.abs(gamepad2.left_stick_x)>0.2 && !limelightActive) {
manualOffset += 1.35 * gamepad2.left_stick_x; manualOffset += 1.35 * gamepad2.left_stick_x;
} }