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

Added sorted modes and shoot

Browse Source
This commit is contained in:
Keshav Anand
2026-06-04 18:10:31 -05:00
parent deefa19be4
commit 9a4aca90ba
1 changed files with 449 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,8 +1,7 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import android.health.connect.datatypes.units.Velocity;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
@@ -18,12 +17,167 @@ public class SpindexerTransferIntake {
this.commander = com;
}
public enum DesiredPattern {
PPG,
PGP,
GPP
}
public enum SortedShootState {
IDLE,
MOVE_TO_1,
WAIT_FOR_1,
SHOOT_1,
RETRACT_1,
MOVE_TO_2,
WAIT_FOR_2,
SHOOT_2,
RETRACT_2,
MOVE_TO_3,
WAIT_FOR_3,
SHOOT_3,
RETRACT_3,
DONE
}
int[] shootOrder = {0,1,2};
private final double sensorDistanceThreshold = 6.0;
private final long pulseTime = 100; // ms
private DesiredPattern desiredPattern = DesiredPattern.GPP;
private SortedShootState shootState = SortedShootState.IDLE;
private long shootStateStartTime = System.currentTimeMillis();
private void setShootState(SortedShootState newState){
shootState = newState;
shootStateStartTime = System.currentTimeMillis();
}
private long shootStateTime(){
return System.currentTimeMillis() - shootStateStartTime;
}
private int[] buildShootOrder(
BallStates[] loaded,
DesiredPattern desired) {
BallStates[] target;
switch (desired) {
case PPG:
target = new BallStates[]{
BallStates.PURPLE,
BallStates.PURPLE,
BallStates.GREEN
};
break;
case PGP:
target = new BallStates[]{
BallStates.PURPLE,
BallStates.GREEN,
BallStates.PURPLE
};
break;
default: // GPP
target = new BallStates[]{
BallStates.GREEN,
BallStates.PURPLE,
BallStates.PURPLE
};
}
int[] order = new int[3];
boolean[] used = new boolean[3];
// first pass: exact color matches
for (int i = 0; i < 3; i++) {
order[i] = -1;
for (int slot = 0; slot < 3; slot++) {
if (!used[slot]
&& loaded[slot] == target[i]) {
order[i] = slot;
used[slot] = true;
break;
}
}
}
// second pass: fill leftovers
for (int i = 0; i < 3; i++) {
if (order[i] != -1)
continue;
for (int slot = 0; slot < 3; slot++) {
if (!used[slot]) {
order[i] = slot;
used[slot] = true;
break;
}
}
}
return order;
}
private void moveToSlot(int slot) {
switch(slot) {
case 0:
robot.setSpinPos(
ServoPositions.spindexer_A1
);
break;
case 1:
robot.setSpinPos(
ServoPositions.spindexer_A2
);
break;
case 2:
robot.setSpinPos(
ServoPositions.spindexer_A3
);
break;
}
}
public enum SortedIntakeStates {
NOTHING,
IDLE,
INTAKE_1,
DELAY_1,
INTAKE_2,
DELAY_2,
INTAKE_3,
REVERSE,
}
public enum SpindexerMode {
RAPID,
SORTED
SORTED,
SHOOT_SORTED
}
public enum BallStates {
GREEN,
PURPLE,
UNKNOWN
}
public enum RapidMode {
@@ -39,11 +193,16 @@ public class SpindexerTransferIntake {
private SpindexerMode mode = SpindexerMode.RAPID;
private RapidMode rapidMode = RapidMode.INTAKE;
private SortedIntakeStates sortedIntakeStates = SortedIntakeStates.IDLE;
private BallStates[] ballColors = {BallStates.UNKNOWN, BallStates.UNKNOWN, BallStates.UNKNOWN};
private final double greenThresh = 0.40;
private final double spinMovementTime = 250;
/**
* Time when current state was entered.
*/
private long stateStartTime = System.currentTimeMillis();
private long sortedStateStartTime = System.currentTimeMillis();
public void setRapidMode(RapidMode newMode) {
if (rapidMode != newMode) {
@@ -52,6 +211,13 @@ public class SpindexerTransferIntake {
}
}
public void setSortedIntakeMode(SortedIntakeStates newMode) {
if (sortedIntakeStates != newMode) {
sortedIntakeStates = newMode;
sortedStateStartTime = System.currentTimeMillis();
}
}
public void setSpindexerMode(SpindexerMode spindexerMode) {
this.mode = spindexerMode;
}
@@ -64,6 +230,10 @@ public class SpindexerTransferIntake {
return System.currentTimeMillis() - stateStartTime;
}
private long sortedStateTime() {
return System.currentTimeMillis() - sortedStateStartTime;
}
public void update() {
if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){
@@ -179,7 +349,282 @@ public class SpindexerTransferIntake {
case SORTED:
// Future sorted-intake logic
switch (sortedIntakeStates){
case NOTHING:
break;
case IDLE:
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open
);
robot.setSpindexBlockerPos(
ServoPositions.spindexBlocker_Closed
);
robot.setSpinPos(
ServoPositions.spindexer_A1
);
robot.setTransferServoPos(
ServoPositions.transferServo_out
);
robot.setIntakePower(1);
robot.setTransferPower(-1);
if (sortedStateTime() > 200) {
setSortedIntakeMode(SortedIntakeStates.INTAKE_1);
}
break;
case INTAKE_1:
robot.setIntakePower(1);
robot.setTransferPower(-1);
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold){
NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
ballColors[0] = BallStates.GREEN;
} else {
ballColors[0] = BallStates.PURPLE;
}
robot.setSpinPos(ServoPositions.spindexer_A2);
setSortedIntakeMode(SortedIntakeStates.DELAY_1);
}
break;
case DELAY_1:
robot.setSpinPos(ServoPositions.spindexer_A2);
if (sortedStateTime() > spinMovementTime){
setSortedIntakeMode(SortedIntakeStates.INTAKE_2);
}
break;
case INTAKE_2:
robot.setIntakePower(1);
robot.setTransferPower(-1);
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold){
NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
ballColors[1] = BallStates.GREEN;
} else {
ballColors[1] = BallStates.PURPLE;
}
robot.setSpinPos(ServoPositions.spindexer_A3);
setSortedIntakeMode(SortedIntakeStates.DELAY_2);
}
break;
case DELAY_2:
robot.setSpinPos(
ServoPositions.spindexer_A3
);
if (sortedStateTime() > spinMovementTime){
setSortedIntakeMode(
SortedIntakeStates.INTAKE_3
);
}
break;
case INTAKE_3:
robot.setIntakePower(1);
robot.setTransferPower(-1);
if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold){
NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
if ((revColor.green / (revColor.red + revColor.blue + revColor.green)) > greenThresh) {
ballColors[2] = BallStates.GREEN;
} else {
ballColors[2] = BallStates.PURPLE;
}
setSortedIntakeMode(SortedIntakeStates.REVERSE);
}
break;
case REVERSE:
robot.setTransferPower(-0.3);
robot.setIntakePower(-0.1);
}
break;
case SHOOT_SORTED:
//TODO: ADD transfer intake powers here
switch (shootState){
case IDLE:
shootOrder = buildShootOrder(
ballColors,
desiredPattern
);
setShootState(SortedShootState.MOVE_TO_1);
mode = SpindexerMode.SHOOT_SORTED;
case MOVE_TO_1:
moveToSlot(shootOrder[0]);
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open
);
robot.setSpindexBlockerPos(
ServoPositions.spindexBlocker_Closed
);
setShootState(
SortedShootState.WAIT_FOR_1
);
break;
case WAIT_FOR_1:
if(shootStateTime() > 250){
setShootState(
SortedShootState.SHOOT_1
);
}
break;
case SHOOT_1:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
robot.setTransferServoPos(ServoPositions.transferServo_in);
if(shootStateTime() > 300){
setShootState(
SortedShootState.RETRACT_1
);
}
break;
case RETRACT_1:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed);
robot.setTransferServoPos(ServoPositions.transferServo_out);
if(shootStateTime() > 150){
setShootState(
SortedShootState.MOVE_TO_2
);
}
break;
case MOVE_TO_2:
moveToSlot(shootOrder[1]);
setShootState(
SortedShootState.WAIT_FOR_2
);
break;
case WAIT_FOR_2:
if(shootStateTime() > 250){
setShootState(
SortedShootState.SHOOT_2
);
}
break;
case SHOOT_2:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
robot.setTransferServoPos(ServoPositions.transferServo_in);
if(shootStateTime() > 300){
setShootState(
SortedShootState.RETRACT_2
);
}
break;
case RETRACT_2:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Closed);
robot.setTransferServoPos(ServoPositions.transferServo_out);
if(shootStateTime() > 150){
setShootState(
SortedShootState.MOVE_TO_3
);
}
break;
case MOVE_TO_3:
moveToSlot(shootOrder[2]);
setShootState(
SortedShootState.WAIT_FOR_3
);
break;
case WAIT_FOR_3:
if(shootStateTime() > 250){
setShootState(
SortedShootState.SHOOT_3
);
}
break;
case SHOOT_3:
robot.setSpindexBlockerPos(ServoPositions.spindexBlocker_Open);
robot.setTransferServoPos(ServoPositions.transferServo_in);
if(shootStateTime() > 300){
setShootState(
SortedShootState.RETRACT_3
);
}
break;
case RETRACT_3:
robot.setTransferServoPos(ServoPositions.transferServo_out);
if(shootStateTime() > 150){
setShootState(
SortedShootState.DONE
);
}
break;
case DONE:
robot.setRapidFireBlockerPos(
ServoPositions.rapidFireBlocker_Open
);
robot.setSpindexBlockerPos(
ServoPositions.spindexBlocker_Closed
);
robot.setSpinPos(
ServoPositions.spindexer_A1
);
robot.setTransferServoPos(
ServoPositions.transferServo_out
);
robot.setIntakePower(1);
robot.setTransferPower(-1);
setSortedIntakeMode(SortedIntakeStates.NOTHING);
mode = SpindexerMode.SORTED;
break;
}
break;
}
}