Added sorted modes and shoot
This commit is contained in:
@@ -1,8 +1,7 @@
|
|||||||
package org.firstinspires.ftc.teamcode.utilsv2;
|
package org.firstinspires.ftc.teamcode.utilsv2;
|
||||||
|
|
||||||
import android.health.connect.datatypes.units.Velocity;
|
|
||||||
|
|
||||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||||
|
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
|
||||||
|
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||||
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
|
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
|
||||||
@@ -18,12 +17,167 @@ public class SpindexerTransferIntake {
|
|||||||
this.commander = com;
|
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 double sensorDistanceThreshold = 6.0;
|
||||||
private final long pulseTime = 100; // ms
|
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 {
|
public enum SpindexerMode {
|
||||||
RAPID,
|
RAPID,
|
||||||
SORTED
|
SORTED,
|
||||||
|
SHOOT_SORTED
|
||||||
|
}
|
||||||
|
|
||||||
|
public enum BallStates {
|
||||||
|
GREEN,
|
||||||
|
PURPLE,
|
||||||
|
UNKNOWN
|
||||||
}
|
}
|
||||||
|
|
||||||
public enum RapidMode {
|
public enum RapidMode {
|
||||||
@@ -39,11 +193,16 @@ public class SpindexerTransferIntake {
|
|||||||
|
|
||||||
private SpindexerMode mode = SpindexerMode.RAPID;
|
private SpindexerMode mode = SpindexerMode.RAPID;
|
||||||
private RapidMode rapidMode = RapidMode.INTAKE;
|
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.
|
* Time when current state was entered.
|
||||||
*/
|
*/
|
||||||
private long stateStartTime = System.currentTimeMillis();
|
private long stateStartTime = System.currentTimeMillis();
|
||||||
|
private long sortedStateStartTime = System.currentTimeMillis();
|
||||||
|
|
||||||
public void setRapidMode(RapidMode newMode) {
|
public void setRapidMode(RapidMode newMode) {
|
||||||
if (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) {
|
public void setSpindexerMode(SpindexerMode spindexerMode) {
|
||||||
this.mode = spindexerMode;
|
this.mode = spindexerMode;
|
||||||
}
|
}
|
||||||
@@ -64,6 +230,10 @@ public class SpindexerTransferIntake {
|
|||||||
return System.currentTimeMillis() - stateStartTime;
|
return System.currentTimeMillis() - stateStartTime;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private long sortedStateTime() {
|
||||||
|
return System.currentTimeMillis() - sortedStateStartTime;
|
||||||
|
}
|
||||||
|
|
||||||
public void update() {
|
public void update() {
|
||||||
|
|
||||||
if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){
|
if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){
|
||||||
@@ -179,7 +349,282 @@ public class SpindexerTransferIntake {
|
|||||||
|
|
||||||
case SORTED:
|
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;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user