diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java index 6146119..966316e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java @@ -21,4 +21,6 @@ public class ShooterVars { // VELOCITY CONSTANTS public static int AUTO_CLOSE_VEL = 3175; //3300; public static int AUTO_FAR_VEL = 4000; //TODO: test this + + public static Types.Motif currentMotif = Types.Motif.NONE; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Types.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Types.java new file mode 100644 index 0000000..c619eb1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Types.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode.constants; + +public class Types { + public enum Motif { + NONE, + GPP, // Green, Purple, Purple + PGP, // Purple, Green, Purple + PPG // Purple, Purple, Green + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java deleted file mode 100644 index 53e7fcb..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - -public class blank { -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index 3d95256..60616b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -16,9 +16,11 @@ import static org.firstinspires.ftc.teamcode.utils.Servos.spinI; import static org.firstinspires.ftc.teamcode.utils.Servos.spinP; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.constants.Types; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; public class Spindexer { + Robot robot; Servos servos; Flywheel flywheel; @@ -36,6 +38,7 @@ public class Spindexer { public double distanceFrontDriver = 0.0; public double distanceFrontPassenger = 0.0; + public Types.Motif desiredMotif = Types.Motif.NONE; // For Use enum RotatedBallPositionNames { REARCENTER, @@ -62,6 +65,8 @@ public class Spindexer { SHOOTNEXT, SHOOTMOVING, SHOOTWAIT, + SHOOT_ALL_PREP, + SHOOT_ALL_READY }; public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; @@ -327,6 +332,7 @@ public class Spindexer { } if (currentIntakeState != Spindexer.IntakeState.MOVING) { // Full + commandedIntakePosition = bestFitMotif(); currentIntakeState = Spindexer.IntakeState.FULL; } moveSpindexerToPos(intakePositions[commandedIntakePosition]); @@ -355,6 +361,28 @@ public class Spindexer { moveSpindexerToPos(intakePositions[commandedIntakePosition]); break; + case SHOOT_ALL_PREP: + // We get here with function call to prepareToShootMotif + // Stopping when we get to the new position + if (servos.spinEqual(intakePositions[commandedIntakePosition])) { + currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_READY; + } else { + // Keep moving the spindexer + moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + } + break; + + case SHOOT_ALL_READY: + // Double Check Colors + detectBalls(false, false); // Minimize hardware calls + if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) { + // All ball shot move to intake state + currentIntakeState = Spindexer.IntakeState.FINDNEXT; + } + // Maintain Position + moveSpindexerToPos(intakePositions[commandedIntakePosition]); + break; + case SHOOTNEXT: // Find Next Open Position and start movement if (!ballPositions[0].isEmpty) { @@ -383,16 +411,6 @@ public class Spindexer { // Stopping when we get to the new position if (servos.spinEqual(outakePositions[commandedIntakePosition])) { currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; - ballPositions[commandedIntakePosition].isEmpty = true; - // Advance to next full position and wait -// commandedIntakePosition++; -// if (commandedIntakePosition > 2) { -// commandedIntakePosition = 0; -// } -// // Continue moving to next position -// servos.setSpinPos(intakePositions[commandedIntakePosition]); -// currentIntakeState = Spindexer.IntakeState.MOVING; - } else { // Keep moving the spindexer moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" @@ -420,6 +438,55 @@ public class Spindexer { return false; } + public void setDesiredMotif (Types.Motif newMotif) { + desiredMotif = newMotif; + } + + // Returns the best fit for the motiff + public int bestFitMotif () { + switch (desiredMotif) { + case GPP: + if (ballPositions[0].ballColor == BallColor.GREEN) { + return 2; + } else if (ballPositions[1].ballColor == BallColor.GREEN) { + return 0; + } else { + return 1; + } + //break; + case PGP: + if (ballPositions[0].ballColor == BallColor.GREEN) { + return 0; + } else if (ballPositions[1].ballColor == BallColor.GREEN) { + return 1; + } else { + return 3; + } + //break; + case PPG: + if (ballPositions[0].ballColor == BallColor.GREEN) { + return 1; + } else if (ballPositions[1].ballColor == BallColor.GREEN) { + return 0; + } else { + return 2; + } + //break; + case NONE: + return 0; + //break; + } + return 0; + } + + void prepareToShootMotif () { + commandedIntakePosition = bestFitMotif(); + } + + void shootAllToIntake () { + currentIntakeState = Spindexer.IntakeState.FINDNEXT; + } + public void update() { }