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

added delay based on ticks

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-04 19:51:57 -05:00
parent 9b92a59a75
commit 3afab333ef
1 changed files with 43 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java
View File

@@ -261,6 +261,8 @@ public class SpindexerTransferIntake {
return System.currentTimeMillis() - sortedStateStartTime; return System.currentTimeMillis() - sortedStateStartTime;
} }
private int greenTicks = 0;
private int ballTicks = 0;
public void update() { public void update() {
TELE.addData("Sorted State", sortedIntakeStates); TELE.addData("Sorted State", sortedIntakeStates);
@@ -431,15 +433,22 @@ public class SpindexerTransferIntake {
robot.setIntakePower(1); robot.setIntakePower(1);
robot.setTransferPower(-1); robot.setTransferPower(-1);
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
//TODO: ADD DELAY OR AVERGE @ DANIEL
NormalizedRGBA revColor = robot.revSensor.getNormalizedColors(); NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) { if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
ballColors[0] = BallStates.GREEN; greenTicks++;
} else { }
ballColors[0] = BallStates.PURPLE; ballTicks++;
if (ballTicks > 15) {
if (greenTicks > 10){
ballColors[0] = BallStates.GREEN;
} else {
ballColors[0] = BallStates.PURPLE;
}
robot.setSpinPos(ServoPositions.spindexer_A2);
setSortedIntakeMode(SortedIntakeStates.DELAY_1);
ballTicks = 0;
greenTicks = 0;
} }
robot.setSpinPos(ServoPositions.spindexer_A2);
setSortedIntakeMode(SortedIntakeStates.DELAY_1);
} }
break; break;
case DELAY_1: case DELAY_1:
@@ -449,17 +458,27 @@ public class SpindexerTransferIntake {
} }
break; break;
case INTAKE_2: case INTAKE_2:
robot.setIntakePower(1);
robot.setTransferPower(-1);
robot.setIntakePower(1); robot.setIntakePower(1);
robot.setTransferPower(-1); robot.setTransferPower(-1);
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
NormalizedRGBA revColor = robot.revSensor.getNormalizedColors(); NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) { if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
ballColors[1] = BallStates.GREEN; greenTicks++;
} else { }
ballColors[1] = BallStates.PURPLE; ballTicks++;
if (ballTicks > 15) {
if (greenTicks > 10){
ballColors[0] = BallStates.GREEN;
} else {
ballColors[0] = BallStates.PURPLE;
}
robot.setSpinPos(ServoPositions.spindexer_A3);
setSortedIntakeMode(SortedIntakeStates.DELAY_2);
ballTicks = 0;
greenTicks = 0;
} }
robot.setSpinPos(ServoPositions.spindexer_A3);
setSortedIntakeMode(SortedIntakeStates.DELAY_2);
} }
break; break;
case DELAY_2: case DELAY_2:
@@ -481,17 +500,25 @@ public class SpindexerTransferIntake {
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
NormalizedRGBA revColor = robot.revSensor.getNormalizedColors(); NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) { if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
ballColors[2] = BallStates.GREEN; greenTicks++;
} else { }
ballColors[2] = BallStates.PURPLE; ballTicks++;
if (ballTicks > 15) {
if (greenTicks > 10){
ballColors[0] = BallStates.GREEN;
} else {
ballColors[0] = BallStates.PURPLE;
}
setSortedIntakeMode(SortedIntakeStates.REVERSE);
ballTicks = 0;
greenTicks = 0;
} }
setSortedIntakeMode(SortedIntakeStates.REVERSE);
} }
break; break;
case REVERSE: case REVERSE:
robot.setTransferPower(-0.3); robot.setTransferPower(-0.3);
robot.setIntakePower(-0.1); robot.setIntakePower(-0.1);
break;
} }