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

Add shoot all ball order

Browse Source
This commit is contained in:
Matt9150
2026-01-25 11:33:56 -06:00
parent 7625f9a640
commit 7ffc51f60a
4 changed files with 89 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java
View File

@@ -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;
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Types.java Normal file
View File

@@ -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
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java
View File

@@ -1,4 +0,0 @@
package org.firstinspires.ftc.teamcode.constants;
public class blank {
}

87
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -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()
{
}