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

Add a PID to Limelight tracking bringing in track function from Abyss. Simplify spindexer due to errors with advanced code. Start adding Interpolation to Targeting (commented out for now).

Browse Source
This commit is contained in:
Matt9150
2026-01-27 00:58:15 -06:00
parent e665ddf032
commit 04372ec410
3 changed files with 168 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -305,33 +305,32 @@ public class Spindexer {
// Find Next Open Position and start movement
double currentSpindexerPos = servos.getSpinPos();
double commandedtravelDistance = 2.0;
double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos);
if (ballPositions[0].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
//double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos);
//if (ballPositions[0].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
if (ballPositions[0].isEmpty) {
// Position 1
commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING;
commandedtravelDistance = proposedTravelDistance;
}
proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
//proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
//if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
if (ballPositions[1].isEmpty) {
// Position 2
commandedIntakePosition = 1;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING;
commandedtravelDistance = proposedTravelDistance;
}
proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
if (ballPositions[2].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
//proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
if (ballPositions[2].isEmpty) {
// Position 3
commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING;
commandedtravelDistance = proposedTravelDistance;
}
if (currentIntakeState != Spindexer.IntakeState.MOVING) {
// Full
commandedIntakePosition = bestFitMotif();
//commandedIntakePosition = bestFitMotif();
currentIntakeState = Spindexer.IntakeState.FULL;
}
moveSpindexerToPos(intakePositions[commandedIntakePosition]);

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

@@ -14,6 +14,8 @@ public class Targeting {
double unitConversionFactor = 0.95;
int tileSize = 24; //inches
public final int TILE_UPPER_QUARTILE = 18;
public final int TILE_LOWER_QUARTILE = 6;
public double robotInchesX, robotInchesY = 0.0;
@@ -100,6 +102,60 @@ public class Targeting {
int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
int remX = Math.floorMod((int)robotInchesX, tileSize);
int remY = Math.floorMod((int)robotInchesX, tileSize);
// Determine if we need to interpolate based on tile position.
// if near upper or lower quarter or tile interpolate with next tile.
int x1 = 0;
int y1 = 0;
// interpolate = false;
// if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// (robotGridX < 5) && (robotGridY <5)) {
// // +X, +Y
// interpolate = true;
// x1 = robotGridX + 1;
// y1 = robotGridY + 1;
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// (robotGridX > 0) && (robotGridY > 0)) {
// // -X, -Y
// interpolate = true;
// x1 = robotGridX - 1;
// y1 = robotGridY - 1;
// } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// (robotGridX < 5) && (robotGridY > 0)) {
// // +X, -Y
// interpolate = true;
// x1 = robotGridX + 1;
// y1 = robotGridY - 1;
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// (robotGridX > 0) && (robotGridY < 5)) {
// // -X, +Y
// interpolate = true;
// x1 = robotGridX - 1;
// y1 = robotGridY + 1;
// } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) {
// // -X, Y
// interpolate = true;
// x1 = robotGridX - 1;
// y1 = robotGridY;
// } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) {
// // X, -Y
// interpolate = true;
// x1 = robotGridX;
// y1 = robotGridY - 1;
// } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) {
// // +X, Y
// interpolate = true;
// x1 = robotGridX + 1;
// y1 = robotGridY;
// } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) {
// // X, +Y
// interpolate = true;
// x1 = robotGridX;
// y1 = robotGridY + 1;
// }
//clamp
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
@@ -115,9 +171,9 @@ public class Targeting {
// bilinear interpolation
int x0 = robotGridX;
int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1);
int y0 = gridY;
int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
//int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1);
int y0 = robotGridY;
//int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
double x = (robotInchesX - (x0 * tileSize)) / tileSize;
double y = (robotInchesY - (y0 * tileSize)) / tileSize;

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

@@ -2,9 +2,12 @@ package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static java.lang.Math.abs;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDController;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
@@ -43,9 +46,17 @@ public class Turret {
private int obeliskID = 0;
private double offset = 0.0;
private double currentTrackOffset = 0.0;
private int currentTrackCount = 0;
private double permanentOffset = 0.0;
LLResult result;
private PIDController bearingPID;
public static double B_PID_P = 0.3, B_PID_I = 0.0, B_PID_D = 0.05;
boolean bearingAligned = false;
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
this.TELE = tele;
this.robot = rob;
@@ -56,6 +67,7 @@ public class Turret {
} else {
webcam.pipelineSwitch(2);
}
bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D);
}
public void zeroTurretEncoder() {
@@ -79,12 +91,12 @@ public class Turret {
private void limelightRead() { // only for tracking purposes, not general reads
if (redAlliance) {
webcam.pipelineSwitch(3);
webcam.pipelineSwitch(4);
} else {
webcam.pipelineSwitch(2);
}
LLResult result = webcam.getLatestResult();
result = webcam.getLatestResult();
if (result != null) {
if (result.isValid()) {
tx = result.getTx();
@@ -149,6 +161,47 @@ public class Turret {
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
*/
private double bearingAlign (LLResult llResult) {
double bearingOffset = 0.0;
double targetTx = llResult.getTx(); // How far left or right the target is (degrees)
final double MIN_OFFSET_POWER = 0.15;
final double TARGET_POSITION_TOLERANCE = 1.0;
// LL has 54.5 degree total Horizontal FOV; very edges are not useful.
final double HORIZONTAL_FOV_RANGE = 26.0; // Total usable horizontal degrees from center +/-
final double DRIVE_POWER_REDUCTION = 2.0;
if (abs(targetTx) < TARGET_POSITION_TOLERANCE) {
bearingAligned = true;
} else {
bearingAligned = false;
}
// Only with valid data and if too far off target
if (llResult.isValid() && !bearingAligned)
{
// Adjust Robot Speed based on how far the target is located
// Only drive at half speed max
// switched to PID but original formula left for reference in comments
//drivePower = targetTx/HORIZONTAL_FOV_RANGE / DRIVE_POWER_REDUCTION;
bearingOffset = -(bearingPID.calculate(targetTx, 0.0));
// // Make sure we have enough power to actually drive the wheels
// if (abs(bearingOffset) < MIN_OFFSET_POWER) {
// if (bearingOffset > 0.0) {
// bearingOffset = MIN_OFFSET_POWER;
// } else {
// bearingOffset = -MIN_OFFSET_POWER;
// }
//
// }
}
return bearingOffset;
}
public void trackGoal(Pose2d deltaPos) {
/* ---------------- FIELD → TURRET GEOMETRY ---------------- */
@@ -171,41 +224,53 @@ public class Turret {
/* ---------------- LIMELIGHT VISION CORRECTION ---------------- */
double tagBearingDeg = getBearing(); // + = target is to the left
boolean hasValidTarget = (tagBearingDeg != 1000.0);
// Update local limelight results
//double tagBearingDeg = getBearing(); // + = target is to the left
//boolean hasValidTarget = (tagBearingDeg != 1000.0);
turretAngleDeg += permanentOffset;
limelightRead();
// Active correction if we see the target
if (hasValidTarget && !lockOffset) {
double bearingError = Math.abs(tagBearingDeg);
if (bearingError > cameraBearingEqual) {
// Apply sqrt scaling to reduce aggressive corrections at large errors
double filteredBearing = Math.signum(tagBearingDeg) * Math.sqrt(Math.abs(tagBearingDeg));
// Calculate correction
double offsetChange = visionCorrectionGain * filteredBearing;
// Limit rate of change to prevent jumps
offsetChange = Math.max(-maxOffsetChangePerCycle,
Math.min(maxOffsetChangePerCycle, offsetChange));
// Accumulate the correction
offset += offsetChange;
TELE.addData("Bearing Error", tagBearingDeg);
TELE.addData("Offset Change", offsetChange);
TELE.addData("Total Offset", offset);
if (result.isValid() && !lockOffset) {
currentTrackOffset += bearingAlign(result);
currentTrackCount++;
// double bearingError = Math.abs(tagBearingDeg);
//
// if (bearingError > cameraBearingEqual) {
// // Apply sqrt scaling to reduce aggressive corrections at large errors
// double filteredBearing = Math.signum(tagBearingDeg) * Math.sqrt(Math.abs(tagBearingDeg));
//
// // Calculate correction
// double offsetChange = visionCorrectionGain * filteredBearing;
//
// // Limit rate of change to prevent jumps
// offsetChange = Math.max(-maxOffsetChangePerCycle,
// Math.min(maxOffsetChangePerCycle, offsetChange));
//
// // Accumulate the correction
// offset += offsetChange;
//
// TELE.addData("Bearing Error", tagBearingDeg);
// TELE.addData("Offset Change", offsetChange);
// TELE.addData("Total Offset", offset);
// } else {
// // When centered, lock in the learned offset
// permanentOffset = offset;
// offset = 0.0;
// }
} else {
// When centered, lock in the learned offset
permanentOffset = offset;
offset = 0.0;
}
// only store perma update after 20+ successful tracks
// this did not work good in testing; only current works best so far.
// if (currentTrackCount > 20) {
// offset = currentTrackOffset;
// }
currentTrackOffset = 0.0;
currentTrackCount = 0;
}
// Apply accumulated offset
turretAngleDeg += offset;
turretAngleDeg += offset + currentTrackOffset;
/* ---------------- ANGLE → SERVO POSITION ---------------- */
@@ -236,7 +301,10 @@ public class Turret {
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("LL Valid", result.isValid());
TELE.addData("LL getTx", result.getTx());
TELE.addData("LL Offset", offset);
//TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET");
TELE.addData("Learned Offset", "%.2f", offset);
}