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

Added transfer stuff

Browse Source
This commit is contained in:
Keshav Anand
2026-06-04 18:12:52 -05:00
parent 8c2a655c5c
commit cca86f3691
1 changed files with 25 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -49,7 +49,7 @@ public class SpindexerTransferIntake {
DONE DONE
} }
int[] shootOrder = {0,1,2}; int[] shootOrder = {0, 1, 2};
private final double sensorDistanceThreshold = 6.0; private final double sensorDistanceThreshold = 6.0;
private final long pulseTime = 100; // ms private final long pulseTime = 100; // ms
@@ -58,12 +58,12 @@ public class SpindexerTransferIntake {
private SortedShootState shootState = SortedShootState.IDLE; private SortedShootState shootState = SortedShootState.IDLE;
private long shootStateStartTime = System.currentTimeMillis(); private long shootStateStartTime = System.currentTimeMillis();
private void setShootState(SortedShootState newState){ private void setShootState(SortedShootState newState) {
shootState = newState; shootState = newState;
shootStateStartTime = System.currentTimeMillis(); shootStateStartTime = System.currentTimeMillis();
} }
private long shootStateTime(){ private long shootStateTime() {
return System.currentTimeMillis() - shootStateStartTime; return System.currentTimeMillis() - shootStateStartTime;
} }
@@ -139,7 +139,7 @@ public class SpindexerTransferIntake {
private void moveToSlot(int slot) { private void moveToSlot(int slot) {
switch(slot) { switch (slot) {
case 0: case 0:
robot.setSpinPos( robot.setSpinPos(
@@ -227,7 +227,7 @@ public class SpindexerTransferIntake {
this.mode = spindexerMode; this.mode = spindexerMode;
} }
public RapidMode getRapidState(){ public RapidMode getRapidState() {
return this.rapidMode; return this.rapidMode;
} }
@@ -336,7 +336,7 @@ public class SpindexerTransferIntake {
setRapidMode(RapidMode.SHOOT); setRapidMode(RapidMode.SHOOT);
} }
if (Shooter.manualFlywheel){ if (Shooter.manualFlywheel) {
robot.setTransferPower(NewShooterTest.transferPower); robot.setTransferPower(NewShooterTest.transferPower);
} else { } else {
robot.setTransferPower(commander.getTransferPow()); robot.setTransferPower(commander.getTransferPow());
@@ -353,7 +353,7 @@ public class SpindexerTransferIntake {
setRapidMode(RapidMode.INTAKE); setRapidMode(RapidMode.INTAKE);
} }
if (Shooter.manualFlywheel){ if (Shooter.manualFlywheel) {
robot.setTransferPower(NewShooterTest.transferPower); robot.setTransferPower(NewShooterTest.transferPower);
} else { } else {
robot.setTransferPower(commander.getTransferPow()); robot.setTransferPower(commander.getTransferPow());
@@ -365,7 +365,7 @@ public class SpindexerTransferIntake {
case SORTED: case SORTED:
switch (sortedIntakeStates){ switch (sortedIntakeStates) {
case NOTHING: case NOTHING:
break; break;
case IDLE: case IDLE:
@@ -390,7 +390,7 @@ public class SpindexerTransferIntake {
case INTAKE_1: case INTAKE_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[0] = BallStates.GREEN; ballColors[0] = BallStates.GREEN;
@@ -403,14 +403,14 @@ public class SpindexerTransferIntake {
break; break;
case DELAY_1: case DELAY_1:
robot.setSpinPos(ServoPositions.spindexer_A2); robot.setSpinPos(ServoPositions.spindexer_A2);
if (sortedStateTime() > spinMovementTime){ if (sortedStateTime() > spinMovementTime) {
setSortedIntakeMode(SortedIntakeStates.INTAKE_2); setSortedIntakeMode(SortedIntakeStates.INTAKE_2);
} }
break; break;
case INTAKE_2: case INTAKE_2:
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; ballColors[1] = BallStates.GREEN;
@@ -427,7 +427,7 @@ public class SpindexerTransferIntake {
ServoPositions.spindexer_A3 ServoPositions.spindexer_A3
); );
if (sortedStateTime() > spinMovementTime){ if (sortedStateTime() > spinMovementTime) {
setSortedIntakeMode( setSortedIntakeMode(
SortedIntakeStates.INTAKE_3 SortedIntakeStates.INTAKE_3
); );
@@ -437,7 +437,7 @@ public class SpindexerTransferIntake {
case INTAKE_3: case INTAKE_3:
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[2] = BallStates.GREEN; ballColors[2] = BallStates.GREEN;
@@ -457,9 +457,10 @@ public class SpindexerTransferIntake {
break; break;
case SHOOT_SORTED: case SHOOT_SORTED:
//TODO: ADD transfer intake powers here robot.setIntakePower(commander.getTransferPow());
switch (shootState){
switch (shootState) {
case IDLE: case IDLE:
shootOrder = buildShootOrder( shootOrder = buildShootOrder(
ballColors, ballColors,
@@ -486,7 +487,7 @@ public class SpindexerTransferIntake {
break; break;
case WAIT_FOR_1: case WAIT_FOR_1:
if(shootStateTime() > 250){ if (shootStateTime() > 250) {
setShootState( setShootState(
SortedShootState.SHOOT_1 SortedShootState.SHOOT_1
@@ -501,7 +502,7 @@ public class SpindexerTransferIntake {
robot.setTransferServoPos(ServoPositions.transferServo_in); robot.setTransferServoPos(ServoPositions.transferServo_in);
if(shootStateTime() > 300){ if (shootStateTime() > 300) {
setShootState( setShootState(
SortedShootState.RETRACT_1 SortedShootState.RETRACT_1
@@ -514,7 +515,7 @@ public class SpindexerTransferIntake {
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed); robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed);
robot.setTransferServoPos(ServoPositions.transferServo_out); robot.setTransferServoPos(ServoPositions.transferServo_out);
if(shootStateTime() > 150){ if (shootStateTime() > 150) {
setShootState( setShootState(
SortedShootState.MOVE_TO_2 SortedShootState.MOVE_TO_2
@@ -533,7 +534,7 @@ public class SpindexerTransferIntake {
break; break;
case WAIT_FOR_2: case WAIT_FOR_2:
if(shootStateTime() > 250){ if (shootStateTime() > 250) {
setShootState( setShootState(
SortedShootState.SHOOT_2 SortedShootState.SHOOT_2
@@ -546,7 +547,7 @@ public class SpindexerTransferIntake {
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open); robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
robot.setTransferServoPos(ServoPositions.transferServo_in); robot.setTransferServoPos(ServoPositions.transferServo_in);
if(shootStateTime() > 300){ if (shootStateTime() > 300) {
setShootState( setShootState(
SortedShootState.RETRACT_2 SortedShootState.RETRACT_2
@@ -560,8 +561,7 @@ public class SpindexerTransferIntake {
robot.setTransferServoPos(ServoPositions.transferServo_out); robot.setTransferServoPos(ServoPositions.transferServo_out);
if (shootStateTime() > 150) {
if(shootStateTime() > 150){
setShootState( setShootState(
SortedShootState.MOVE_TO_3 SortedShootState.MOVE_TO_3
@@ -580,7 +580,7 @@ public class SpindexerTransferIntake {
break; break;
case WAIT_FOR_3: case WAIT_FOR_3:
if(shootStateTime() > 250){ if (shootStateTime() > 250) {
setShootState( setShootState(
SortedShootState.SHOOT_3 SortedShootState.SHOOT_3
@@ -593,7 +593,7 @@ public class SpindexerTransferIntake {
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open); robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
robot.setTransferServoPos(ServoPositions.transferServo_in); robot.setTransferServoPos(ServoPositions.transferServo_in);
if(shootStateTime() > 300){ if (shootStateTime() > 300) {
setShootState( setShootState(
SortedShootState.RETRACT_3 SortedShootState.RETRACT_3
@@ -605,7 +605,7 @@ public class SpindexerTransferIntake {
robot.setTransferServoPos(ServoPositions.transferServo_out); robot.setTransferServoPos(ServoPositions.transferServo_out);
if(shootStateTime() > 150){ if (shootStateTime() > 150) {
setShootState( setShootState(
SortedShootState.DONE SortedShootState.DONE
@@ -636,11 +636,9 @@ public class SpindexerTransferIntake {
break; break;
} }
break; break;
} }
} }