From cfd09df8a07cb8cf1e9a3afd8e42460a5d407cbf Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Mon, 19 Jan 2026 11:11:22 -0600 Subject: [PATCH] Working Spindexer prototype with original shoot all functionality. --- .../teamcode/constants/ServoPositions.java | 4 +- .../ftc/teamcode/teleop/TeleopV3.java | 66 ++++- .../ftc/teamcode/utils/Spindexer.java | 275 ++++++++++++++++++ 3 files changed, 338 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index da99606..b0a886f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -7,9 +7,9 @@ public class ServoPositions { public static double spindexer_intakePos1 = 0.39; - public static double spindexer_intakePos2 = 0.5; + public static double spindexer_intakePos2 = 0.55;//0.5; - public static double spindexer_intakePos3 = 0.66; + public static double spindexer_intakePos3 = 0.71;//0.66; public static double spindexer_outtakeBall3 = 0.47; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 498f49c..3e1f2ce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -32,6 +32,7 @@ import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; +import org.firstinspires.ftc.teamcode.utils.Spindexer; import java.util.ArrayList; import java.util.List; @@ -70,6 +71,7 @@ public class TeleopV3 extends LinearOpMode { Servos servo; Flywheel flywheel; MecanumDrive drive; + Spindexer spindexer; double autoHoodOffset = 0.0; int shooterTicker = 0; @@ -94,7 +96,8 @@ public class TeleopV3 extends LinearOpMode { boolean shootA = true; boolean shootB = true; boolean shootC = true; - boolean autoSpintake = true; + boolean autoSpintake = false; + boolean enableSpindexerManager = true; List shootOrder = new ArrayList<>(); boolean outtake1 = false; boolean outtake2 = false; @@ -142,6 +145,7 @@ public class TeleopV3 extends LinearOpMode { servo = new Servos(hardwareMap); flywheel = new Flywheel(hardwareMap); drive = new MecanumDrive(hardwareMap, teleStart); + spindexer = new Spindexer(hardwareMap); PIDFController tController = new PIDFController(tp, ti, td, tf); @@ -529,7 +533,7 @@ public class TeleopV3 extends LinearOpMode { // } // } - if (gamepad1.left_bumper) { + if (gamepad1.left_bumper && !enableSpindexerManager) { robot.transferServo.setPosition(transferServo_out); @@ -560,14 +564,14 @@ public class TeleopV3 extends LinearOpMode { } - if (gamepad1.leftBumperWasReleased()) { + if (gamepad1.leftBumperWasReleased() && !enableSpindexerManager) { shootStamp = getRuntime(); shootAll = true; shooterTicker = 0; } - if (shootAll) { + if (shootAll && !enableSpindexerManager) { TELE.addData("100% works", shootOrder); @@ -765,7 +769,57 @@ public class TeleopV3 extends LinearOpMode { // } //EXTRA STUFFINESS: + if (enableSpindexerManager) { + if (!shootAll) { + spindexer.processIntake(); + } + // RIGHT_BUMPER + if (gamepad1.right_bumper) { + robot.intake.setPower(1); + + } else { + robot.intake.setPower(0); + } + + // LEFT_BUMPER + if (gamepad1.leftBumperWasReleased()) { + shootStamp = getRuntime(); + shootAll = true; + + shooterTicker = 0; + } + + if (shootAll) { + + TELE.addData("100% works", shootOrder); + + intake = false; + reject = false; + + shooterTicker++; + + spindexPos = spindexer_intakePos1; + + if (getRuntime() - shootStamp < 3.5) { + + robot.transferServo.setPosition(transferServo_in); + + robot.spin1.setPower(-spinPow); + robot.spin2.setPower(spinPow); + + } else { + robot.transferServo.setPosition(transferServo_out); + spindexPos = spindexer_intakePos1; + + shootAll = false; + + robot.transferServo.setPosition(transferServo_out); + + spindexer.resetSpindexer(); + } + } + } drive.updatePoseEstimate(); for (LynxModule hub : allHubs) { @@ -786,9 +840,11 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("oddColor", oddBallColor); TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1)); + TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition); + TELE.addData("spinIntakeState", spindexer.currentIntakeState); + TELE.addData("spinTestCounter", spindexer.counter); TELE.addData("autoSpintake", autoSpintake); TELE.addData("timeSinceStamp", getRuntime() - shootStamp); - TELE.update(); ticker++; 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 new file mode 100644 index 0000000..3aeeaa2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -0,0 +1,275 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.arcrobotics.ftclib.controller.PIDFController; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; +import static org.firstinspires.ftc.teamcode.utils.Servos.spinD; +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 org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; + +public class Spindexer { + 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; + + enum IntakeState { + UNKNOWN, + INTAKE, + FINDNEXT, + MOVING, + FULL + }; + + public IntakeState currentIntakeState = IntakeState.UNKNOWN; + + enum BallColor { + UNKNOWN, + GREEN, + PURPLE + }; + + class BallPosition { + boolean isEmpty = true; + BallColor ballColor = BallColor.UNKNOWN; + } + + BallPosition[] ballPositions = new BallPosition[3]; + + public boolean init () { + return true; + } + + public Spindexer(HardwareMap hardwareMap) { + robot = new Robot(hardwareMap); + servos = new Servos(hardwareMap); + flywheel = new Flywheel(hardwareMap); + //TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + lastKnownSpinPos = servos.getSpinPos(); + + ballPositions[0] = new BallPosition(); + ballPositions[1] = new BallPosition(); + ballPositions[2] = new BallPosition(); + + + } + + + double[] outakePositions = + {spindexer_outtakeBall1, spindexer_outtakeBall2, spindexer_outtakeBall3}; + + 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 resetSpindexer () { + for (int i = 0; i < 3; i++) { + ballPositions[i].isEmpty = true; + ballPositions[i].ballColor = BallColor.UNKNOWN; + } + currentIntakeState = IntakeState.UNKNOWN; + } + + // Detects if a ball is found and what color. + // Returns true is there was a new ball found in Position 1 + public boolean detectBalls() { + + boolean newPos1Detection = false; + + // Read Distances + double s1D = robot.color1.getDistance(DistanceUnit.MM); + double s2D = robot.color2.getDistance(DistanceUnit.MM); + double s3D = robot.color3.getDistance(DistanceUnit.MM); + + // Position 1 + if (s1D < 43) { + + // Mark Ball Found + newPos1Detection = ballPositions[commandedIntakePosition].isEmpty; + + // Detect which color + double green = robot.color1.getNormalizedColors().green; + double red = robot.color1.getNormalizedColors().red; + double blue = robot.color1.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + // FIXIT - Add filtering to improve accuracy. + if (gP >= 0.4) { + ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple + } else { + ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple + } + } +// FIXIT - Need to find Position base on rotation +// // Position 2 +// if (s2D < 60) { +// +// double green = robot.color2.getNormalizedColors().green; +// double red = robot.color2.getNormalizedColors().red; +// double blue = robot.color2.getNormalizedColors().blue; +// +// double gP = green / (green + red + blue); +// +// if (gP >= 0.4) { +// b2 = 2; // purple +// } else { +// b2 = 1; // green +// } +// } +// +// // Position 3 +// if (s3D < 33) { +// +// double green = robot.color3.getNormalizedColors().green; +// double red = robot.color3.getNormalizedColors().red; +// double blue = robot.color3.getNormalizedColors().blue; +// +// double gP = green / (green + red + blue); +// +// if (gP >= 0.4) { +// b3 = 2; // purple +// } else { +// b3 = 1; // green +// } +// } + +// TELE.addData("Velocity", velo); +// TELE.addLine("Detecting"); +// TELE.addData("Distance 1", s1D); +// TELE.addData("Distance 2", s2D); +// TELE.addData("Distance 3", s3D); +// TELE.addData("B1", b1); +// TELE.addData("B2", b2); +// TELE.addData("B3", b3); +// TELE.update(); + + return newPos1Detection; + } + + public void moveSpindexerToPos(double pos) { + spinCurrentPos = servos.getSpinPos(); + + double spindexPID = spinPID.calculate(spinCurrentPos, pos); + + robot.spin1.setPower(spindexPID); + robot.spin2.setPower(-spindexPID); + } + + public void stopSpindexer() { + robot.spin1.setPower(0); + robot.spin2.setPower(0); + } + + public boolean isFull () { + return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty); + } + public boolean processIntake() { + + switch (currentIntakeState) { + case UNKNOWN: + // For now just set position ONE if UNKNOWN + commandedIntakePosition = 0; + servos.setSpinPos(intakePositions[0]); + currentIntakeState = Spindexer.IntakeState.MOVING; + break; + case INTAKE: + // Ready for intake and Detecting a New Ball + if (detectBalls()) { + ballPositions[commandedIntakePosition].isEmpty = false; + currentIntakeState = Spindexer.IntakeState.FINDNEXT; + } else { + // Maintain Position + moveSpindexerToPos(intakePositions[commandedIntakePosition]); + } + break; + case FINDNEXT: + // Find Next Open Position and start movement + if (ballPositions[0].isEmpty) { + // Position 1 + commandedIntakePosition = 0; + servos.setSpinPos(intakePositions[commandedIntakePosition]); + currentIntakeState = Spindexer.IntakeState.MOVING; + } else if (ballPositions[1].isEmpty) { + // Position 2 + commandedIntakePosition = 1; + servos.setSpinPos(intakePositions[commandedIntakePosition]); + currentIntakeState = Spindexer.IntakeState.MOVING; + } else if (ballPositions[2].isEmpty) { + // Position 3 + commandedIntakePosition = 2; + servos.setSpinPos(intakePositions[commandedIntakePosition]); + currentIntakeState = Spindexer.IntakeState.MOVING; + } else { + // Full + currentIntakeState = Spindexer.IntakeState.FULL; + } + moveSpindexerToPos(intakePositions[commandedIntakePosition]); + break; + + case MOVING: + // Stopping when we get to the new position + if (servos.spinEqual(intakePositions[commandedIntakePosition])) { + currentIntakeState = Spindexer.IntakeState.INTAKE; + stopSpindexer(); + detectBalls(); + } else { + // Keep moving the spindexer + moveSpindexerToPos(intakePositions[commandedIntakePosition]); + } + break; + + case FULL: + // Double Check Colors? + // Maintain Position + moveSpindexerToPos(intakePositions[commandedIntakePosition]); + + default: + // Statements to execute if no case matches + } + //TELE.addData("commandedIntakePosition", commandedIntakePosition); + //TELE.update(); + // Signal a successful intake + return false; + } + + public void update() + { + } +}