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

Add Auto ball detect on startup to spindexer to detect how many balls are already in spindexer on power on.

Browse Source
This commit is contained in:
Matt9150
2026-01-22 21:59:58 -06:00
parent daccec4fdd
commit 46a5366a4a
1 changed files with 68 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -52,7 +52,9 @@ public class Spindexer {
} }
enum IntakeState { enum IntakeState {
UNKNOWN, UNKNOWN_START,
UNKNOWN_MOVE,
UNKNOWN_DETECT,
INTAKE, INTAKE,
FINDNEXT, FINDNEXT,
MOVING, MOVING,
@@ -62,8 +64,8 @@ public class Spindexer {
SHOOTWAIT, SHOOTWAIT,
}; };
public IntakeState currentIntakeState = IntakeState.UNKNOWN; public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public int unknownColorDetect = 0;
enum BallColor { enum BallColor {
UNKNOWN, UNKNOWN,
GREEN, GREEN,
@@ -131,13 +133,13 @@ public class Spindexer {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
resetBallPosition(i); resetBallPosition(i);
} }
currentIntakeState = IntakeState.UNKNOWN; currentIntakeState = IntakeState.UNKNOWN_START;
} }
// Detects if a ball is found and what color. // Detects if a ball is found and what color.
// Returns true is there was a new ball found in Position 1 // 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. // 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; boolean newPos1Detection = false;
int spindexerBallPos = 0; int spindexerBallPos = 0;
@@ -153,6 +155,7 @@ public class Spindexer {
// Mark Ball Found // Mark Ball Found
newPos1Detection = true; newPos1Detection = true;
if (detectRearColor) {
// Detect which color // Detect which color
double green = robot.color1.getNormalizedColors().green; double green = robot.color1.getNormalizedColors().green;
double red = robot.color1.getNormalizedColors().red; double red = robot.color1.getNormalizedColors().red;
@@ -167,24 +170,26 @@ public class Spindexer {
ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple
} }
} }
}
// Position 2 // Position 2
// Find which ball position this is in the spindexer // Find which ball position this is in the spindexer
spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]; spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()];
if (distanceFrontDriver < 60) { if (distanceFrontDriver < 60) {
// reset FoundEmpty because looking for 3 in a row before reset // reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0; ballPositions[spindexerBallPos].foundEmpty = 0;
// FIXIT: Comment out for now due to loop time concerns if (detectFrontColor) {
// double green = robot.color2.getNormalizedColors().green; double green = robot.color2.getNormalizedColors().green;
// double red = robot.color2.getNormalizedColors().red; double red = robot.color2.getNormalizedColors().red;
// double blue = robot.color2.getNormalizedColors().blue; double blue = robot.color2.getNormalizedColors().blue;
//
// double gP = green / (green + red + blue);
// if (gP >= 0.4) { double gP = green / (green + red + blue);
// b2 = 2; // purple
// } else { if (gP >= 0.4) {
// b2 = 1; // green ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
// } } else {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple
}
}
} else { } else {
if (!ballPositions[spindexerBallPos].isEmpty) { if (!ballPositions[spindexerBallPos].isEmpty) {
if (ballPositions[spindexerBallPos].foundEmpty > 3) { if (ballPositions[spindexerBallPos].foundEmpty > 3) {
@@ -201,18 +206,19 @@ public class Spindexer {
// reset FoundEmpty because looking for 3 in a row before reset // reset FoundEmpty because looking for 3 in a row before reset
ballPositions[spindexerBallPos].foundEmpty = 0; ballPositions[spindexerBallPos].foundEmpty = 0;
// FIXIT: Comment out for now due to loop time concerns if (detectFrontColor) {
// double green = robot.color3.getNormalizedColors().green; double green = robot.color3.getNormalizedColors().green;
// double red = robot.color3.getNormalizedColors().red; double red = robot.color3.getNormalizedColors().red;
// double blue = robot.color3.getNormalizedColors().blue; double blue = robot.color3.getNormalizedColors().blue;
// double gP = green / (green + red + blue); double gP = green / (green + red + blue);
// if (gP >= 0.4) { if (gP >= 0.4) {
// b3 = 2; // purple ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
// } else { } else {
// b3 = 1; // green ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple
// } }
}
} else { } else {
if (!ballPositions[spindexerBallPos].isEmpty) { if (!ballPositions[spindexerBallPos].isEmpty) {
if (ballPositions[spindexerBallPos].foundEmpty > 3) { if (ballPositions[spindexerBallPos].foundEmpty > 3) {
@@ -255,15 +261,35 @@ public class Spindexer {
public boolean processIntake() { public boolean processIntake() {
switch (currentIntakeState) { switch (currentIntakeState) {
case UNKNOWN: case UNKNOWN_START:
// For now just set position ONE if UNKNOWN // For now just set position ONE if UNKNOWN
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[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; break;
case INTAKE: case INTAKE:
// Ready for intake and Detecting a New Ball // Ready for intake and Detecting a New Ball
if (detectBalls()) { if (detectBalls(true, false)) {
ballPositions[commandedIntakePosition].isEmpty = false; ballPositions[commandedIntakePosition].isEmpty = false;
currentIntakeState = Spindexer.IntakeState.FINDNEXT; currentIntakeState = Spindexer.IntakeState.FINDNEXT;
} else { } else {
@@ -311,7 +337,7 @@ public class Spindexer {
if (servos.spinEqual(intakePositions[commandedIntakePosition])) { if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.INTAKE; currentIntakeState = Spindexer.IntakeState.INTAKE;
stopSpindexer(); stopSpindexer();
detectBalls(); detectBalls(false, false);
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); moveSpindexerToPos(intakePositions[commandedIntakePosition]);
@@ -320,7 +346,7 @@ public class Spindexer {
case FULL: case FULL:
// Double Check Colors // Double Check Colors
detectBalls(); 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) {
// Error handling found an empty spot, get it ready for a ball // Error handling found an empty spot, get it ready for a ball
currentIntakeState = Spindexer.IntakeState.FINDNEXT; currentIntakeState = Spindexer.IntakeState.FINDNEXT;
@@ -378,7 +404,7 @@ public class Spindexer {
if (servos.spinEqual(intakePositions[commandedIntakePosition])) { if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
currentIntakeState = Spindexer.IntakeState.INTAKE; currentIntakeState = Spindexer.IntakeState.INTAKE;
stopSpindexer(); stopSpindexer();
detectBalls(); detectBalls(true, false);
} else { } else {
// Keep moving the spindexer // Keep moving the spindexer
moveSpindexerToPos(intakePositions[commandedIntakePosition]); moveSpindexerToPos(intakePositions[commandedIntakePosition]);