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

Comment out color sensor reads for now to speed up loop times.

Browse Source
This commit is contained in:
Matt9150
2026-01-19 23:40:17 -06:00
parent 40d51ce757
commit 6f3a178a08
1 changed files with 26 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -136,6 +136,7 @@ public class Spindexer {
// 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.
public boolean detectBalls() { public boolean detectBalls() {
boolean newPos1Detection = false; boolean newPos1Detection = false;
@@ -172,11 +173,12 @@ public class Spindexer {
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;
double green = robot.color2.getNormalizedColors().green; // FIXIT: Comment out for now due to loop time concerns
double red = robot.color2.getNormalizedColors().red; // double green = robot.color2.getNormalizedColors().green;
double blue = robot.color2.getNormalizedColors().blue; // double red = robot.color2.getNormalizedColors().red;
// double blue = robot.color2.getNormalizedColors().blue;
double gP = green / (green + red + blue); //
// double gP = green / (green + red + blue);
// if (gP >= 0.4) { // if (gP >= 0.4) {
// b2 = 2; // purple // b2 = 2; // purple
@@ -199,11 +201,12 @@ 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;
double green = robot.color3.getNormalizedColors().green; // FIXIT: Comment out for now due to loop time concerns
double red = robot.color3.getNormalizedColors().red; // double green = robot.color3.getNormalizedColors().green;
double blue = robot.color3.getNormalizedColors().blue; // 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) { // if (gP >= 0.4) {
// b3 = 2; // purple // b3 = 2; // purple
@@ -271,24 +274,32 @@ public class Spindexer {
case FINDNEXT: case FINDNEXT:
// Find Next Open Position and start movement // Find Next Open Position and start movement
double currentSpindexerPos = servos.getSpinPos(); double currentSpindexerPos = servos.getSpinPos();
double travelDistance = 0.0; double commandedtravelDistance = 2.0;
if (ballPositions[0].isEmpty) { double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos);
if (ballPositions[0].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
// Position 1 // Position 1
commandedIntakePosition = 0; commandedIntakePosition = 0;
servos.setSpinPos(intakePositions[commandedIntakePosition]); servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
commandedtravelDistance = proposedTravelDistance;
} else if (ballPositions[1].isEmpty) { }
proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos);
if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
// Position 2 // Position 2
commandedIntakePosition = 1; commandedIntakePosition = 1;
servos.setSpinPos(intakePositions[commandedIntakePosition]); servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} else if (ballPositions[2].isEmpty) { commandedtravelDistance = proposedTravelDistance;
}
proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos);
if (ballPositions[2].isEmpty && (proposedTravelDistance < commandedtravelDistance)) {
// Position 3 // Position 3
commandedIntakePosition = 2; commandedIntakePosition = 2;
servos.setSpinPos(intakePositions[commandedIntakePosition]); servos.setSpinPos(intakePositions[commandedIntakePosition]);
currentIntakeState = Spindexer.IntakeState.MOVING; currentIntakeState = Spindexer.IntakeState.MOVING;
} else { commandedtravelDistance = proposedTravelDistance;
}
if (currentIntakeState != Spindexer.IntakeState.MOVING) {
// Full // Full
currentIntakeState = Spindexer.IntakeState.FULL; currentIntakeState = Spindexer.IntakeState.FULL;
} }