From ae25df03930cb94166c500e2c01e44716c484ccd Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Tue, 2 Jun 2026 17:08:26 -0500 Subject: [PATCH] Fixed spidnexer i think --- .../ftc/teamcode/teleop/TeleopV4.java | 9 +- .../ftc/teamcode/utilsv2/Drivetrain.java | 2 - .../ftc/teamcode/utilsv2/Intake.java | 20 --- .../ftc/teamcode/utilsv2/Robot.java | 11 +- .../utilsv2/SpindexerTransferIntake.java | 160 ++++++++++-------- 5 files changed, 98 insertions(+), 104 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index 7d80708..c280bd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -6,8 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.utilsv2.Drivetrain; -import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.*; @TeleOp @Config @@ -18,7 +17,7 @@ public class TeleopV4 extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); + robot = Robot.getInstance(hardwareMap); TELE = new MultipleTelemetry( FtcDashboard.getInstance().getTelemetry(), telemetry @@ -26,8 +25,6 @@ public class TeleopV4 extends LinearOpMode { drivetrain = new Drivetrain(robot, TELE); - drivetrain.setTelemetry(true); - waitForStart(); if (isStopRequested()) return; @@ -42,6 +39,8 @@ public class TeleopV4 extends LinearOpMode { gamepad1.left_stick_x ); + + TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java index 6047868..8623588 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java @@ -4,8 +4,6 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.hardware.DcMotor; -import org.firstinspires.ftc.teamcode.utils.Robot; - @Config public class Drivetrain { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java deleted file mode 100644 index ab240b4..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.firstinspires.ftc.teamcode.utilsv2; - -import org.firstinspires.ftc.teamcode.utilsv2.Robot; - -public class Intake { - - Robot rob; - public Intake(Robot robot) { - - this.rob = robot; - - } - - public void setIntakePower(double pow){ - rob.intake.setPower(pow); - } - public void setTransferPower(double pow){ - rob.transfer.setPower(pow); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java index c805842..58a54d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -63,9 +63,9 @@ public class Robot { public Servo turr2; public Servo spin1; public Servo spin2; - public TouchSensor beam1; - public TouchSensor beam2; - public TouchSensor beam3; + public TouchSensor insideBeam; + public TouchSensor outsideBeam; + public RevColorSensorV3 revSensor; public VoltageSensor voltage; @@ -139,9 +139,8 @@ public class Robot { tilt1 = hardwareMap.get(Servo.class, "tilt1"); tilt2 = hardwareMap.get(Servo.class, "tilt2"); -// beam1 = hardwareMap.get(TouchSensor.class, "beam1"); -// beam2 = hardwareMap.get(TouchSensor.class, "beam2"); -// beam3 = hardwareMap.get(TouchSensor.class, "beam3"); + insideBeam = hardwareMap.get(TouchSensor.class, "beam1"); + outsideBeam = hardwareMap.get(TouchSensor.class, "beam2"); revSensor = hardwareMap.get(RevColorSensorV3.class, "rev"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java index 950fdfc..fefb67d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -1,161 +1,179 @@ package org.firstinspires.ftc.teamcode.utilsv2; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; public class SpindexerTransferIntake { - private Servo spin1; - private Servo spin2; + private final Robot robot; public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) { - this.spin1 = rob.spin1; - this.spin2 = rob.spin2; - + this.robot = rob; } - enum SpindexerMode { + private final double sensorDistanceThreshold = 4.0; + private final long pulseTime = 50; // ms + + public enum SpindexerMode { RAPID, SORTED } - enum RapidMode { - INTAKE, // Normal intake operation - TRANSFER_OFF, // Slow transfer, waiting for rings to settle - BEFORE_PULSE_OUT, // Brief forward delay before pulse - PULSE_OUT, // Small reverse pulse to unjam/reposition rings - PULSE_IN, // Feed rings back forward - HOLD_BALLS, // Maintain ring position - OPEN_GATE, // Open shooter gate - SHOOT // Feed ring into shooter + public enum RapidMode { + INTAKE, + TRANSFER_OFF, + BEFORE_PULSE_OUT, + PULSE_OUT, + PULSE_IN, + HOLD_BALLS, + OPEN_GATE, + SHOOT } private SpindexerMode mode = SpindexerMode.RAPID; - private RapidMode rapidMode = RapidMode.INTAKE; - public void setRapidMode (RapidMode rMode) { - this.rapidMode = rMode; + /** + * Time when current state was entered. + */ + private long stateStartTime = System.currentTimeMillis(); + + public void setRapidMode(RapidMode newMode) { + if (rapidMode != newMode) { + rapidMode = newMode; + stateStartTime = System.currentTimeMillis(); + } } - public void setSpindexerMode (SpindexerMode spindexerMode){ + public void setSpindexerMode(SpindexerMode spindexerMode) { this.mode = spindexerMode; } + private long stateTime() { + return System.currentTimeMillis() - stateStartTime; + } + public void update() { switch (mode) { case RAPID: + robot.setSpindexBlockerPos( + ServoPositions.spindexBlocker_Open + ); + switch (rapidMode) { case INTAKE: - // Run front intake - // Run transfer intake - // Keep shooter gate closed - // Keep kicker retracted - // Keep spindexer in default position + robot.setIntakePower(1); + robot.setTransferPower(1); + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Closed + ); + robot.setSpinPos( + ServoPositions.spindexer_A2 + ); + robot.setTransferServoPos( + ServoPositions.transferServo_out + ); - // If first beam break sees ring: - // rapidMode = TRANSFER_OFF; + if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { + + setRapidMode(RapidMode.TRANSFER_OFF); + } - // If shoot button pressed: - // rapidMode = OPEN_GATE; break; case TRANSFER_OFF: - // Slow down transfer intake - // Allow rings to settle into storage + robot.setTransferPower(0.3); - // If both beam breaks occupied: - // rapidMode = BEFORE_PULSE_OUT; - - // If shoot button pressed: - // rapidMode = OPEN_GATE; + if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) { + setRapidMode(RapidMode.BEFORE_PULSE_OUT); + } break; case BEFORE_PULSE_OUT: - // Continue running intake forward + robot.setIntakePower(1.0); - // After ~0.3 sec: - // rapidMode = PULSE_OUT; + if (stateTime() >= 300) { + setRapidMode(RapidMode.PULSE_OUT); + } break; case PULSE_OUT: - // Reverse intake slightly - // Helps separate rings and prevent jams + robot.setIntakePower(-0.1); - // After pulse time: - // rapidMode = PULSE_IN; + if (stateTime() >= pulseTime) { + setRapidMode(RapidMode.PULSE_IN); + } break; case PULSE_IN: - // Run intake forward again - // Re-seat rings after pulse + robot.setIntakePower(1.0); + + if (stateTime() >= 200) { + setRapidMode(RapidMode.HOLD_BALLS); + } - // After ~0.2 sec: - // rapidMode = HOLD_BALLS; break; case HOLD_BALLS: - // If both sensors occupied: - // hold intake at low power + if (robot.insideBeam.isPressed() + && robot.outsideBeam.isPressed()) { - // Else: - // run intake forward to pull rings back in + robot.setIntakePower(0.1); + + } else { + + robot.setIntakePower(1); + } - // If shoot button pressed: - // rapidMode = OPEN_GATE; - // If resume intake button pressed: - // rapidMode = INTAKE; break; case OPEN_GATE: - // Open upper gate - // Keep intake feeding + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open + ); - // After ~0.1 sec: - // rapidMode = SHOOT; + if (stateTime() >= 100) { + setRapidMode(RapidMode.SHOOT); + } break; case SHOOT: - // Activate kicker - // Feed ring into shooter - - // After ~0.4 sec: - // rapidMode = INTAKE; - + robot.setTransferServoPos( + ServoPositions.transferServo_in + ); + if (stateTime() >= 400) { + setRapidMode(RapidMode.INTAKE); + } break; } - break; case SORTED: // Future sorted-intake logic - // Color sorting - // Alliance filtering - // Different spindexer behavior - break; } } -} +} \ No newline at end of file