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

Create shootAll state machine in spindexer and call from TeleOpV3. Experiment with averaging tiles in Targeting, which is permanently disabled at the moment.

Browse Source
This commit is contained in:
Matt9150
2026-01-28 13:06:53 -06:00
parent 2a45eee878
commit d088fba20a
3 changed files with 153 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -614,25 +614,32 @@ public class TeleopV3 extends LinearOpMode {
intake = false; intake = false;
reject = false; reject = false;
if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) { // if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) {
//
if (shooterTicker == 0){ // if (shooterTicker == 0){
robot.transferServo.setPosition(transferServo_out); // robot.transferServo.setPosition(transferServo_out);
robot.spin1.setPosition(spinStartPos); // robot.spin1.setPosition(spinStartPos);
robot.spin2.setPosition(1-spinStartPos); // robot.spin2.setPosition(1-spinStartPos);
if (servo.spinEqual(spinStartPos)){ // if (servo.spinEqual(spinStartPos)){
shooterTicker++; // shooterTicker++;
} // }
TELE.addLine("starting to shoot"); // TELE.addLine("starting to shoot");
} else { // } else {
robot.transferServo.setPosition(transferServo_in); // robot.transferServo.setPosition(transferServo_in);
shooterTicker++; // shooterTicker++;
double prevSpinPos = servo.getSpinPos(); // double prevSpinPos = servo.getSpinPos();
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); // robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); // robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
TELE.addLine("shooting"); // 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 { } else {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
//spindexPos = spindexer_intakePos1; //spindexPos = spindexer_intakePos1;
@@ -640,9 +647,10 @@ public class TeleopV3 extends LinearOpMode {
shootAll = false; shootAll = false;
spindexer.resetSpindexer(); spindexer.resetSpindexer();
spindexer.processIntake(); //spindexer.processIntake();
TELE.addLine("stop shooting"); TELE.addLine("stop shooting");
} }
spindexer.processIntake();
} }
if (gamepad1.left_stick_button){ if (gamepad1.left_stick_button){

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

@@ -72,7 +72,10 @@ public class Spindexer {
SHOOT_ALL_READY SHOOT_ALL_READY
} }
int shootWaitCount = 0;
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
public int unknownColorDetect = 0; public int unknownColorDetect = 0;
enum BallColor { enum BallColor {
UNKNOWN, UNKNOWN,
@@ -397,23 +400,21 @@ public class Spindexer {
case SHOOT_ALL_PREP: case SHOOT_ALL_PREP:
// We get here with function call to prepareToShootMotif // We get here with function call to prepareToShootMotif
// Stopping when we get to the new position // Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) { if (!servos.spinEqual(outakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_READY;
} else {
// Keep moving the spindexer // 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; break;
case SHOOT_ALL_READY: case SHOOT_ALL_READY: // Not used
// Double Check Colors // Double Check Colors
//detectBalls(false, false); // Minimize hardware calls //detectBalls(false, false); // Minimize hardware calls
if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) { if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) {
// All ball shot move to intake state // All ball shot move to intake state
currentIntakeState = Spindexer.IntakeState.FINDNEXT; currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
} }
// Maintain Position // Maintain Position
moveSpindexerToPos(intakePositions[commandedIntakePosition]); moveSpindexerToPos(outakePositions[commandedIntakePosition]);
break; break;
case SHOOTNEXT: case SHOOTNEXT:
@@ -431,13 +432,13 @@ public class Spindexer {
} else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? } else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty?
// Position 3 // Position 3
commandedIntakePosition = 2; commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" servos.setSpinPos(outakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING;
} else { } else {
// Empty return to intake state // Empty return to intake state
currentIntakeState = IntakeState.FINDNEXT; currentIntakeState = IntakeState.FINDNEXT;
} }
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" moveSpindexerToPos(outakePositions[commandedIntakePosition]);
break; break;
case SHOOTMOVING: case SHOOTMOVING:
@@ -446,15 +447,22 @@ public class Spindexer {
currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; currentIntakeState = Spindexer.IntakeState.SHOOTWAIT;
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" moveSpindexerToPos(outakePositions[commandedIntakePosition]);
} }
break; break;
case SHOOTWAIT: case SHOOTWAIT:
// Stopping when we get to the new position // Stopping when we get to the new position]
if (servos.spinEqual(intakePositions[commandedIntakePosition])) { if (prevIntakeState != currentIntakeState) {
currentIntakeState = Spindexer.IntakeState.INTAKE; shootWaitCount = 0;
stopSpindexer(); } else {
shootWaitCount++;
}
// wait 3 cycles
if ((shootWaitCount > 3) && (servos.spinEqual(intakePositions[commandedIntakePosition]))) {
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
ballPositions[commandedIntakePosition].isEmpty = true;
//stopSpindexer();
//detectBalls(true, false); //detectBalls(true, false);
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
@@ -465,6 +473,8 @@ public class Spindexer {
default: default:
// Statements to execute if no case matches // Statements to execute if no case matches
} }
prevIntakeState = currentIntakeState;
//TELE.addData("commandedIntakePosition", commandedIntakePosition); //TELE.addData("commandedIntakePosition", commandedIntakePosition);
//TELE.update(); //TELE.update();
// Signal a successful intake // Signal a successful intake
@@ -516,6 +526,22 @@ public class Spindexer {
commandedIntakePosition = bestFitMotif(); 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 () { void shootAllToIntake () {
currentIntakeState = Spindexer.IntakeState.FINDNEXT; currentIntakeState = Spindexer.IntakeState.FINDNEXT;
} }

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

@@ -107,61 +107,81 @@ public class Targeting {
// Determine if we need to interpolate based on tile position. // Determine if we need to interpolate based on tile position.
// if near upper or lower quarter or tile interpolate with next tile. // if near upper or lower quarter or tile interpolate with next tile.
int x0 = 0;
int y0 = 0;
int x1 = 0; int x1 = 0;
int y1 = 0; int y1 = 0;
// interpolate = false; interpolate = false;
// if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// (robotGridX < 5) && (robotGridY <5)) { (robotGridX < 5) && (robotGridY <5)) {
// // +X, +Y // +X, +Y
// interpolate = true; interpolate = true;
// x1 = robotGridX + 1; x0 = robotGridX;
// y1 = robotGridY + 1; x1 = robotGridX + 1;
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && y0 = robotGridY;
// (robotGridX > 0) && (robotGridY > 0)) { y1 = robotGridY + 1;
// // -X, -Y } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// interpolate = true; (robotGridX > 0) && (robotGridY > 0)) {
// x1 = robotGridX - 1; // -X, -Y
// y1 = robotGridY - 1; interpolate = true;
// } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && x0 = robotGridX - 1;
// (robotGridX < 5) && (robotGridY > 0)) { x1 = robotGridX;
// // +X, -Y y0 = robotGridY - 1;
// interpolate = true; y1 = robotGridY;
// x1 = robotGridX + 1; } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) &&
// y1 = robotGridY - 1; (robotGridX < 5) && (robotGridY > 0)) {
// } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && // +X, -Y
// (robotGridX > 0) && (robotGridY < 5)) { interpolate = true;
// // -X, +Y x0 = robotGridX;
// interpolate = true; x1 = robotGridX + 1;
// x1 = robotGridX - 1; y0 = robotGridY - 1;
// y1 = robotGridY + 1; y1 = robotGridY;
// } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) { } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) &&
// // -X, Y (robotGridX > 0) && (robotGridY < 5)) {
// interpolate = true; // -X, +Y
// x1 = robotGridX - 1; interpolate = true;
// y1 = robotGridY; x0 = robotGridX - 1;
// } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) { x1 = robotGridX;
// // X, -Y y0 = robotGridY;
// interpolate = true; y1 = robotGridY + 1;
// x1 = robotGridX; } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) {
// y1 = robotGridY - 1; // -X, Y
// } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) { interpolate = true;
// // +X, Y x0 = robotGridX - 1;
// interpolate = true; x1 = robotGridX;
// x1 = robotGridX + 1; y0 = robotGridY;
// y1 = robotGridY; y1 = robotGridY;
// } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) { } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) {
// // X, +Y // X, -Y
// interpolate = true; interpolate = true;
// x1 = robotGridX; x0 = robotGridX;
// y1 = robotGridY + 1; 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 //clamp
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
// basic search // basic search
if(!interpolate) { if(true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX <6)) { if ((robotGridY < 6) && (robotGridX <6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
@@ -170,27 +190,29 @@ public class Targeting {
} else { } else {
// bilinear interpolation // bilinear interpolation
int x0 = robotGridX; //int x0 = robotGridX;
//int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1); //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); //int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
double x = (robotInchesX - (x0 * tileSize)) / tileSize; // double x = (robotInchesX - (x0 * tileSize)) / tileSize;
double y = (robotInchesY - (y0 * tileSize)) / tileSize; // double y = (robotInchesY - (y0 * tileSize)) / tileSize;
double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM; // double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM; // double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM; // double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM; // double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
//
double angle00 = KNOWNTARGETING[y0][x0].hoodAngle; // double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
double angle10 = KNOWNTARGETING[y0][x1].hoodAngle; // double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
double angle01 = KNOWNTARGETING[y1][x0].hoodAngle; // double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
double angle11 = KNOWNTARGETING[y1][x1].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;
// 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; return recommendedSettings;
} }
} }