From 2fd87c90936c1665f22613db6bcc9c9c710081c1 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 27 Jan 2026 18:38:41 -0600 Subject: [PATCH] @Matt please take a look at this code --- .../teamcode/constants/ServoPositions.java | 4 +- .../ftc/teamcode/teleop/TeleopV3.java | 114 +++++++++--------- .../ftc/teamcode/utils/Spindexer.java | 4 + 3 files changed, 65 insertions(+), 57 deletions(-) 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 63a5661..ebf480f 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 @@ -11,11 +11,11 @@ public class ServoPositions { public static double spindexer_intakePos3 = 0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.8; + public static double spindexer_outtakeBall3 = 0.24; public static double spindexer_outtakeBall2 = 0.6; public static double spindexer_outtakeBall1 = 0.4; - public static double spinStartPos = spindexer_outtakeBall1 - 0.08; + public static double spinStartPos = spindexer_outtakeBall3 - 0.1; public static double transferServo_out = 0.15; 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 c203c7a..5ce64b7 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 @@ -3,6 +3,9 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; 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_outtakeBall2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; @@ -123,8 +126,8 @@ public class TeleopV3 extends LinearOpMode { private int tickerA = 1; private boolean transferIn = false; boolean turretInterpolate = false; - public static double spinSpeedIncrease = 0.04; - public static int resetSpinTicks = 20; + public static double spinSpeedIncrease = 0.03; + public static int resetSpinTicks = 4; public static double velPrediction(double distance) { if (distance < 30) { @@ -584,20 +587,18 @@ public class TeleopV3 extends LinearOpMode { // } if (enableSpindexerManager) { - // Gives some time to reset spindexer - if (!shootAll && intakeTicker > resetSpinTicks) { + // RIGHT_BUMPER + if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { + robot.intake.setPower(1); spindexer.processIntake(); - } else { - robot.spin1.setPosition(spindexer_intakePos1); - robot.spin2.setPosition(1-spindexer_intakePos1); - } - - // RIGHT_BUMPER - if (gamepad1.right_bumper) { - robot.intake.setPower(1); - } else { robot.intake.setPower(0); + if (spindexer.isFull() && intakeTicker > resetSpinTicks*5){ + robot.spin1.setPosition(spinStartPos); + robot.spin2.setPosition(1-spinStartPos); + } else { + spindexer.processIntake(); + } } // LEFT_BUMPER @@ -609,7 +610,6 @@ public class TeleopV3 extends LinearOpMode { shootAll = true; shooterTicker = 0; - spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball } intakeTicker++; @@ -618,18 +618,23 @@ public class TeleopV3 extends LinearOpMode { intake = false; reject = false; - if (getRuntime() - shootStamp < 3.5 && servo.getSpinPos() < spindexer_outtakeBall3 + 0.1) { + if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4 || shooterTicker == 0) { - if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){ + if (shooterTicker == 0){ robot.transferServo.setPosition(transferServo_out); - robot.spin1.setPosition(spindexPos); - robot.spin2.setPosition(1-spindexPos); + robot.spin1.setPosition(spinStartPos); + robot.spin2.setPosition(1-spinStartPos); + if (servo.spinEqual(spinStartPos)){ + shooterTicker++; + } + TELE.addLine("starting to shoot"); } else { robot.transferServo.setPosition(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); + double prevSpinPos = servo.getSpinPos(); robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); + TELE.addLine("shooting"); } } else { @@ -640,6 +645,7 @@ public class TeleopV3 extends LinearOpMode { spindexer.resetSpindexer(); spindexer.processIntake(); + TELE.addLine("stop shooting"); } } @@ -825,42 +831,40 @@ public class TeleopV3 extends LinearOpMode { hub.clearBulkCache(); } // - TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); - TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); - TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); - - TELE.addData("pose", drive.localizer.getPose()); - TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); - TELE.addData("distanceToGoal", distanceToGoal); - TELE.addData("hood", robot.hood.getPosition()); - TELE.addData("targetVel", vel); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Velo1", flywheel.velo1); - TELE.addData("Velo2", flywheel.velo2); - TELE.addData("shootOrder", shootOrder); - TELE.addData("oddColor", oddBallColor); - - // Spindexer Debug - 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("distanceRearCenter", spindexer.distanceRearCenter); - //TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver); - //TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger); - TELE.addData("shootall commanded", shootAll); - // Targeting Debug - TELE.addData("robotX", robotX); - TELE.addData("robotY", robotY); - TELE.addData("robotInchesX", targeting.robotInchesX); - TELE.addData( "robotInchesY", targeting.robotInchesY); - TELE.addData("Targeting Interpolate", turretInterpolate); - TELE.addData("Targeting GridX", targeting.robotGridX); - TELE.addData("Targeting GridY", targeting.robotGridY); - TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); - TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); - TELE.addData("timeSinceStamp", getRuntime() - shootStamp); +// TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); +// TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); +// TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); +// +// TELE.addData("pose", drive.localizer.getPose()); +// TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); +// TELE.addData("distanceToGoal", distanceToGoal); +// TELE.addData("hood", robot.hood.getPosition()); +// TELE.addData("targetVel", vel); +// TELE.addData("Velocity", flywheel.getVelo()); +// TELE.addData("Velo1", flywheel.velo1); +// TELE.addData("Velo2", flywheel.velo2); +// TELE.addData("shootOrder", shootOrder); +// TELE.addData("oddColor", oddBallColor); +// +// // Spindexer Debug +// 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("shootall commanded", shootAll); +// // Targeting Debug +// TELE.addData("robotX", robotX); +// TELE.addData("robotY", robotY); +// TELE.addData("robotInchesX", targeting.robotInchesX); +// TELE.addData( "robotInchesY", targeting.robotInchesY); +// TELE.addData("Targeting Interpolate", turretInterpolate); +// TELE.addData("Targeting GridX", targeting.robotGridX); +// TELE.addData("Targeting GridY", targeting.robotGridY); +// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); +// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); +// TELE.addData("timeSinceStamp", getRuntime() - shootStamp); TELE.update(); 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 190e00b..03d038d 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 @@ -291,6 +291,10 @@ public class Spindexer { } } + public boolean slotIsEmpty(int slot){ + return !ballPositions[slot].isEmpty; + } + public boolean isFull () { return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty); }