From dea9a10b087fb461f8b1052a250e6ce45538ef6b Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 27 Jan 2026 16:36:46 -0600 Subject: [PATCH] added targeting information and unjaming code (both untested) --- .../ftc/teamcode/teleop/TeleopV3.java | 15 ++++++++--- .../ftc/teamcode/utils/Spindexer.java | 17 ++++++++++-- .../ftc/teamcode/utils/Targeting.java | 26 +++++++++---------- .../ftc/teamcode/utils/Turret.java | 3 --- 4 files changed, 39 insertions(+), 22 deletions(-) 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 d4d3aac..c203c7a 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,7 @@ 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_outtakeBall3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Servos.spinD; @@ -123,6 +124,7 @@ public class TeleopV3 extends LinearOpMode { private boolean transferIn = false; boolean turretInterpolate = false; public static double spinSpeedIncrease = 0.04; + public static int resetSpinTicks = 20; public static double velPrediction(double distance) { if (distance < 30) { @@ -582,8 +584,12 @@ public class TeleopV3 extends LinearOpMode { // } if (enableSpindexerManager) { - if (!shootAll) { + // Gives some time to reset spindexer + if (!shootAll && intakeTicker > resetSpinTicks) { spindexer.processIntake(); + } else { + robot.spin1.setPosition(spindexer_intakePos1); + robot.spin2.setPosition(1-spindexer_intakePos1); } // RIGHT_BUMPER @@ -606,15 +612,16 @@ public class TeleopV3 extends LinearOpMode { spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball } - + intakeTicker++; if (shootAll) { - + intakeTicker = 0; intake = false; reject = false; - if (getRuntime() - shootStamp < 3.5) { + if (getRuntime() - shootStamp < 3.5 && servo.getSpinPos() < spindexer_outtakeBall3 + 0.1) { if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){ + robot.transferServo.setPosition(transferServo_out); robot.spin1.setPosition(spindexPos); robot.spin2.setPosition(1-spindexPos); } else { 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 4496cae..190e00b 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 @@ -39,6 +39,8 @@ public class Spindexer { public double distanceFrontDriver = 0.0; public double distanceFrontPassenger = 0.0; + private double prevPos = 0.0; + public Types.Motif desiredMotif = Types.Motif.NONE; // For Use enum RotatedBallPositionNames { @@ -246,10 +248,21 @@ public class Spindexer { return newPos1Detection; } - - public void moveSpindexerToPos(double pos) { + // Has code to unjam spindexer + private void moveSpindexerToPos(double pos) { robot.spin1.setPosition(pos); robot.spin2.setPosition(1-pos); + double currentPos = servos.getSpinPos(); + if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ + if (currentPos > pos){ + robot.spin1.setPosition(1); + robot.spin2.setPosition(0); + } else { + robot.spin1.setPosition(0); + robot.spin2.setPosition(1); + } + } + prevPos = currentPos; } public void stopSpindexer() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index bfb87d9..12758ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -67,19 +67,19 @@ public class Targeting { KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47); KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47); // ROW 4 - KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1); - KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1); - KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1); - KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1); - KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1); - KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1); - // ROW 1 - KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1); - KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1); - KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1); - KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1); - KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1); - KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1); + KNOWNTARGETING[4][0] = new Settings (3100, 0.49); + KNOWNTARGETING[4][1] = new Settings (3100, 0.49); + KNOWNTARGETING[4][2] = new Settings (3100, 0.5); + KNOWNTARGETING[4][3] = new Settings (3200, 0.5); + KNOWNTARGETING[4][4] = new Settings (3250, 0.49); + KNOWNTARGETING[4][5] = new Settings (3300, 0.49); + // ROW 5 + KNOWNTARGETING[5][0] = new Settings (3200, 0.48); + KNOWNTARGETING[5][1] = new Settings (3200, 0.48); + KNOWNTARGETING[5][2] = new Settings (3300, 0.48); + KNOWNTARGETING[5][3] = new Settings (3350, 0.48); + KNOWNTARGETING[5][4] = new Settings (3350, 0.48); + KNOWNTARGETING[5][5] = new Settings (3350, 0.48); } public Targeting() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 492a02f..d2def91 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -119,17 +119,14 @@ public class Turret { } public double getTy() { - limelightRead(); return ty; } public double getLimelightX() { - limelightRead(); return limelightPosX; } public double getLimelightY() { - limelightRead(); return limelightPosY; }