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

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

@@ -614,25 +614,32 @@ public class TeleopV3 extends LinearOpMode {
intake = false;
reject = false;
if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) {
// 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) {
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 {
spindexer.prepareShootAll();
} else if (!spindexer.shootAllComplete()) {
robot.transferServo.setPosition(transferServo_in);
shooterTicker++;
double prevSpinPos = servo.getSpinPos();
robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease);
robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease);
TELE.addLine("shooting");
}
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){

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

@@ -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;
}

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.
// 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;
}
}