From d088fba20a586996e0e8e1cd9caf1400639e6b5d Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Wed, 28 Jan 2026 13:06:53 -0600 Subject: [PATCH] Create shootAll state machine in spindexer and call from TeleOpV3. Experiment with averaging tiles in Targeting, which is permanently disabled at the moment. --- .../ftc/teamcode/teleop/TeleopV3.java | 48 +++--- .../ftc/teamcode/utils/Spindexer.java | 54 +++++-- .../ftc/teamcode/utils/Targeting.java | 148 ++++++++++-------- 3 files changed, 153 insertions(+), 97 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 ae03417..7228585 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 @@ -614,25 +614,32 @@ public class TeleopV3 extends LinearOpMode { intake = false; reject = false; - if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) { - - if (shooterTicker == 0){ - robot.transferServo.setPosition(transferServo_out); - robot.spin1.setPosition(spinStartPos); - robot.spin2.setPosition(1-spinStartPos); - if (servo.spinEqual(spinStartPos)){ - shooterTicker++; - } - TELE.addLine("starting to shoot"); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = servo.getSpinPos(); - robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); - robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); - TELE.addLine("shooting"); - } - +// if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) { +// +// if (shooterTicker == 0){ +// robot.transferServo.setPosition(transferServo_out); +// robot.spin1.setPosition(spinStartPos); +// robot.spin2.setPosition(1-spinStartPos); +// if (servo.spinEqual(spinStartPos)){ +// shooterTicker++; +// } +// TELE.addLine("starting to shoot"); +// } else { +// robot.transferServo.setPosition(transferServo_in); +// shooterTicker++; +// double prevSpinPos = servo.getSpinPos(); +// robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); +// robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); +// TELE.addLine("shooting"); +// } + if (shooterTicker == 0) { + shooterTicker++; + spindexer.prepareShootAll(); + } else if (!spindexer.shootAllComplete()) { + robot.transferServo.setPosition(transferServo_in); + shooterTicker++; + spindexer.shootAll(); + TELE.addLine("starting to shoot"); } else { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; @@ -640,9 +647,10 @@ public class TeleopV3 extends LinearOpMode { shootAll = false; spindexer.resetSpindexer(); - spindexer.processIntake(); + //spindexer.processIntake(); TELE.addLine("stop shooting"); } + spindexer.processIntake(); } if (gamepad1.left_stick_button){ 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 03d038d..1975199 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 @@ -72,7 +72,10 @@ public class Spindexer { SHOOT_ALL_READY } + int shootWaitCount = 0; + public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; + public IntakeState prevIntakeState = IntakeState.UNKNOWN_START; public int unknownColorDetect = 0; enum BallColor { UNKNOWN, @@ -397,23 +400,21 @@ public class Spindexer { case SHOOT_ALL_PREP: // We get here with function call to prepareToShootMotif // Stopping when we get to the new position - if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_READY; - } else { + if (!servos.spinEqual(outakePositions[commandedIntakePosition])) { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" } break; - case SHOOT_ALL_READY: + case SHOOT_ALL_READY: // Not used // Double Check Colors //detectBalls(false, false); // Minimize hardware calls if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) { // All ball shot move to intake state - currentIntakeState = Spindexer.IntakeState.FINDNEXT; + currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } // Maintain Position - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + moveSpindexerToPos(outakePositions[commandedIntakePosition]); break; case SHOOTNEXT: @@ -431,13 +432,13 @@ public class Spindexer { } else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? // Position 3 commandedIntakePosition = 2; - servos.setSpinPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + servos.setSpinPos(outakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; } else { // Empty return to intake state currentIntakeState = IntakeState.FINDNEXT; } - moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + moveSpindexerToPos(outakePositions[commandedIntakePosition]); break; case SHOOTMOVING: @@ -446,15 +447,22 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + moveSpindexerToPos(outakePositions[commandedIntakePosition]); } break; case SHOOTWAIT: - // Stopping when we get to the new position - if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - currentIntakeState = Spindexer.IntakeState.INTAKE; - stopSpindexer(); + // Stopping when we get to the new position] + if (prevIntakeState != currentIntakeState) { + shootWaitCount = 0; + } else { + shootWaitCount++; + } + // wait 3 cycles + if ((shootWaitCount > 3) && (servos.spinEqual(intakePositions[commandedIntakePosition]))) { + currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; + ballPositions[commandedIntakePosition].isEmpty = true; + //stopSpindexer(); //detectBalls(true, false); } else { // Keep moving the spindexer @@ -465,6 +473,8 @@ public class Spindexer { default: // Statements to execute if no case matches } + + prevIntakeState = currentIntakeState; //TELE.addData("commandedIntakePosition", commandedIntakePosition); //TELE.update(); // Signal a successful intake @@ -516,6 +526,22 @@ public class Spindexer { commandedIntakePosition = bestFitMotif(); } + public void prepareShootAll(){ + currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP; + } + public void shootAll () { + currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; + } + + public boolean shootAllComplete () + { + return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) && + (currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) && + (currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) && + (currentIntakeState != Spindexer.IntakeState.SHOOTNEXT) && + (currentIntakeState != Spindexer.IntakeState.SHOOTWAIT)); + } + void shootAllToIntake () { currentIntakeState = Spindexer.IntakeState.FINDNEXT; } 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 12758ff..56b70a9 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 @@ -107,61 +107,81 @@ public class Targeting { // Determine if we need to interpolate based on tile position. // if near upper or lower quarter or tile interpolate with next tile. + int x0 = 0; + int y0 = 0; 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; -// } + interpolate = false; + if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && + (robotGridX < 5) && (robotGridY <5)) { + // +X, +Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX + 1; + y0 = robotGridY; + y1 = robotGridY + 1; + } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && + (robotGridX > 0) && (robotGridY > 0)) { + // -X, -Y + interpolate = true; + x0 = robotGridX - 1; + x1 = robotGridX; + y0 = robotGridY - 1; + y1 = robotGridY; + } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && + (robotGridX < 5) && (robotGridY > 0)) { + // +X, -Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX + 1; + y0 = robotGridY - 1; + y1 = robotGridY; + } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && + (robotGridX > 0) && (robotGridY < 5)) { + // -X, +Y + interpolate = true; + x0 = robotGridX - 1; + x1 = robotGridX; + y0 = robotGridY; + y1 = robotGridY + 1; + } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) { + // -X, Y + interpolate = true; + x0 = robotGridX - 1; + x1 = robotGridX; + y0 = robotGridY; + y1 = robotGridY; + } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) { + // X, -Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX; + y0 = robotGridY - 1; + y1 = robotGridY; + } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) { + // +X, Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX + 1; + y0 = robotGridY; + y1 = robotGridY; + } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) { + // X, +Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX; + y0 = robotGridY; + y1 = robotGridY + 1; + } else { + interpolate = false; + } //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(true) { //!interpolate) { if ((robotGridY < 6) && (robotGridX <6)) { recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; @@ -170,27 +190,29 @@ public class Targeting { } else { // bilinear interpolation - int x0 = robotGridX; + //int x0 = robotGridX; //int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1); - int y0 = robotGridY; + //int y0 = robotGridY; //int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1); - double x = (robotInchesX - (x0 * tileSize)) / tileSize; - double y = (robotInchesY - (y0 * tileSize)) / tileSize; +// 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; +// 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; + // Average target tiles + recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM)/2.0; + recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle)/2.0; return recommendedSettings; } }