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

stash

Browse Source
This commit is contained in:
Keshav Anand
2026-02-12 20:06:00 -06:00
parent 754cb6c622
commit b9f169b685
1 changed files with 89 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,14 +7,6 @@ import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import static org.firstinspires.ftc.teamcode.constants.Color.*;
import static org.firstinspires.ftc.teamcode.constants.Color.Light0;
import static org.firstinspires.ftc.teamcode.constants.Color.Light1;
import static org.firstinspires.ftc.teamcode.constants.Color.Light2;
import static org.firstinspires.ftc.teamcode.constants.Color.Light3;
import static org.firstinspires.ftc.teamcode.constants.Color.LightGreen;
import static org.firstinspires.ftc.teamcode.constants.Color.LightOrange;
import static org.firstinspires.ftc.teamcode.constants.Color.LightPurple;
import static org.firstinspires.ftc.teamcode.constants.Color.colorFilterAlpha;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
@@ -29,30 +21,36 @@ import static org.firstinspires.ftc.teamcode.utils.Servos.spinF;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinI;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinP;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.StateEnums;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import java.util.Objects;
public class Spindexer {
// Array of commandedIntakePositions with contents
// {RearCenter, FrontDriver, FrontPassenger}
static final int[][] RotatedBallPositions = {{0, 2, 1}, {1, 0, 2}, {2, 1, 0}};
Robot robot;
Servos servos;
Flywheel flywheel;
MecanumDrive drive;
double lastKnownSpinPos = 0.0;
MultipleTelemetry TELE;
PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
double spinCurrentPos = 0.0;
public int commandedIntakePosition = 0;
public double distanceRearCenter = 0.0;
public double distanceFrontDriver = 0.0;
public double distanceFrontPassenger = 0.0;
public double spindexerWiggle = 0.01;
public double spindexerOuttakeWiggle = 0.01;
private double prevPos = 0.0;
public double spindexerPosOffset = 0.00;
public static int shootWaitMax = 4;
public Types.Motif desiredMotif = Types.Motif.NONE;
public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE;
// For Use
enum RotatedBallPositionNames {
REARCENTER,
@@ -87,29 +85,26 @@ public class Spindexer {
int shootWaitCount = 0;
public StateEnums.Motif desiredMotif = StateEnums.Motif.NONE;
public IntakeState currentIntakeState = IntakeState.UNKNOWN_START;
public IntakeState prevIntakeState = IntakeState.UNKNOWN_START;
public int unknownColorDetect = 0;
public int counter = 0;
Robot robot;
Servos servos;
Flywheel flywheel;
MecanumDrive drive;
double lastKnownSpinPos = 0.0;
MultipleTelemetry TELE;
PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF);
double spinCurrentPos = 0.0;
int shootWaitCount = 0;
public enum BallColor {
UNKNOWN,
GREEN,
PURPLE
}
class BallPosition {
boolean isEmpty = true;
int foundEmpty = 0;
BallColor ballColor = BallColor.UNKNOWN;
}
BallPosition[] ballPositions = new BallPosition[3];
double[] outakePositions =
{spindexer_outtakeBall1 + spindexerPosOffset,
spindexer_outtakeBall2 + spindexerPosOffset,
spindexer_outtakeBall3 + spindexerPosOffset};
double[] intakePositions =
{spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3};
private double prevPos = 0.0;
private double intakeTicker = 0;
public boolean init () {
return true;
}
public Spindexer(HardwareMap hardwareMap) {
robot = new Robot(hardwareMap);
@@ -123,13 +118,36 @@ public class Spindexer {
ballPositions[1] = new BallPosition();
ballPositions[2] = new BallPosition();
}
public boolean init() {
return true;
}
public void resetBallPosition(int pos) {
double[] outakePositions =
{spindexer_outtakeBall1+spindexerPosOffset,
spindexer_outtakeBall2+spindexerPosOffset,
spindexer_outtakeBall3+spindexerPosOffset};
double[] intakePositions =
{spindexer_intakePos1, spindexer_intakePos2, spindexer_intakePos3};
public int counter = 0;
// private double getTimeSeconds ()
// {
// return (double) System.currentTimeMillis()/1000.0;
// }
// public double getPos() {
// robot.spin1Pos.getVoltage();
// robot.spin1Pos.getMaxVoltage();
// return (robot.spin1Pos.getVoltage()/robot.spin1Pos.getMaxVoltage());
// }
// public void manageSpindexer() {
//
// }
public void resetBallPosition (int pos) {
ballPositions[pos].isEmpty = true;
ballPositions[pos].foundEmpty = 0;
ballPositions[pos].ballColor = BallColor.UNKNOWN;
@@ -138,7 +156,7 @@ public class Spindexer {
distanceFrontPassenger = 61;
}
public void resetSpindexer() {
public void resetSpindexer () {
for (int i = 0; i < 3; i++) {
resetBallPosition(i);
}
@@ -155,11 +173,11 @@ public class Spindexer {
// Read Distances
double dRearCenter = robot.color1.getDistance(DistanceUnit.MM);
distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1 - colorFilterAlpha) * distanceRearCenter);
distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1-colorFilterAlpha) * distanceRearCenter);
double dFrontDriver = robot.color2.getDistance(DistanceUnit.MM);
distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1 - colorFilterAlpha) * distanceFrontDriver);
distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1-colorFilterAlpha) * distanceFrontDriver);
double dFrontPassenger = robot.color3.getDistance(DistanceUnit.MM);
distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1 - colorFilterAlpha) * distanceFrontPassenger);
distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
// Position 1
if (distanceRearCenter < 60) {
@@ -246,7 +264,6 @@ public class Spindexer {
return newPos1Detection;
}
// Has code to unjam spindexer
private void moveSpindexerToPos(double pos) {
servos.setSpinPos(pos);
@@ -263,69 +280,20 @@ public class Spindexer {
// prevPos = pos;
}
// private double getTimeSeconds ()
// {
// return (double) System.currentTimeMillis()/1000.0;
// }
// public double getPos() {
// robot.spin1Pos.getVoltage();
// robot.spin1Pos.getMaxVoltage();
// return (robot.spin1Pos.getVoltage()/robot.spin1Pos.getMaxVoltage());
// }
// public void manageSpindexer() {
//
// }
public void stopSpindexer() {
}
private double prevLight = 0.0;
public void ballCounterLight(){
public double getRearCenterLight() {
BallColor color = GetRearCenterColor();
if (Objects.equals(color, BallColor.GREEN)) {
return LightGreen;
} else if (Objects.equals(color, BallColor.PURPLE)) {
return LightPurple;
} else {
return LightOrange;
}
}
public double getDriverLight() {
BallColor color = GetFrontDriverColor();
if (Objects.equals(color, BallColor.GREEN)) {
return LightGreen;
} else if (Objects.equals(color, BallColor.PURPLE)) {
return LightPurple;
} else {
return LightOrange;
}
}
public double getPassengerLight() {
BallColor color = GetFrontPassengerColor();
if (Objects.equals(color, BallColor.GREEN)) {
return LightGreen;
} else if (Objects.equals(color, BallColor.PURPLE)) {
return LightPurple;
} else {
return LightOrange;
}
}
public double ballCounterLight() {
int counter = 0;
if (!ballPositions[0].isEmpty) {
if (!ballPositions[0].isEmpty){
counter++;
}
if (!ballPositions[1].isEmpty) {
if (!ballPositions[1].isEmpty){
counter++;
}
if (!ballPositions[2].isEmpty) {
if (!ballPositions[2].isEmpty){
counter++;
}
@@ -336,16 +304,8 @@ public class Spindexer {
light = Light2;
} else if (counter == 1){
light = Light1;
if (counter == 3) {
return Light3;
} else if (counter == 2) {
return Light2;
} else if (counter == 1) {
return Light1;
} else {
light = Light0;
return Light0;
}
if (light != prevLight){
robot.light.setPosition(light);
@@ -353,14 +313,15 @@ public class Spindexer {
prevLight = light;
}
public boolean slotIsEmpty(int slot) {
public boolean slotIsEmpty(int slot){
return !ballPositions[slot].isEmpty;
}
public boolean isFull() {
public boolean isFull () {
return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty);
}
private double intakeTicker = 0;
public boolean processIntake() {
switch (currentIntakeState) {
@@ -382,7 +343,7 @@ public class Spindexer {
}
break;
case UNKNOWN_DETECT:
if (unknownColorDetect > 5) {
if (unknownColorDetect >5) {
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
} else {
//detectBalls(true, true);
@@ -435,7 +396,7 @@ public class Spindexer {
case MOVING:
// Stopping when we get to the new position
if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
if (intakeTicker > 1) {
if (intakeTicker > 1){
currentIntakeState = Spindexer.IntakeState.INTAKE;
stopSpindexer();
intakeTicker = 0;
@@ -516,7 +477,7 @@ public class Spindexer {
case SHOOTWAIT:
// Stopping when we get to the new position
if (prevIntakeState != currentIntakeState) {
if (commandedIntakePosition == 2) {
if (commandedIntakePosition==2) {
shootWaitMax = 5;
}
shootWaitCount = 0;
@@ -537,7 +498,7 @@ public class Spindexer {
break;
case SHOOT_PREP_CONTINOUS:
if (servos.spinEqual(spinStartPos)) {
if (servos.spinEqual(spinStartPos)){
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
} else {
servos.setSpinPos(spinStartPos);
@@ -548,7 +509,7 @@ public class Spindexer {
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
if (servos.getSpinPos() > spinEndPos) {
if (servos.getSpinPos() > spinEndPos){
currentIntakeState = IntakeState.FINDNEXT;
} else {
double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
@@ -570,12 +531,12 @@ public class Spindexer {
return false;
}
public void setDesiredMotif(StateEnums.Motif newMotif) {
public void setDesiredMotif (StateEnums.Motif newMotif) {
desiredMotif = newMotif;
}
// Returns the best fit for the motiff
public int bestFitMotif() {
public int bestFitMotif () {
switch (desiredMotif) {
case GPP:
if (ballPositions[0].ballColor == BallColor.GREEN) {
@@ -611,33 +572,31 @@ public class Spindexer {
return 0;
}
void prepareToShootMotif() {
void prepareToShootMotif () {
commandedIntakePosition = bestFitMotif();
}
public void prepareShootAll() {
public void prepareShootAll(){
currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP;
}
public void prepareShootAllContinous() {
public void prepareShootAllContinous(){
currentIntakeState = Spindexer.IntakeState.SHOOT_PREP_CONTINOUS;
}
public void shootAll() {
public void shootAll () {
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
currentIntakeState = Spindexer.IntakeState.SHOOTNEXT;
}
public void shootAllContinous() {
public void shootAllContinous(){
ballPositions[0].isEmpty = false;
ballPositions[1].isEmpty = false;
ballPositions[2].isEmpty = false;
currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS;
}
public boolean shootAllComplete() {
public boolean shootAllComplete ()
{
return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) &&
(currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) &&
(currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) &&
@@ -647,66 +606,25 @@ public class Spindexer {
(currentIntakeState != Spindexer.IntakeState.SHOOT_CONTINOUS));
}
void shootAllToIntake() {
void shootAllToIntake () {
currentIntakeState = Spindexer.IntakeState.FINDNEXT;
}
public void update() {
public void update()
{
}
public BallColor GetFrontDriverColor() {
public BallColor GetFrontDriverColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]].ballColor;
}
public BallColor GetFrontPassengerColor() {
public BallColor GetFrontPassengerColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]].ballColor;
}
public BallColor GetRearCenterColor() {
public BallColor GetRearCenterColor () {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
}
// For Use
enum RotatedBallPositionNames {
REARCENTER,
FRONTDRIVER,
FRONTPASSENGER
}
enum IntakeState {
UNKNOWN_START,
UNKNOWN_MOVE,
UNKNOWN_DETECT,
INTAKE,
FINDNEXT,
MOVING,
FULL,
SHOOTNEXT,
SHOOTMOVING,
SHOOTWAIT,
SHOOT_ALL_PREP,
SHOOT_ALL_READY,
SHOOT_PREP_CONTINOUS,
SHOOT_CONTINOUS
}
public enum BallColor {
UNKNOWN,
GREEN,
PURPLE
}
class spindexerBallRoatation {
int rearCenter = 0; // aka commanded Position
int frontDriver = 0;
int frontPassenger = 0;
}
class BallPosition {
boolean isEmpty = true;
int foundEmpty = 0;
BallColor ballColor = BallColor.UNKNOWN;
}
private double prevPow = 0.501;
public void setIntakePower(double pow){
if (prevPow != 0.501 && prevPow != pow){