diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index 9ffa9f7..9e28ed0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -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]); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index 38ce24c..bfb87d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index ba2fc6a..b6a04ad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -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); - } else { - // When centered, lock in the learned offset - permanentOffset = offset; - offset = 0.0; - } + 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 { + // 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; + // Apply accumulated 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); }