From b9f169b685e1b3349f449a0882dd5995215c55cb Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Thu, 12 Feb 2026 20:06:00 -0600 Subject: [PATCH] stash --- .../ftc/teamcode/utils/Spindexer.java | 260 ++++++------------ 1 file changed, 89 insertions(+), 171 deletions(-) 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 f395b14..64b1468 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 @@ -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){