From f1d4bb9d24dcae75b1e33ac3d717b6f6a3c52412 Mon Sep 17 00:00:00 2001 From: abhiramtx Date: Mon, 19 Jan 2026 10:38:34 -0600 Subject: [PATCH 1/6] continous ll tracking, TEST --- .../ftc/teamcode/teleop/TeleopV3.java | 100 ++++++++++++++---- 1 file changed, 78 insertions(+), 22 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 37bc223..e003d7b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -48,6 +48,8 @@ public class TeleopV3 extends LinearOpMode { public static double spindexPos = spindexer_intakePos1; public static double spinPow = 0.09; 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 boolean manualTurret = true; public double vel = 3000; @@ -431,46 +433,100 @@ public class TeleopV3 extends LinearOpMode { //TODO: test the camera teleop code + // TODO: TEST THIS CODE + 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(); - if (result != null) { - if (result.isValid()) { - bearing = result.getTx() * bMult; - - double bearingCorrection = bearing / bDiv; - - error += bearingCorrection; - - camTicker++; - TELE.addData("tx", bearingCorrection); - TELE.addData("ty", result.getTy()); + LLResult result = robot.limelight.getLatestResult(); + boolean limelightActive = false; + + double turretMin = 0.13; + double turretMax = 0.83; + + 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++; + TELE.addData("tx", tx); + 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 { - camTicker = 0; - overrideTurr = false; + 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 { + camTicker = 0; + overrideTurr = false; + limelightActive = false; + } } - if (!overrideTurr) { + if (!limelightActive && !overrideTurr) { turretPos = pos; } TELE.addData("posS3", turretPos); - if (manualTurret) { + if (manualTurret && !limelightActive) { pos = turrDefault + (manualOffset / 100) + error; } - if (!overrideTurr) { + if (!overrideTurr && !limelightActive) { 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; } From 50212015e3d2ddef28d615276ae4b2535eda11a7 Mon Sep 17 00:00:00 2001 From: abhiramtx Date: Wed, 21 Jan 2026 19:04:30 -0600 Subject: [PATCH 2/6] trackGoal expected robot centric view, but was fed a field centric view. simple trig to use a deltaPos instead of just pos --- .../ftc/teamcode/tests/TurretTest.java | 27 +++- .../ftc/teamcode/utils/Turret.java | 128 ++++++------------ 2 files changed, 69 insertions(+), 86 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 9c8a1cd..7401aaa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -1,9 +1,12 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.Poses.goalPose; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -35,7 +38,29 @@ public class TurretTest extends LinearOpMode { while(opModeIsActive()){ drive.updatePoseEstimate(); - turret.trackGoal(drive.localizer.getPose()); + Pose2d robotPose = drive.localizer.getPose(); + + double dx = goalPose.position.x - robotPose.position.x; + double dy = goalPose.position.y - robotPose.position.y; + + double heading = robotPose.heading.toDouble(); + + // field vector -> robot frame... avoids double calculation + double relX = dx * Math.cos(-heading) - dy * Math.sin(-heading); + double relY = dx * Math.sin(-heading) + dy * Math.cos(-heading); + + Pose2d deltaPos = new Pose2d( + new Vector2d(relX, relY), + robotPose.heading + ); + + + turret.trackGoal(deltaPos); + + TELE.addData("Robot Pose", robotPose); + TELE.addData("Goal Pose", goalPose); + TELE.addData("Delta Pos", deltaPos); + TELE.update(); } } 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 a32b58e..b73a733 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 @@ -5,79 +5,61 @@ 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.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @Config public class Turret { - public static double turretTolerance = 0.02; - public static double turrPosScalar = 1.009; - public static double turret180Range = 0.6; public static double turrDefault = 0.4; - public static double cameraBearingEqual = 1.5; - public static double errorLearningRate = 2; + public static double turretRange = 0.6; public static double turrMin = 0.2; public static double turrMax = 0.8; - Robot robot; - MultipleTelemetry TELE; - AprilTagWebcam webcam; + + public static double turretTolerance = 0.02; + + public static double cameraBearingEqual = 1.5; + public static double errorLearningRate = 0.02; // must be low + public static double maxOffsetDeg = 30.0; + + private final Robot robot; + private final MultipleTelemetry TELE; + private final AprilTagWebcam webcam; + private int obeliskID = 0; - private double turrPos = 0.0; - private double offset = 0.0; - private double bearing = 0.0; + private double offsetDeg = 0.0; public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) { - this.TELE = tele; this.robot = rob; + this.TELE = tele; this.webcam = cam; } - public double getTurrPos() { - return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3); - + public double getTurretPos() { + return robot.turr1Pos.getVoltage() / 3.3; } - public void manualSetTurret(double pos){ + public void manualSetTurret(double pos) { + pos = Range.clip(pos, turrMin, turrMax); robot.turr1.setPosition(pos); - robot.turr2.setPosition(1-pos); + robot.turr2.setPosition(1.0 - pos); } - public boolean turretEqual(double pos) { - return Math.abs(pos - this.getTurrPos()) < turretTolerance; + public boolean turretAt(double pos) { + return Math.abs(pos - getTurretPos()) < turretTolerance; } - public double getBearing() { - if (redAlliance) { - AprilTagDetection d24 = webcam.getTagById(24); - if (d24 != null) { - bearing = d24.ftcPose.bearing; - return bearing; - } else { - return 1000.0; - } - } else { - AprilTagDetection d20 = webcam.getTagById(20); - if (d20 != null) { - bearing = d20.ftcPose.bearing; - return bearing; - } else { - return 1000.0; - } - } + public double getBearingDeg() { + AprilTagDetection tag = + redAlliance ? webcam.getTagById(24) : webcam.getTagById(20); + return (tag != null) ? tag.ftcPose.bearing : Double.NaN; } public int detectObelisk() { - AprilTagDetection id21 = webcam.getTagById(21); - AprilTagDetection id22 = webcam.getTagById(22); - AprilTagDetection id23 = webcam.getTagById(23); - if (id21 != null) { - obeliskID = 21; - } else if (id22 != null) { - obeliskID = 22; - } else if (id23 != null) { - obeliskID = 23; - } + if (webcam.getTagById(21) != null) obeliskID = 21; + else if (webcam.getTagById(22) != null) obeliskID = 22; + else if (webcam.getTagById(23) != null) obeliskID = 23; return obeliskID; } @@ -85,58 +67,34 @@ public class Turret { return obeliskID; } - public void update() { - - } - - /* - Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading - */ public void trackGoal(Pose2d deltaPos) { - /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ - - // Angle from robot to goal in robot frame - double desiredTurretAngleDeg = Math.toDegrees( + double turretAngleDeg = Math.toDegrees( Math.atan2(deltaPos.position.y, deltaPos.position.x) ); - // Robot heading (field → robot) - double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble()); + double bearingDeg = getBearingDeg(); - // Turret angle needed relative to robot - double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; + if (!Double.isNaN(bearingDeg) && + Math.abs(bearingDeg) < cameraBearingEqual) { - // Normalize to [-180, 180] - while (turretAngleDeg > 180) turretAngleDeg -= 360; - while (turretAngleDeg < -180) turretAngleDeg += 360; - - /* ---------------- APRILTAG CORRECTION ---------------- */ - - double tagBearingDeg = getBearing(); // + = target is to the left - - if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) < cameraBearingEqual) { - // Slowly learn turret offset (persistent calibration) - offset -= tagBearingDeg * errorLearningRate; + offsetDeg -= bearingDeg * errorLearningRate; + offsetDeg = Range.clip(offsetDeg, -maxOffsetDeg, maxOffsetDeg); } - turretAngleDeg += offset; + turretAngleDeg += offsetDeg; - /* ---------------- ANGLE → SERVO ---------------- */ + double turretPos = + turrDefault + (turretAngleDeg / 180.0) * turretRange; - double turretPos = turrDefault + (turretAngleDeg / (turret180Range * 2.0)); - - // Clamp to servo range - turretPos = Math.max(turrMin, Math.min(turretPos, turrMax)); + turretPos = Range.clip(turretPos, turrMin, turrMax); robot.turr1.setPosition(turretPos); robot.turr2.setPosition(1.0 - turretPos); - /* ---------------- TELEMETRY ---------------- */ - - TELE.addData("Turret Angle", turretAngleDeg); - TELE.addData("Bearing", tagBearingDeg); - TELE.addData("Offset", offset); + TELE.addData("Turret Angle (deg)", turretAngleDeg); + TELE.addData("Offset (deg)", offsetDeg); + TELE.addData("Tag Bearing (deg)", bearingDeg); + TELE.addData("Turret Servo", turretPos); } - } From 45199b952b75282d5942fa420312209378d62097 Mon Sep 17 00:00:00 2001 From: abhiramtx Date: Thu, 22 Jan 2026 20:03:00 -0600 Subject: [PATCH 3/6] added interpolation --- .../ftc/teamcode/utils/Targeting.java | 50 +++++++++++++++---- 1 file changed, 40 insertions(+), 10 deletions(-) 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 7a2acb0..d802ba2 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 @@ -84,28 +84,58 @@ public class Targeting { { } - public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity) { + public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); double cos45 = Math.cos(Math.toRadians(-45)); double sin45 = Math.sin(Math.toRadians(-45)); - double rotatedY = (robotX -40.0) * sin45 + (robotY +7.0) * cos45; - double rotatedX = (robotX -40.0) * cos45 - (robotY +7.0) * sin45; + double rotatedY = (robotX - 40.0) * sin45 + (robotY + 7.0) * cos45; + double rotatedX = (robotX - 40.0) * cos45 - (robotY + 7.0) * sin45; // Convert robot coordinates to inches robotInchesX = rotatedX * unitConversionFactor; robotInchesY = rotatedY * unitConversionFactor; // Find approximate location in the grid - robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) +1); + robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); - // Use Grid Location to perform lookup - // Keep it simple for now but may want to interpolate results - if ((robotGridY < 6) && (robotGridX <6)) { - recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; - recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; + //clamp + robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); + robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); + + // basic search + if(!interpolate) { + if ((robotGridY < 6) && (robotGridX <6)) { + recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; + recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; + } + return recommendedSettings; + } else { + + // 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); + + double x = (robotInchesX - (x0 * tileSize)) / tileSize; + double y = (robotInchesY - (y0 * tileSize)) / tileSize; + + double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM; + double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM; + double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM; + double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM; + + double angle00 = KNOWNTARGETING[y0][x0].hoodAngle; + double angle10 = KNOWNTARGETING[y0][x1].hoodAngle; + double angle01 = KNOWNTARGETING[y1][x0].hoodAngle; + double angle11 = KNOWNTARGETING[y1][x1].hoodAngle; + + recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11; + recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11; + + return recommendedSettings; } - return recommendedSettings; } } From 2f0fcad1287b3f62b8c178cd09c73ac0df5350ef Mon Sep 17 00:00:00 2001 From: abhiramtx Date: Thu, 22 Jan 2026 20:06:08 -0600 Subject: [PATCH 4/6] updated interpolation in teleop --- .../java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index f1eca29..ec6012e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -123,6 +123,7 @@ public class TeleopV3 extends LinearOpMode { private double transferStamp = 0.0; private int tickerA = 1; private boolean transferIn = false; + boolean turretInterpolate = true; public static double velPrediction(double distance) { if (distance < 30) { @@ -413,7 +414,7 @@ public class TeleopV3 extends LinearOpMode { targetingSettings = targeting.calculateSettings - (robotX,robotY,robotHeading,0.0); + (robotX,robotY,robotHeading,0.0, turretInterpolate); //VELOCITY AUTOMATIC if (targetingVel) { @@ -929,6 +930,7 @@ public class TeleopV3 extends LinearOpMode { TELE.addData( "robotY", robotY); TELE.addData("robotInchesX", targeting.robotInchesX); TELE.addData( "robotInchesY", targeting.robotInchesY); + TELE.addData("Targeting Interpolate", turretInterpolate); TELE.addData("Targeting GridX", targeting.robotGridX); TELE.addData("Targeting GridY", targeting.robotGridY); TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); From 46a5366a4a311d912f06a09d77dd9907fdd0ba8d Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Thu, 22 Jan 2026 21:59:58 -0600 Subject: [PATCH 5/6] Add Auto ball detect on startup to spindexer to detect how many balls are already in spindexer on power on. --- .../ftc/teamcode/utils/Spindexer.java | 110 +++++++++++------- 1 file changed, 68 insertions(+), 42 deletions(-) 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 e5b7d5b..3d95256 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 @@ -52,7 +52,9 @@ public class Spindexer { } enum IntakeState { - UNKNOWN, + UNKNOWN_START, + UNKNOWN_MOVE, + UNKNOWN_DETECT, INTAKE, FINDNEXT, MOVING, @@ -62,8 +64,8 @@ public class Spindexer { SHOOTWAIT, }; - public IntakeState currentIntakeState = IntakeState.UNKNOWN; - + public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; + public int unknownColorDetect = 0; enum BallColor { UNKNOWN, GREEN, @@ -131,13 +133,13 @@ public class Spindexer { for (int i = 0; i < 3; i++) { resetBallPosition(i); } - currentIntakeState = IntakeState.UNKNOWN; + currentIntakeState = IntakeState.UNKNOWN_START; } // Detects if a ball is found and what color. // Returns true is there was a new ball found in Position 1 // FIXIT: Reduce number of times that we read the color sensors for loop times. - public boolean detectBalls() { + public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) { boolean newPos1Detection = false; int spindexerBallPos = 0; @@ -153,18 +155,20 @@ public class Spindexer { // Mark Ball Found newPos1Detection = true; - // Detect which color - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; + if (detectRearColor) { + // Detect which color + double green = robot.color1.getNormalizedColors().green; + double red = robot.color1.getNormalizedColors().red; + double blue = robot.color1.getNormalizedColors().blue; - double gP = green / (green + red + blue); + double gP = green / (green + red + blue); - // FIXIT - Add filtering to improve accuracy. - if (gP >= 0.4) { - ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple - } else { - ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple + // FIXIT - Add filtering to improve accuracy. + if (gP >= 0.4) { + ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple + } else { + ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple + } } } // Position 2 @@ -173,18 +177,19 @@ public class Spindexer { if (distanceFrontDriver < 60) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; - // FIXIT: Comment out for now due to loop time concerns -// double green = robot.color2.getNormalizedColors().green; -// double red = robot.color2.getNormalizedColors().red; -// double blue = robot.color2.getNormalizedColors().blue; -// -// double gP = green / (green + red + blue); + if (detectFrontColor) { + double green = robot.color2.getNormalizedColors().green; + double red = robot.color2.getNormalizedColors().red; + double blue = robot.color2.getNormalizedColors().blue; -// if (gP >= 0.4) { -// b2 = 2; // purple -// } else { -// b2 = 1; // green -// } + double gP = green / (green + red + blue); + + if (gP >= 0.4) { + ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple + } else { + ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple + } + } } else { if (!ballPositions[spindexerBallPos].isEmpty) { if (ballPositions[spindexerBallPos].foundEmpty > 3) { @@ -201,18 +206,19 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; - // FIXIT: Comment out for now due to loop time concerns -// double green = robot.color3.getNormalizedColors().green; -// double red = robot.color3.getNormalizedColors().red; -// double blue = robot.color3.getNormalizedColors().blue; + if (detectFrontColor) { + double green = robot.color3.getNormalizedColors().green; + double red = robot.color3.getNormalizedColors().red; + double blue = robot.color3.getNormalizedColors().blue; -// double gP = green / (green + red + blue); + double gP = green / (green + red + blue); -// if (gP >= 0.4) { -// b3 = 2; // purple -// } else { -// b3 = 1; // green -// } + if (gP >= 0.4) { + ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple + } else { + ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple + } + } } else { if (!ballPositions[spindexerBallPos].isEmpty) { if (ballPositions[spindexerBallPos].foundEmpty > 3) { @@ -255,15 +261,35 @@ public class Spindexer { public boolean processIntake() { switch (currentIntakeState) { - case UNKNOWN: + case UNKNOWN_START: // For now just set position ONE if UNKNOWN commandedIntakePosition = 0; servos.setSpinPos(intakePositions[0]); - currentIntakeState = Spindexer.IntakeState.MOVING; + currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; + break; + case UNKNOWN_MOVE: + // Stopping when we get to the new position + if (servos.spinEqual(intakePositions[commandedIntakePosition])) { + currentIntakeState = Spindexer.IntakeState.UNKNOWN_DETECT; + stopSpindexer(); + detectBalls(true, true); + unknownColorDetect = 0; + } else { + // Keep moving the spindexer + moveSpindexerToPos(intakePositions[commandedIntakePosition]); + } + break; + case UNKNOWN_DETECT: + if (unknownColorDetect >5) { + currentIntakeState = Spindexer.IntakeState.FINDNEXT; + } else { + detectBalls(true, true); + unknownColorDetect++; + } break; case INTAKE: // Ready for intake and Detecting a New Ball - if (detectBalls()) { + if (detectBalls(true, false)) { ballPositions[commandedIntakePosition].isEmpty = false; currentIntakeState = Spindexer.IntakeState.FINDNEXT; } else { @@ -311,7 +337,7 @@ public class Spindexer { if (servos.spinEqual(intakePositions[commandedIntakePosition])) { currentIntakeState = Spindexer.IntakeState.INTAKE; stopSpindexer(); - detectBalls(); + detectBalls(false, false); } else { // Keep moving the spindexer moveSpindexerToPos(intakePositions[commandedIntakePosition]); @@ -320,7 +346,7 @@ public class Spindexer { case FULL: // Double Check Colors - detectBalls(); + detectBalls(false, false); // Minimize hardware calls if (ballPositions[0].isEmpty || ballPositions[1].isEmpty || ballPositions[2].isEmpty) { // Error handling found an empty spot, get it ready for a ball currentIntakeState = Spindexer.IntakeState.FINDNEXT; @@ -378,7 +404,7 @@ public class Spindexer { if (servos.spinEqual(intakePositions[commandedIntakePosition])) { currentIntakeState = Spindexer.IntakeState.INTAKE; stopSpindexer(); - detectBalls(); + detectBalls(true, false); } else { // Keep moving the spindexer moveSpindexerToPos(intakePositions[commandedIntakePosition]); From 4050a354f7fb5855d2e42ee4bb720bdc2db60951 Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Fri, 23 Jan 2026 20:19:21 -0600 Subject: [PATCH 6/6] Update TelopV3 and Targeting for merge conflicts. --- .../java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java | 2 +- .../java/org/firstinspires/ftc/teamcode/utils/Targeting.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index bc81e2f..df2fc6d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -120,7 +120,7 @@ public class TeleopV3 extends LinearOpMode { private double transferStamp = 0.0; private int tickerA = 1; private boolean transferIn = false; - boolean turretInterpolate = true; + boolean turretInterpolate = false; public static double velPrediction(double distance) { if (distance < 30) { 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 d802ba2..d9850a5 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 @@ -97,8 +97,8 @@ public class Targeting { robotInchesY = rotatedY * unitConversionFactor; // Find approximate location in the grid - robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); - robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); + int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); + int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); //clamp robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));