From dea9a10b087fb461f8b1052a250e6ce45538ef6b Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 27 Jan 2026 16:36:46 -0600 Subject: [PATCH 01/18] 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; } From 1715fa96cb9a141966b2f1527a5577c36ef5feab Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 27 Jan 2026 16:44:55 -0600 Subject: [PATCH 02/18] updated dash version --- build.dependencies.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 770fdae..28189d8 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -29,7 +29,7 @@ dependencies { implementation "com.acmerobotics.roadrunner:ftc:0.1.25" //RR implementation "com.acmerobotics.roadrunner:core:1.0.1" //RR implementation "com.acmerobotics.roadrunner:actions:1.0.1" //RR - implementation "com.acmerobotics.dashboard:dashboard:0.4.17" //FTC Dash + implementation "com.acmerobotics.dashboard:dashboard:0.5.1" //FTC Dash implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB From 2fd87c90936c1665f22613db6bcc9c9c710081c1 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 27 Jan 2026 18:38:41 -0600 Subject: [PATCH 03/18] @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); } From a6fe8c14e62c14ab1394001b401eaa0db9eace9e Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 27 Jan 2026 18:51:24 -0600 Subject: [PATCH 04/18] @Matt please take a look at this code --- .../firstinspires/ftc/teamcode/teleop/TeleopV3.java | 12 ++++-------- 1 file changed, 4 insertions(+), 8 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 5ce64b7..ae03417 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 @@ -587,18 +587,14 @@ public class TeleopV3 extends LinearOpMode { // } if (enableSpindexerManager) { + spindexer.processIntake(); + // RIGHT_BUMPER if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { robot.intake.setPower(1); - spindexer.processIntake(); } 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 @@ -618,7 +614,7 @@ public class TeleopV3 extends LinearOpMode { intake = false; reject = false; - if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4 || shooterTicker == 0) { + if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) { if (shooterTicker == 0){ robot.transferServo.setPosition(transferServo_out); From 2a45eee878d9a6947aa15a573ea3de91346fb95f Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Wed, 28 Jan 2026 00:45:21 -0600 Subject: [PATCH 05/18] Update spindexer positions after repair. --- .../ftc/teamcode/constants/ServoPositions.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 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 ebf480f..d5b302b 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 @@ -5,16 +5,16 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.13; + public static double spindexer_intakePos1 = 0.00; //0.13; - public static double spindexer_intakePos2 = 0.33;//0.5; + public static double spindexer_intakePos2 = 0.19; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.53;//0.66; + public static double spindexer_intakePos3 = 0.38; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.24; + public static double spindexer_outtakeBall3 = 0.27; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.6; - public static double spindexer_outtakeBall1 = 0.4; + public static double spindexer_outtakeBall2 = 0.65; //0.46; //0.6; + public static double spindexer_outtakeBall1 = 0.46; //0.27; //0.4; public static double spinStartPos = spindexer_outtakeBall3 - 0.1; From d088fba20a586996e0e8e1cd9caf1400639e6b5d Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Wed, 28 Jan 2026 13:06:53 -0600 Subject: [PATCH 06/18] Create shootAll state machine in spindexer and call from TeleOpV3. Experiment with averaging tiles in Targeting, which is permanently disabled at the moment. --- .../ftc/teamcode/teleop/TeleopV3.java | 48 +++--- .../ftc/teamcode/utils/Spindexer.java | 54 +++++-- .../ftc/teamcode/utils/Targeting.java | 148 ++++++++++-------- 3 files changed, 153 insertions(+), 97 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 ae03417..7228585 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 @@ -614,25 +614,32 @@ public class TeleopV3 extends LinearOpMode { intake = false; reject = false; - if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) { - - if (shooterTicker == 0){ - robot.transferServo.setPosition(transferServo_out); - 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 = servo.getSpinPos(); - robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); - robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); - TELE.addLine("shooting"); - } - +// if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) { +// +// if (shooterTicker == 0){ +// robot.transferServo.setPosition(transferServo_out); +// 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 = servo.getSpinPos(); +// robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); +// robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); +// TELE.addLine("shooting"); +// } + if (shooterTicker == 0) { + shooterTicker++; + spindexer.prepareShootAll(); + } else if (!spindexer.shootAllComplete()) { + robot.transferServo.setPosition(transferServo_in); + shooterTicker++; + spindexer.shootAll(); + TELE.addLine("starting to shoot"); } else { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; @@ -640,9 +647,10 @@ public class TeleopV3 extends LinearOpMode { shootAll = false; spindexer.resetSpindexer(); - spindexer.processIntake(); + //spindexer.processIntake(); TELE.addLine("stop shooting"); } + spindexer.processIntake(); } if (gamepad1.left_stick_button){ 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 03d038d..1975199 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 @@ -72,7 +72,10 @@ public class Spindexer { SHOOT_ALL_READY } + int shootWaitCount = 0; + public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; + public IntakeState prevIntakeState = IntakeState.UNKNOWN_START; public int unknownColorDetect = 0; enum BallColor { UNKNOWN, @@ -397,23 +400,21 @@ public class Spindexer { case SHOOT_ALL_PREP: // We get here with function call to prepareToShootMotif // Stopping when we get to the new position - if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_READY; - } else { + if (!servos.spinEqual(outakePositions[commandedIntakePosition])) { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" } break; - case SHOOT_ALL_READY: + case SHOOT_ALL_READY: // Not used // Double Check Colors //detectBalls(false, false); // Minimize hardware calls if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) { // All ball shot move to intake state - currentIntakeState = Spindexer.IntakeState.FINDNEXT; + currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } // Maintain Position - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + moveSpindexerToPos(outakePositions[commandedIntakePosition]); break; case SHOOTNEXT: @@ -431,13 +432,13 @@ public class Spindexer { } else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? // Position 3 commandedIntakePosition = 2; - servos.setSpinPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + servos.setSpinPos(outakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; } else { // Empty return to intake state currentIntakeState = IntakeState.FINDNEXT; } - moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + moveSpindexerToPos(outakePositions[commandedIntakePosition]); break; case SHOOTMOVING: @@ -446,15 +447,22 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + moveSpindexerToPos(outakePositions[commandedIntakePosition]); } break; case SHOOTWAIT: - // Stopping when we get to the new position - if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - currentIntakeState = Spindexer.IntakeState.INTAKE; - stopSpindexer(); + // Stopping when we get to the new position] + if (prevIntakeState != currentIntakeState) { + shootWaitCount = 0; + } else { + shootWaitCount++; + } + // wait 3 cycles + if ((shootWaitCount > 3) && (servos.spinEqual(intakePositions[commandedIntakePosition]))) { + currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; + ballPositions[commandedIntakePosition].isEmpty = true; + //stopSpindexer(); //detectBalls(true, false); } else { // Keep moving the spindexer @@ -465,6 +473,8 @@ public class Spindexer { default: // Statements to execute if no case matches } + + prevIntakeState = currentIntakeState; //TELE.addData("commandedIntakePosition", commandedIntakePosition); //TELE.update(); // Signal a successful intake @@ -516,6 +526,22 @@ public class Spindexer { commandedIntakePosition = bestFitMotif(); } + public void prepareShootAll(){ + currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP; + } + public void shootAll () { + currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; + } + + public boolean shootAllComplete () + { + return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) && + (currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) && + (currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) && + (currentIntakeState != Spindexer.IntakeState.SHOOTNEXT) && + (currentIntakeState != Spindexer.IntakeState.SHOOTWAIT)); + } + void shootAllToIntake () { currentIntakeState = Spindexer.IntakeState.FINDNEXT; } 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 12758ff..56b70a9 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 @@ -107,61 +107,81 @@ public class Targeting { // Determine if we need to interpolate based on tile position. // if near upper or lower quarter or tile interpolate with next tile. + int x0 = 0; + int y0 = 0; int x1 = 0; int y1 = 0; -// interpolate = false; -// if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && -// (robotGridX < 5) && (robotGridY <5)) { -// // +X, +Y -// interpolate = true; -// x1 = robotGridX + 1; -// y1 = robotGridY + 1; -// } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && -// (robotGridX > 0) && (robotGridY > 0)) { -// // -X, -Y -// interpolate = true; -// x1 = robotGridX - 1; -// y1 = robotGridY - 1; -// } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && -// (robotGridX < 5) && (robotGridY > 0)) { -// // +X, -Y -// interpolate = true; -// x1 = robotGridX + 1; -// y1 = robotGridY - 1; -// } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && -// (robotGridX > 0) && (robotGridY < 5)) { -// // -X, +Y -// interpolate = true; -// x1 = robotGridX - 1; -// y1 = robotGridY + 1; -// } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) { -// // -X, Y -// interpolate = true; -// x1 = robotGridX - 1; -// y1 = robotGridY; -// } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) { -// // X, -Y -// interpolate = true; -// x1 = robotGridX; -// y1 = robotGridY - 1; -// } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) { -// // +X, Y -// interpolate = true; -// x1 = robotGridX + 1; -// y1 = robotGridY; -// } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) { -// // X, +Y -// interpolate = true; -// x1 = robotGridX; -// y1 = robotGridY + 1; -// } + interpolate = false; + if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && + (robotGridX < 5) && (robotGridY <5)) { + // +X, +Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX + 1; + y0 = robotGridY; + y1 = robotGridY + 1; + } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && + (robotGridX > 0) && (robotGridY > 0)) { + // -X, -Y + interpolate = true; + x0 = robotGridX - 1; + x1 = robotGridX; + y0 = robotGridY - 1; + y1 = robotGridY; + } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && + (robotGridX < 5) && (robotGridY > 0)) { + // +X, -Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX + 1; + y0 = robotGridY - 1; + y1 = robotGridY; + } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && + (robotGridX > 0) && (robotGridY < 5)) { + // -X, +Y + interpolate = true; + x0 = robotGridX - 1; + x1 = robotGridX; + y0 = robotGridY; + y1 = robotGridY + 1; + } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) { + // -X, Y + interpolate = true; + x0 = robotGridX - 1; + x1 = robotGridX; + y0 = robotGridY; + y1 = robotGridY; + } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) { + // X, -Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX; + y0 = robotGridY - 1; + y1 = robotGridY; + } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) { + // +X, Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX + 1; + y0 = robotGridY; + y1 = robotGridY; + } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) { + // X, +Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX; + y0 = robotGridY; + y1 = robotGridY + 1; + } else { + interpolate = false; + } //clamp robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); // basic search - if(!interpolate) { + if(true) { //!interpolate) { if ((robotGridY < 6) && (robotGridX <6)) { recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; @@ -170,27 +190,29 @@ public class Targeting { } else { // bilinear interpolation - int x0 = robotGridX; + //int x0 = robotGridX; //int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1); - int y0 = robotGridY; + //int y0 = robotGridY; //int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1); - double x = (robotInchesX - (x0 * tileSize)) / tileSize; - double y = (robotInchesY - (y0 * tileSize)) / tileSize; +// double x = (robotInchesX - (x0 * tileSize)) / tileSize; +// double y = (robotInchesY - (y0 * tileSize)) / tileSize; - double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM; - double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM; - double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM; - double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM; - - double angle00 = KNOWNTARGETING[y0][x0].hoodAngle; - double angle10 = KNOWNTARGETING[y0][x1].hoodAngle; - double angle01 = KNOWNTARGETING[y1][x0].hoodAngle; - double angle11 = KNOWNTARGETING[y1][x1].hoodAngle; - - recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11; - recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11; +// double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM; +// double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM; +// double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM; +// double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM; +// +// double angle00 = KNOWNTARGETING[y0][x0].hoodAngle; +// double angle10 = KNOWNTARGETING[y0][x1].hoodAngle; +// double angle01 = KNOWNTARGETING[y1][x0].hoodAngle; +// double angle11 = KNOWNTARGETING[y1][x1].hoodAngle; +// recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11; +// recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11; + // Average target tiles + recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM)/2.0; + recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle)/2.0; return recommendedSettings; } } From f767e82e31ba055e40289b4a92d8db48a0d26ad5 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 28 Jan 2026 13:38:04 -0600 Subject: [PATCH 07/18] changed servo positions --- .../ftc/teamcode/constants/ServoPositions.java | 6 +++--- 1 file changed, 3 insertions(+), 3 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 d5b302b..9652da6 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,10 +11,10 @@ public class ServoPositions { public static double spindexer_intakePos3 = 0.38; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.27; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.65; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.65; //0.46; //0.6; - public static double spindexer_outtakeBall1 = 0.46; //0.27; //0.4; + public static double spindexer_outtakeBall2 = 0.46; //0.46; //0.6; + public static double spindexer_outtakeBall1 = 0.27; //0.27; //0.4; public static double spinStartPos = spindexer_outtakeBall3 - 0.1; From 5d0a569f82b6dff98ec4444fc7644dfd12433f1f Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 28 Jan 2026 15:23:17 -0600 Subject: [PATCH 08/18] spindex progress: not good --- .../ftc/teamcode/teleop/TeleopV3.java | 13 +++++----- .../ftc/teamcode/utils/Spindexer.java | 26 +++++++------------ 2 files changed, 17 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 7228585..f5e77d5 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 @@ -635,11 +635,12 @@ public class TeleopV3 extends LinearOpMode { if (shooterTicker == 0) { shooterTicker++; spindexer.prepareShootAll(); + TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { robot.transferServo.setPosition(transferServo_in); shooterTicker++; spindexer.shootAll(); - TELE.addLine("starting to shoot"); + TELE.addLine("shoot"); } else { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; @@ -851,11 +852,11 @@ public class TeleopV3 extends LinearOpMode { // 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("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 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 1975199..c167a43 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 @@ -258,11 +258,11 @@ public class Spindexer { double currentPos = servos.getSpinPos(); if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ if (currentPos > pos){ - robot.spin1.setPosition(1); - robot.spin2.setPosition(0); + robot.spin1.setPosition(servos.getSpinPos() + 0.05); + robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05); } else { - robot.spin1.setPosition(0); - robot.spin2.setPosition(1); + robot.spin1.setPosition(servos.getSpinPos() - 0.05); + robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); } } prevPos = currentPos; @@ -307,7 +307,7 @@ public class Spindexer { case UNKNOWN_START: // For now just set position ONE if UNKNOWN commandedIntakePosition = 0; - servos.setSpinPos(intakePositions[0]); + moveSpindexerToPos(intakePositions[0]); currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; break; case UNKNOWN_MOVE: @@ -348,7 +348,6 @@ public class Spindexer { if (ballPositions[0].isEmpty) { // Position 1 commandedIntakePosition = 0; - servos.setSpinPos(intakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.MOVING; } //proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos); @@ -356,14 +355,12 @@ public class Spindexer { if (ballPositions[1].isEmpty) { // Position 2 commandedIntakePosition = 1; - servos.setSpinPos(intakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.MOVING; } //proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); if (ballPositions[2].isEmpty) { // Position 3 commandedIntakePosition = 2; - servos.setSpinPos(intakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.MOVING; } if (currentIntakeState != Spindexer.IntakeState.MOVING) { @@ -422,17 +419,14 @@ public class Spindexer { if (!ballPositions[0].isEmpty) { // Position 1 commandedIntakePosition = 0; - servos.setSpinPos(outakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; - } else if (ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty? + } else if (!ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty? // Position 2 commandedIntakePosition = 1; - servos.setSpinPos(outakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; - } else if (ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? + } else if (!ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? // Position 3 commandedIntakePosition = 2; - servos.setSpinPos(outakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; } else { // Empty return to intake state @@ -452,21 +446,21 @@ public class Spindexer { break; case SHOOTWAIT: - // Stopping when we get to the new position] + // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { shootWaitCount = 0; } else { shootWaitCount++; } // wait 3 cycles - if ((shootWaitCount > 3) && (servos.spinEqual(intakePositions[commandedIntakePosition]))) { + if ((shootWaitCount > 15) && (servos.spinEqual(outakePositions[commandedIntakePosition]))) { currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; ballPositions[commandedIntakePosition].isEmpty = true; //stopSpindexer(); //detectBalls(true, false); } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + moveSpindexerToPos(outakePositions[commandedIntakePosition]); } break; From 641d947ec6bf6ce0eaf68ba1fe6badd09f2475f0 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 28 Jan 2026 15:36:44 -0600 Subject: [PATCH 09/18] last edit --- .../ftc/teamcode/utils/Spindexer.java | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 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 c167a43..488c0e2 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 @@ -255,17 +255,17 @@ public class 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(servos.getSpinPos() + 0.05); - robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05); - } else { - robot.spin1.setPosition(servos.getSpinPos() - 0.05); - robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); - } - } - prevPos = currentPos; +// double currentPos = servos.getSpinPos(); +// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ +// if (currentPos > pos){ +// robot.spin1.setPosition(servos.getSpinPos() + 0.05); +// robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05); +// } else { +// robot.spin1.setPosition(servos.getSpinPos() - 0.05); +// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); +// } +// } +// prevPos = currentPos; } public void stopSpindexer() { From 159b130b5fcf793ad87aaa13264c20f17e535cf7 Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Wed, 28 Jan 2026 17:33:37 -0600 Subject: [PATCH 10/18] Integrate shootAll on the Robot. This version was working except with 1 ball. --- .../teamcode/constants/ServoPositions.java | 12 ++++---- .../ftc/teamcode/teleop/TeleopV3.java | 12 ++++---- .../ftc/teamcode/utils/Spindexer.java | 30 +++++++++---------- 3 files changed, 27 insertions(+), 27 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 9652da6..3fa4906 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 @@ -5,16 +5,16 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.00; //0.13; + public static double spindexer_intakePos1 = 0.05; //0.13; - public static double spindexer_intakePos2 = 0.19; //0.33;//0.5; + public static double spindexer_intakePos2 = 0.24; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.38; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.43; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.65; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.70; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.46; //0.46; //0.6; - public static double spindexer_outtakeBall1 = 0.27; //0.27; //0.4; + public static double spindexer_outtakeBall2 = 0.51; //0.46; //0.6; + public static double spindexer_outtakeBall1 = 0.32; //0.27; //0.4; public static double spinStartPos = spindexer_outtakeBall3 - 0.1; 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 f5e77d5..567cf73 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 @@ -633,25 +633,25 @@ public class TeleopV3 extends LinearOpMode { // TELE.addLine("shooting"); // } if (shooterTicker == 0) { - shooterTicker++; spindexer.prepareShootAll(); + TELE.addLine("preparing to shoot"); + } else if (shooterTicker == 2) { + robot.transferServo.setPosition(transferServo_in); + spindexer.shootAll(); TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - spindexer.shootAll(); TELE.addLine("shoot"); } else { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; - shootAll = false; - spindexer.resetSpindexer(); //spindexer.processIntake(); TELE.addLine("stop shooting"); } - spindexer.processIntake(); + shooterTicker++; + //spindexer.processIntake(); } if (gamepad1.left_stick_button){ 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 488c0e2..79dcab3 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 @@ -343,21 +343,21 @@ public class Spindexer { // Find Next Open Position and start movement double currentSpindexerPos = servos.getSpinPos(); double commandedtravelDistance = 2.0; - //double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos); + double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos); //if (ballPositions[0].isEmpty && (proposedTravelDistance < commandedtravelDistance)) { if (ballPositions[0].isEmpty) { // Position 1 commandedIntakePosition = 0; currentIntakeState = Spindexer.IntakeState.MOVING; } - //proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos); + proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos); //if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) { if (ballPositions[1].isEmpty) { // Position 2 commandedIntakePosition = 1; currentIntakeState = Spindexer.IntakeState.MOVING; } - //proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); + proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); if (ballPositions[2].isEmpty) { // Position 3 commandedIntakePosition = 2; @@ -416,18 +416,18 @@ public class Spindexer { case SHOOTNEXT: // Find Next Open Position and start movement - if (!ballPositions[0].isEmpty) { + if (!ballPositions[1].isEmpty) { // Position 1 - commandedIntakePosition = 0; - currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; - } else if (!ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty? - // Position 2 commandedIntakePosition = 1; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; - } else if (!ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? - // Position 3 + } else if (!ballPositions[2].isEmpty) { + // Position 2 commandedIntakePosition = 2; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; + } else if (!ballPositions[0].isEmpty) { + // Position 3 + commandedIntakePosition = 0; + currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; } else { // Empty return to intake state currentIntakeState = IntakeState.FINDNEXT; @@ -452,16 +452,16 @@ public class Spindexer { } else { shootWaitCount++; } - // wait 3 cycles - if ((shootWaitCount > 15) && (servos.spinEqual(outakePositions[commandedIntakePosition]))) { + // wait 10 cycles + if (shootWaitCount > 2) { currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; ballPositions[commandedIntakePosition].isEmpty = true; + shootWaitCount = 0; //stopSpindexer(); //detectBalls(true, false); - } else { - // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); } + // Keep moving the spindexer + moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.01)); break; default: From 661730ef1845143056a31dcdd3f326112ca5b67b Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 28 Jan 2026 19:31:52 -0600 Subject: [PATCH 11/18] stash --- .../teamcode/autonomous/Auto_LT_Indexed.java | 1744 ++++++++--------- .../autonomous/ProtoAutoClose_V4.java | 901 +++++++++ .../ftc/teamcode/constants/Poses.java | 13 +- .../ftc/teamcode/libs/RR/MecanumDrive.java | 6 +- 4 files changed, 1786 insertions(+), 878 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java index cab8505..518e821 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java @@ -1,872 +1,872 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bHGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShoot1Tangent; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bXGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bYGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2c; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2a; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2b; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2c; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2c; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2c; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rHGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShoot1Tangent; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rXGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rYGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2c; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2a; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2b; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2c; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2c; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2c; -import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.SequentialAction; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.TranslationalVelConstraint; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.Servo; - -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 org.firstinspires.ftc.teamcode.utils.Targeting; -import org.firstinspires.ftc.teamcode.utils.Turret; - -import java.util.ArrayList; -import java.util.List; - -@Autonomous(preselectTeleOp = "TeleopV3") -@Config -public class Auto_LT_Indexed extends LinearOpMode { - - public static double shoot0Vel = 2300, shoot0Hood = 0.93; - public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.014; - - public static double spindexerSpeedIncrease = 0.02; - - public static double obeliskTurrPos = 0.53; - public static double gatePickupTime = 3.0; - public static double shoot1Turr = 0.57; - public static double shoot0XTolerance = 1.0; - public static double turretShootPos = 0.72; - public static double shootAllTime = 1.8; - public static double shoot0Time = 1.6; - public static double intake1Time = 3.0; - public static double flywheel0Time = 3.5; - public static double pickup1Speed = 20.0; - public static double pickup2Speed = 20.0; - public static double pickup3Speed = 20.0; - - // ---- SECOND SHOT / PICKUP ---- - public static double shoot1Vel = 2300; - public static double shoot1Hood = 0.93; - - public static double shootAllVelocity = 2500; - public static double shootAllHood = 0.78; - // ---- PICKUP TIMING ---- - public static double pickup1Time = 3.0; - // ---- PICKUP POSITION TOLERANCES ---- - public static double pickup1XTolerance = 2.0; - public static double pickup1YTolerance = 2.0; - // ---- OBELISK DETECTION ---- - public static double obelisk1Time = 1.5; - public static double obelisk1XTolerance = 2.0; - public static double obelisk1YTolerance = 2.0; - public static double shoot1ToleranceX = 2.0; - public static double shoot1ToleranceY = 2.0; - public static double shoot1Time = 2.0; - public static double shootCycleTime = 2.5; - - List s1 = new ArrayList<>(); - - List s2 = new ArrayList<>(); - List s3 = new ArrayList<>(); - public int motif = 0; - Robot robot; - MultipleTelemetry TELE; - MecanumDrive drive; - Servos servos; - Spindexer spindexer; - Flywheel flywheel; - Turret turret; - Targeting targeting; - Targeting.Settings targetingSettings; - private double x1, y1, h1; - - private double x2a, y2a, h2a, t2a; - - private double x2b, y2b, h2b, t2b; - private double x2c, y2c, h2c, t2c; - - private double xShoot, yShoot, hShoot; - private double xGate, yGate, hGate; - - private double shoot1Tangent; - - // ---- OPEN GATE / MIDFIELD ---- - private double x3, y3, h3, t3; - - // ---- PICKUP 2 ---- - private double x4a, y4a, h4a; - private double x4b, y4b, h4b; - - // ---- PICKUP 3 ---- - private double x5a, y5a, h5a; - private double x5b, y5b, h5b; - - // ---- PARK ---- - private double xPark, yPark, hPark; - - public Action prepareShootIndexed(double time) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - - boolean testColor = false; - - int s1Green = 0; - int s2Green = 0; - int s3Green = 0; - - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - ticker++; - - if ((System.currentTimeMillis() - stamp) < (goToDetectTime)) { //0.25-0.4 ish??? - robot.spin1.setPosition(colorDetectPos); //0.43 - robot.spin2.setPosition(1 - colorDetectPos); - } else if ((System.currentTimeMillis() - stamp) < (colorSenseTime + goToDetectTime)) { - - - double green1 = robot.color1.getNormalizedColors().green; - double red1 = robot.color1.getNormalizedColors().red; - double blue1 = robot.color1.getNormalizedColors().blue; - - double gP1 = green1 / (green1 + red1 + blue1); - - - if (gP1 >= color1Thresh) { - s1Green ++; - } - - double green2 = robot.color2.getNormalizedColors().green; - double red2 = robot.color2.getNormalizedColors().red; - double blue2 = robot.color2.getNormalizedColors().blue; - - double gP2 = green2 / (green2 + red2 + blue2); - - if (gP2 >= color2Thresh) { - s2Green ++; - } - - - double green3 = robot.color3.getNormalizedColors().green; - double red3 = robot.color3.getNormalizedColors().red; - double blue3 = robot.color3.getNormalizedColors().blue; - - double gP3 = green3 / (green3 + red3 + blue3); - - if (gP3 >= color3Thresh) { - s3Green ++; - } - - - testColor = true; - - - - } else { - - double spindexPos; - if (motif == 21) { - spindexPos = - } - } - - robot.transferServo.setPosition(transferServo_out); - - turret.manualSetTurret(turretShootPos); - - robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000); - - return (System.currentTimeMillis() - stamp) < (time * 1000); - - } - }; - } - - public Action shootAll(int vel, double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - - double stamp = 0.0; - - double velo = vel; - - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); - TELE.update(); - - flywheel.manageFlywheel(vel); - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(0); - - if (getRuntime() - stamp < shootTime) { - - if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); - } - - return true; - - } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - - } - }; - } - - public Action shootAllAuto(double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - - double stamp = 0.0; - - double velo = 0.0; - - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); - TELE.update(); - - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(0); - - if (getRuntime() - stamp < shootTime) { - - if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); - } - - return true; - - } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - - } - }; - } - - public Action intake(double intakeTime) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - ticker++; - - spindexer.processIntake(); - robot.intake.setPower(1); - - motif = turret.detectObelisk(); - - spindexer.ballCounterLight(); - - return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); - - } - }; - } - - public Action detectObelisk( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance, - double turrPos - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - motif = turret.detectObelisk(); - - robot.turr1.setPosition(turrPos); - robot.turr2.setPosition(1 - turrPos); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - - return !shouldFinish; - - } - }; - } - - public Action manageFlywheel( - double vel, - double hoodPos, - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - flywheel.manageFlywheel(vel); - robot.hood.setPosition(hoodPos); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - - return !shouldFinish; - - } - }; - } - - public Action manageShooterAuto( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - double robotX = drive.localizer.getPose().position.x; - double robotY = drive.localizer.getPose().position.y; - - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robotX - goalX; // delta x from robot to goal - double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robotX, robotY, robotHeading, 0.0, false); - - turret.trackGoal(deltaPose); - - robot.hood.setPosition(targetingSettings.hoodAngle); - - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - - return !shouldFinish; - - } - }; - } - - public Action manageFlywheelAuto( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - double robotX = drive.localizer.getPose().position.x; - double robotY = drive.localizer.getPose().position.y; - - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robotX - goalX; // delta x from robot to goal - double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robotX, robotY, robotHeading, 0.0, false); - - robot.hood.setPosition(targetingSettings.hoodAngle); - - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - - return !shouldFinish; - - } - }; - } - - @Override - public void runOpMode() throws InterruptedException { - - hardwareMap.get(Servo.class, "light").setPosition(0); - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - flywheel = new Flywheel(hardwareMap); - - targeting = new Targeting(); - targetingSettings = new Targeting.Settings(0.0, 0.0); - - spindexer = new Spindexer(hardwareMap); - - servos = new Servos(hardwareMap); - - robot.limelight.pipelineSwitch(1); - - turret = new Turret(robot, TELE, robot.limelight); - - turret.manualSetTurret(0.4); - - drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - - TrajectoryActionBuilder shoot0 = null; - - TrajectoryActionBuilder pickup1 = null; - TrajectoryActionBuilder shoot1 = null; - TrajectoryActionBuilder pickup2 = null; - TrajectoryActionBuilder shoot2 = null; - TrajectoryActionBuilder pickup3 = null; - TrajectoryActionBuilder shoot3 = null; - TrajectoryActionBuilder openGate = null; - TrajectoryActionBuilder park = null; - - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - - robot.transferServo.setPosition(transferServo_out); - - robot.light.setPosition(1); - - while (opModeInInit()) { - - robot.hood.setPosition(shoot0Hood); - - if (gamepad2.crossWasPressed()) { - redAlliance = !redAlliance; - } - - if (redAlliance) { - robot.light.setPosition(0.28); - - // ===== FIRST SHOT ===== - x1 = rx1; - y1 = ry1; - h1 = rh1; - - // ===== PICKUP PATH ===== - x2a = rx2a; - y2a = ry2a; - h2a = rh2a; - t2a = rt2a; - - x2b = rx2b; - y2b = ry2b; - h2b = rh2b; - t2b = rt2b; - - x2c = rx2c; - y2c = ry2c; - h2c = rh2c; - t2c = rt2c; - - // ===== SHOOT POSE ===== - xShoot = rShootX; - yShoot = rShootY; - hShoot = rShootH; - shoot1Tangent = rShoot1Tangent; - - // ===== GATE ===== - xGate = rXGate; - yGate = rYGate; - hGate = rHGate; - - } else { - robot.light.setPosition(0.6); - - // ===== FIRST SHOT ===== - x1 = bx1; - y1 = by1; - h1 = bh1; - - // ===== PICKUP PATH ===== - x2a = bx2a; - y2a = by2a; - h2a = bh2a; - t2a = bt2a; - - x2b = bx2b; - y2b = by2b; - h2b = bh2b; - t2b = bt2b; - - x2c = bx2c; - y2c = by2c; - h2c = bh2c; - t2c = bt2c; - - // ===== SHOOT POSE ===== - xShoot = bShootX; - yShoot = bShootY; - hShoot = bShootH; - shoot1Tangent = bShoot1Tangent; - - // ===== GATE ===== - xGate = bXGate; - yGate = bYGate; - hGate = bHGate; - } - - shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1); - - pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) - .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, - new TranslationalVelConstraint(pickup1Speed)); - - openGate = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) - .setReversed(true) - .splineToConstantHeading(new Vector2d(x3, y3), t3); - - shoot1 = drive.actionBuilder(new Pose2d(x3, y3, h2b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); - - pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) - .strafeToLinearHeading(new Vector2d(x4a, y4a), h4a) - .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, - new TranslationalVelConstraint(pickup2Speed)); - shoot2 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); - - pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) - .strafeToLinearHeading(new Vector2d(x5a, y5a), h5a) - .strafeToLinearHeading(new Vector2d(x5b, y5b), h5b, - new TranslationalVelConstraint(pickup3Speed)); - shoot3 = drive.actionBuilder(new Pose2d(x5b, y5b, h5b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); - park = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) - .strafeToLinearHeading(new Vector2d(xPark, yPark), hPark); - - } - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()) { - - robot.transfer.setPower(1); - - assert shoot0 != null; - - Actions.runBlocking( - new ParallelAction( - shoot0.build(), - manageFlywheel( - shoot0Vel, - shoot0Hood, - flywheel0Time, - x1, - 0.501, - shoot0XTolerance, - 0.501 - ) - - ) - ); - - Actions.runBlocking( - shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) - ); - - //WORKING SHOOT ALL for the preload - - //Pickup first pile - - Actions.runBlocking( - new SequentialAction( - new ParallelAction( - pickup1.build(), - manageFlywheel( - shootAllVelocity, - shootAllHood, - pickup1Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake1Time), - detectObelisk( - obelisk1Time, - x2b, - y2c, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos - ) - ), - new ParallelAction( - openGate.build() //TODO: Add other managing stuff here - ) - ) - - ); - - motif = turret.detectObelisk(); //Detect Obelisk if not alr - - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shootAllVelocity, - shootAllHood, - shoot1Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot1.build(), - prepareShootIndexed(shoot1Time) - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllIndexed(shootAllTime, spindexerSpeedIncrease) - ) - - ); - - //Shoot from first pile - - } - - } -} +//package org.firstinspires.ftc.teamcode.autonomous; +// +//import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bHGate; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShoot1Tangent; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootH; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootX; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootY; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bXGate; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bYGate; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh1; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2a; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2b; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2c; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2a; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2b; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2c; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx1; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2a; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2b; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2c; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by1; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2a; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2b; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2c; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rHGate; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShoot1Tangent; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootH; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootX; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootY; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rXGate; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rYGate; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh1; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2a; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2b; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2c; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2a; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2b; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2c; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx1; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2a; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2b; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2c; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry1; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2a; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2b; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2c; +//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.teleStart; +//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; +//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +// +//import androidx.annotation.NonNull; +// +//import com.acmerobotics.dashboard.FtcDashboard; +//import com.acmerobotics.dashboard.config.Config; +//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +//import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +//import com.acmerobotics.roadrunner.Action; +//import com.acmerobotics.roadrunner.ParallelAction; +//import com.acmerobotics.roadrunner.Pose2d; +//import com.acmerobotics.roadrunner.SequentialAction; +//import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +//import com.acmerobotics.roadrunner.TranslationalVelConstraint; +//import com.acmerobotics.roadrunner.Vector2d; +//import com.acmerobotics.roadrunner.ftc.Actions; +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +//import com.qualcomm.robotcore.hardware.Servo; +// +//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 org.firstinspires.ftc.teamcode.utils.Targeting; +//import org.firstinspires.ftc.teamcode.utils.Turret; +// +//import java.util.ArrayList; +//import java.util.List; +// +//@Autonomous(preselectTeleOp = "TeleopV3") +//@Config +//public class Auto_LT_Indexed extends LinearOpMode { +// +// public static double shoot0Vel = 2300, shoot0Hood = 0.93; +// public static double autoSpinStartPos = 0.2; +// public static double shoot0SpinSpeedIncrease = 0.014; +// +// public static double spindexerSpeedIncrease = 0.02; +// +// public static double obeliskTurrPos = 0.53; +// public static double gatePickupTime = 3.0; +// public static double shoot1Turr = 0.57; +// public static double shoot0XTolerance = 1.0; +// public static double turretShootPos = 0.72; +// public static double shootAllTime = 1.8; +// public static double shoot0Time = 1.6; +// public static double intake1Time = 3.0; +// public static double flywheel0Time = 3.5; +// public static double pickup1Speed = 20.0; +// public static double pickup2Speed = 20.0; +// public static double pickup3Speed = 20.0; +// +// // ---- SECOND SHOT / PICKUP ---- +// public static double shoot1Vel = 2300; +// public static double shoot1Hood = 0.93; +// +// public static double shootAllVelocity = 2500; +// public static double shootAllHood = 0.78; +// // ---- PICKUP TIMING ---- +// public static double pickup1Time = 3.0; +// // ---- PICKUP POSITION TOLERANCES ---- +// public static double pickup1XTolerance = 2.0; +// public static double pickup1YTolerance = 2.0; +// // ---- OBELISK DETECTION ---- +// public static double obelisk1Time = 1.5; +// public static double obelisk1XTolerance = 2.0; +// public static double obelisk1YTolerance = 2.0; +// public static double shoot1ToleranceX = 2.0; +// public static double shoot1ToleranceY = 2.0; +// public static double shoot1Time = 2.0; +// public static double shootCycleTime = 2.5; +// +// List s1 = new ArrayList<>(); +// +// List s2 = new ArrayList<>(); +// List s3 = new ArrayList<>(); +// public int motif = 0; +// Robot robot; +// MultipleTelemetry TELE; +// MecanumDrive drive; +// Servos servos; +// Spindexer spindexer; +// Flywheel flywheel; +// Turret turret; +// Targeting targeting; +// Targeting.Settings targetingSettings; +// private double x1, y1, h1; +// +// private double x2a, y2a, h2a, t2a; +// +// private double x2b, y2b, h2b, t2b; +// private double x2c, y2c, h2c, t2c; +// +// private double xShoot, yShoot, hShoot; +// private double xGate, yGate, hGate; +// +// private double shoot1Tangent; +// +// // ---- OPEN GATE / MIDFIELD ---- +// private double x3, y3, h3, t3; +// +// // ---- PICKUP 2 ---- +// private double x4a, y4a, h4a; +// private double x4b, y4b, h4b; +// +// // ---- PICKUP 3 ---- +// private double x5a, y5a, h5a; +// private double x5b, y5b, h5b; +// +// // ---- PARK ---- +// private double xPark, yPark, hPark; +// +// public Action prepareShootIndexed(double time) { +// return new Action() { +// double stamp = 0.0; +// int ticker = 0; +// +// boolean testColor = false; +// +// int s1Green = 0; +// int s2Green = 0; +// int s3Green = 0; +// +// +// @Override +// public boolean run(@NonNull TelemetryPacket telemetryPacket) { +// if (ticker == 0) { +// stamp = System.currentTimeMillis(); +// } +// ticker++; +// +// if ((System.currentTimeMillis() - stamp) < (goToDetectTime)) { //0.25-0.4 ish??? +// robot.spin1.setPosition(colorDetectPos); //0.43 +// robot.spin2.setPosition(1 - colorDetectPos); +// } else if ((System.currentTimeMillis() - stamp) < (colorSenseTime + goToDetectTime)) { +// +// +// double green1 = robot.color1.getNormalizedColors().green; +// double red1 = robot.color1.getNormalizedColors().red; +// double blue1 = robot.color1.getNormalizedColors().blue; +// +// double gP1 = green1 / (green1 + red1 + blue1); +// +// +// if (gP1 >= color1Thresh) { +// s1Green ++; +// } +// +// double green2 = robot.color2.getNormalizedColors().green; +// double red2 = robot.color2.getNormalizedColors().red; +// double blue2 = robot.color2.getNormalizedColors().blue; +// +// double gP2 = green2 / (green2 + red2 + blue2); +// +// if (gP2 >= color2Thresh) { +// s2Green ++; +// } +// +// +// double green3 = robot.color3.getNormalizedColors().green; +// double red3 = robot.color3.getNormalizedColors().red; +// double blue3 = robot.color3.getNormalizedColors().blue; +// +// double gP3 = green3 / (green3 + red3 + blue3); +// +// if (gP3 >= color3Thresh) { +// s3Green ++; +// } +// +// +// testColor = true; +// +// +// +// } else { +// +// double spindexPos; +// if (motif == 21) { +// spindexPos = +// } +// } +// +// robot.transferServo.setPosition(transferServo_out); +// +// turret.manualSetTurret(turretShootPos); +// +// robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000); +// +// return (System.currentTimeMillis() - stamp) < (time * 1000); +// +// } +// }; +// } +// +// public Action shootAll(int vel, double shootTime, double spindexSpeed) { +// return new Action() { +// int ticker = 1; +// +// double stamp = 0.0; +// +// double velo = vel; +// +// int shooterTicker = 0; +// +// @Override +// public boolean run(@NonNull TelemetryPacket telemetryPacket) { +// TELE.addData("Velocity", velo); +// TELE.addLine("shooting"); +// TELE.update(); +// +// flywheel.manageFlywheel(vel); +// velo = flywheel.getVelo(); +// +// drive.updatePoseEstimate(); +// +// teleStart = drive.localizer.getPose(); +// +// robot.intake.setPower(-0.3); +// +// if (ticker == 1) { +// stamp = getRuntime(); +// } +// ticker++; +// +// robot.intake.setPower(0); +// +// if (getRuntime() - stamp < shootTime) { +// +// if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { +// robot.spin1.setPosition(autoSpinStartPos); +// robot.spin2.setPosition(1 - autoSpinStartPos); +// } else { +// robot.transferServo.setPosition(transferServo_in); +// shooterTicker++; +// double prevSpinPos = robot.spin1.getPosition(); +// robot.spin1.setPosition(prevSpinPos + spindexSpeed); +// robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); +// } +// +// return true; +// +// } else { +// robot.transferServo.setPosition(transferServo_out); +// //spindexPos = spindexer_intakePos1; +// +// spindexer.resetSpindexer(); +// spindexer.processIntake(); +// +// return false; +// +// } +// +// } +// }; +// } +// +// public Action shootAllAuto(double shootTime, double spindexSpeed) { +// return new Action() { +// int ticker = 1; +// +// double stamp = 0.0; +// +// double velo = 0.0; +// +// int shooterTicker = 0; +// +// @Override +// public boolean run(@NonNull TelemetryPacket telemetryPacket) { +// TELE.addData("Velocity", velo); +// TELE.addLine("shooting"); +// TELE.update(); +// +// velo = flywheel.getVelo(); +// +// drive.updatePoseEstimate(); +// +// teleStart = drive.localizer.getPose(); +// +// robot.intake.setPower(-0.3); +// +// if (ticker == 1) { +// stamp = getRuntime(); +// } +// ticker++; +// +// robot.intake.setPower(0); +// +// if (getRuntime() - stamp < shootTime) { +// +// if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { +// robot.spin1.setPosition(autoSpinStartPos); +// robot.spin2.setPosition(1 - autoSpinStartPos); +// } else { +// robot.transferServo.setPosition(transferServo_in); +// shooterTicker++; +// double prevSpinPos = robot.spin1.getPosition(); +// robot.spin1.setPosition(prevSpinPos + spindexSpeed); +// robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); +// } +// +// return true; +// +// } else { +// robot.transferServo.setPosition(transferServo_out); +// //spindexPos = spindexer_intakePos1; +// +// spindexer.resetSpindexer(); +// spindexer.processIntake(); +// +// return false; +// +// } +// +// } +// }; +// } +// +// public Action intake(double intakeTime) { +// return new Action() { +// double stamp = 0.0; +// int ticker = 0; +// +// @Override +// public boolean run(@NonNull TelemetryPacket telemetryPacket) { +// if (ticker == 0) { +// stamp = System.currentTimeMillis(); +// } +// ticker++; +// +// spindexer.processIntake(); +// robot.intake.setPower(1); +// +// motif = turret.detectObelisk(); +// +// spindexer.ballCounterLight(); +// +// return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); +// +// } +// }; +// } +// +// public Action detectObelisk( +// double time, +// double posX, +// double posY, +// double posXTolerance, +// double posYTolerance, +// double turrPos +// ) { +// +// boolean timeFallback = (time != 0.501); +// boolean posXFallback = (posX != 0.501); +// boolean posYFallback = (posY != 0.501); +// +// return new Action() { +// +// double stamp = 0.0; +// int ticker = 0; +// +// @Override +// public boolean run(@NonNull TelemetryPacket telemetryPacket) { +// +// drive.updatePoseEstimate(); +// Pose2d currentPose = drive.localizer.getPose(); +// +// if (ticker == 0) { +// stamp = System.currentTimeMillis(); +// } +// +// ticker++; +// +// motif = turret.detectObelisk(); +// +// robot.turr1.setPosition(turrPos); +// robot.turr2.setPosition(1 - turrPos); +// +// boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; +// boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; +// boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; +// +// boolean shouldFinish = timeDone || xDone || yDone; +// +// return !shouldFinish; +// +// } +// }; +// } +// +// public Action manageFlywheel( +// double vel, +// double hoodPos, +// double time, +// double posX, +// double posY, +// double posXTolerance, +// double posYTolerance +// ) { +// +// boolean timeFallback = (time != 0.501); +// boolean posXFallback = (posX != 0.501); +// boolean posYFallback = (posY != 0.501); +// +// return new Action() { +// +// double stamp = 0.0; +// int ticker = 0; +// +// @Override +// public boolean run(@NonNull TelemetryPacket telemetryPacket) { +// +// drive.updatePoseEstimate(); +// Pose2d currentPose = drive.localizer.getPose(); +// +// if (ticker == 0) { +// stamp = System.currentTimeMillis(); +// } +// +// ticker++; +// +// flywheel.manageFlywheel(vel); +// robot.hood.setPosition(hoodPos); +// +// boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; +// boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; +// boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; +// +// boolean shouldFinish = timeDone || xDone || yDone; +// +// return !shouldFinish; +// +// } +// }; +// } +// +// public Action manageShooterAuto( +// double time, +// double posX, +// double posY, +// double posXTolerance, +// double posYTolerance +// ) { +// +// boolean timeFallback = (time != 0.501); +// boolean posXFallback = (posX != 0.501); +// boolean posYFallback = (posY != 0.501); +// +// return new Action() { +// +// double stamp = 0.0; +// int ticker = 0; +// +// @Override +// public boolean run(@NonNull TelemetryPacket telemetryPacket) { +// +// drive.updatePoseEstimate(); +// Pose2d currentPose = drive.localizer.getPose(); +// +// if (ticker == 0) { +// stamp = System.currentTimeMillis(); +// } +// +// ticker++; +// +// double robotX = drive.localizer.getPose().position.x; +// double robotY = drive.localizer.getPose().position.y; +// +// double robotHeading = drive.localizer.getPose().heading.toDouble(); +// +// double goalX = -15; +// double goalY = 0; +// +// double dx = robotX - goalX; // delta x from robot to goal +// double dy = robotY - goalY; // delta y from robot to goal +// Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); +// +// double distanceToGoal = Math.sqrt(dx * dx + dy * dy); +// +// targetingSettings = targeting.calculateSettings +// (robotX, robotY, robotHeading, 0.0, false); +// +// turret.trackGoal(deltaPose); +// +// robot.hood.setPosition(targetingSettings.hoodAngle); +// +// flywheel.manageFlywheel(targetingSettings.flywheelRPM); +// +// boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; +// boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; +// boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; +// +// boolean shouldFinish = timeDone || xDone || yDone; +// +// return !shouldFinish; +// +// } +// }; +// } +// +// public Action manageFlywheelAuto( +// double time, +// double posX, +// double posY, +// double posXTolerance, +// double posYTolerance +// ) { +// +// boolean timeFallback = (time != 0.501); +// boolean posXFallback = (posX != 0.501); +// boolean posYFallback = (posY != 0.501); +// +// return new Action() { +// +// double stamp = 0.0; +// int ticker = 0; +// +// @Override +// public boolean run(@NonNull TelemetryPacket telemetryPacket) { +// +// drive.updatePoseEstimate(); +// Pose2d currentPose = drive.localizer.getPose(); +// +// if (ticker == 0) { +// stamp = System.currentTimeMillis(); +// } +// +// ticker++; +// +// double robotX = drive.localizer.getPose().position.x; +// double robotY = drive.localizer.getPose().position.y; +// +// double robotHeading = drive.localizer.getPose().heading.toDouble(); +// +// double goalX = -15; +// double goalY = 0; +// +// double dx = robotX - goalX; // delta x from robot to goal +// double dy = robotY - goalY; // delta y from robot to goal +// Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); +// +// double distanceToGoal = Math.sqrt(dx * dx + dy * dy); +// +// targetingSettings = targeting.calculateSettings +// (robotX, robotY, robotHeading, 0.0, false); +// +// robot.hood.setPosition(targetingSettings.hoodAngle); +// +// flywheel.manageFlywheel(targetingSettings.flywheelRPM); +// +// boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; +// boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; +// boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; +// +// boolean shouldFinish = timeDone || xDone || yDone; +// +// return !shouldFinish; +// +// } +// }; +// } +// +// @Override +// public void runOpMode() throws InterruptedException { +// +// hardwareMap.get(Servo.class, "light").setPosition(0); +// +// robot = new Robot(hardwareMap); +// +// TELE = new MultipleTelemetry( +// telemetry, FtcDashboard.getInstance().getTelemetry() +// ); +// +// flywheel = new Flywheel(hardwareMap); +// +// targeting = new Targeting(); +// targetingSettings = new Targeting.Settings(0.0, 0.0); +// +// spindexer = new Spindexer(hardwareMap); +// +// servos = new Servos(hardwareMap); +// +// robot.limelight.pipelineSwitch(1); +// +// turret = new Turret(robot, TELE, robot.limelight); +// +// turret.manualSetTurret(0.4); +// +// drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); +// +// TrajectoryActionBuilder shoot0 = null; +// +// TrajectoryActionBuilder pickup1 = null; +// TrajectoryActionBuilder shoot1 = null; +// TrajectoryActionBuilder pickup2 = null; +// TrajectoryActionBuilder shoot2 = null; +// TrajectoryActionBuilder pickup3 = null; +// TrajectoryActionBuilder shoot3 = null; +// TrajectoryActionBuilder openGate = null; +// TrajectoryActionBuilder park = null; +// +// robot.spin1.setPosition(autoSpinStartPos); +// robot.spin2.setPosition(1 - autoSpinStartPos); +// +// robot.transferServo.setPosition(transferServo_out); +// +// robot.light.setPosition(1); +// +// while (opModeInInit()) { +// +// robot.hood.setPosition(shoot0Hood); +// +// if (gamepad2.crossWasPressed()) { +// redAlliance = !redAlliance; +// } +// +// if (redAlliance) { +// robot.light.setPosition(0.28); +// +// // ===== FIRST SHOT ===== +// x1 = rx1; +// y1 = ry1; +// h1 = rh1; +// +// // ===== PICKUP PATH ===== +// x2a = rx2a; +// y2a = ry2a; +// h2a = rh2a; +// t2a = rt2a; +// +// x2b = rx2b; +// y2b = ry2b; +// h2b = rh2b; +// t2b = rt2b; +// +// x2c = rx2c; +// y2c = ry2c; +// h2c = rh2c; +// t2c = rt2c; +// +// // ===== SHOOT POSE ===== +// xShoot = rShootX; +// yShoot = rShootY; +// hShoot = rShootH; +// shoot1Tangent = rShoot1Tangent; +// +// // ===== GATE ===== +// xGate = rXGate; +// yGate = rYGate; +// hGate = rHGate; +// +// } else { +// robot.light.setPosition(0.6); +// +// // ===== FIRST SHOT ===== +// x1 = bx1; +// y1 = by1; +// h1 = bh1; +// +// // ===== PICKUP PATH ===== +// x2a = bx2a; +// y2a = by2a; +// h2a = bh2a; +// t2a = bt2a; +// +// x2b = bx2b; +// y2b = by2b; +// h2b = bh2b; +// t2b = bt2b; +// +// x2c = bx2c; +// y2c = by2c; +// h2c = bh2c; +// t2c = bt2c; +// +// // ===== SHOOT POSE ===== +// xShoot = bShootX; +// yShoot = bShootY; +// hShoot = bShootH; +// shoot1Tangent = bShoot1Tangent; +// +// // ===== GATE ===== +// xGate = bXGate; +// yGate = bYGate; +// hGate = bHGate; +// } +// +// shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) +// .strafeToLinearHeading(new Vector2d(x1, y1), h1); +// +// pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) +// .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) +// .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, +// new TranslationalVelConstraint(pickup1Speed)); +// +// openGate = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) +// .setReversed(true) +// .splineToConstantHeading(new Vector2d(x3, y3), t3); +// +// shoot1 = drive.actionBuilder(new Pose2d(x3, y3, h2b)) +// .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); +// +// pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) +// .strafeToLinearHeading(new Vector2d(x4a, y4a), h4a) +// .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, +// new TranslationalVelConstraint(pickup2Speed)); +// shoot2 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) +// .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); +// +// pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) +// .strafeToLinearHeading(new Vector2d(x5a, y5a), h5a) +// .strafeToLinearHeading(new Vector2d(x5b, y5b), h5b, +// new TranslationalVelConstraint(pickup3Speed)); +// shoot3 = drive.actionBuilder(new Pose2d(x5b, y5b, h5b)) +// .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); +// park = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) +// .strafeToLinearHeading(new Vector2d(xPark, yPark), hPark); +// +// } +// +// waitForStart(); +// +// if (isStopRequested()) return; +// +// if (opModeIsActive()) { +// +// robot.transfer.setPower(1); +// +// assert shoot0 != null; +// +// Actions.runBlocking( +// new ParallelAction( +// shoot0.build(), +// manageFlywheel( +// shoot0Vel, +// shoot0Hood, +// flywheel0Time, +// x1, +// 0.501, +// shoot0XTolerance, +// 0.501 +// ) +// +// ) +// ); +// +// Actions.runBlocking( +// shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) +// ); +// +// //WORKING SHOOT ALL for the preload +// +// //Pickup first pile +// +// Actions.runBlocking( +// new SequentialAction( +// new ParallelAction( +// pickup1.build(), +// manageFlywheel( +// shootAllVelocity, +// shootAllHood, +// pickup1Time, +// x2b, +// y2b, +// pickup1XTolerance, +// pickup1YTolerance +// ), +// intake(intake1Time), +// detectObelisk( +// obelisk1Time, +// x2b, +// y2c, +// obelisk1XTolerance, +// obelisk1YTolerance, +// obeliskTurrPos +// ) +// ), +// new ParallelAction( +// openGate.build() //TODO: Add other managing stuff here +// ) +// ) +// +// ); +// +// motif = turret.detectObelisk(); //Detect Obelisk if not alr +// +// Actions.runBlocking( +// new ParallelAction( +// manageFlywheel( +// shootAllVelocity, +// shootAllHood, +// shoot1Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// shoot1.build(), +// prepareShootIndexed(shoot1Time) +// ) +// ); +// +// Actions.runBlocking( +// new ParallelAction( +// manageShooterAuto( +// shootAllTime, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// shootAllIndexed(shootAllTime, spindexerSpeedIncrease) +// ) +// +// ); +// +// //Shoot from first pile +// +// } +// +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java new file mode 100644 index 0000000..9e00607 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java @@ -0,0 +1,901 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; +import static org.firstinspires.ftc.teamcode.constants.Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; +import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ParallelAction; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TranslationalVelConstraint; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.constants.Poses_V2; +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 org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; + +@Config +@Autonomous(preselectTeleOp = "TeleopV3") +public class ProtoAutoClose_V4 extends LinearOpMode { + public static double shoot0Vel = 2300, shoot0Hood = 0.93; + public static double autoSpinStartPos = 0.2; + public static double shoot0SpinSpeedIncrease = 0.014; + + public static double spindexerSpeedIncrease = 0.02; + + public static double obeliskTurrPos = 0.53; + public static double normalIntakeTime = 3.0; + public static double shoot1Turr = 0.57; + public static double shoot0XTolerance = 1.0; + public static double turretShootPos = 0.6; + public static double shootAllTime = 1.8; + public static double shoot0Time = 1.6; + public static double intake1Time = 3.0; + public static double flywheel0Time = 3.5; + public static double pickup1Speed = 20; + // ---- SECOND SHOT / PICKUP ---- + public static double shoot1Vel = 2300; + public static double shoot1Hood = 0.93; + + public static double shootAllVelocity = 2500; + public static double shootAllHood = 0.78; + // ---- PICKUP TIMING ---- + public static double pickup1Time = 3.0; + // ---- PICKUP POSITION TOLERANCES ---- + public static double pickup1XTolerance = 2.0; + public static double pickup1YTolerance = 2.0; + // ---- OBELISK DETECTION ---- + public static double obelisk1Time = 1.5; + public static double obelisk1XTolerance = 2.0; + public static double obelisk1YTolerance = 2.0; + public static double shoot1ToleranceX = 2.0; + public static double shoot1ToleranceY = 2.0; + public static double shoot1Time = 2.0; + public static double shootCycleTime = 2.5; + public int motif = 0; + Robot robot; + MultipleTelemetry TELE; + MecanumDrive drive; + Servos servos; + Spindexer spindexer; + Flywheel flywheel; + Turret turret; + Targeting targeting; + Targeting.Settings targetingSettings; + private double x1, y1, h1; + + private double x2a, y2a, h2a, t2a; + + private double x2b, y2b, h2b, t2b; + private double x2c, y2c, h2c, t2c; + + private double x3a, y3a, h3a; + private double x3b, y3b, h3b; + private double x4a, y4a, h4a; + private double x4b, y4b, h4b; + + private double xShoot, yShoot, hShoot; + private double xGate, yGate, hGate; + private double xPrep, yPrep, hPrep; + + private double shoot1Tangent; + + + public Action prepareShootAll(double time) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + ticker++; + + robot.spin1.setPosition(autoSpinStartPos); + robot.spin2.setPosition(1 - autoSpinStartPos); + + robot.transferServo.setPosition(transferServo_out); + + turret.manualSetTurret(turretShootPos); + + robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + return (System.currentTimeMillis() - stamp) < (time * 1000); + + } + }; + } + + public Action shootAll(int vel, double shootTime, double spindexSpeed) { + return new Action() { + int ticker = 1; + + double stamp = 0.0; + + double velo = vel; + + int shooterTicker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", velo); + TELE.addLine("shooting"); + TELE.update(); + + flywheel.manageFlywheel(vel); + velo = flywheel.getVelo(); + + drive.updatePoseEstimate(); + + Poses_V2.teleStart = drive.localizer.getPose(); + + robot.intake.setPower(-0.3); + + if (ticker == 1) { + stamp = getRuntime(); + } + ticker++; + + robot.intake.setPower(0); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + if (getRuntime() - stamp < shootTime) { + + if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { + robot.spin1.setPosition(autoSpinStartPos); + robot.spin2.setPosition(1 - autoSpinStartPos); + } else { + robot.transferServo.setPosition(transferServo_in); + shooterTicker++; + double prevSpinPos = robot.spin1.getPosition(); + robot.spin1.setPosition(prevSpinPos + spindexSpeed); + robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + } + + return true; + + } else { + robot.transferServo.setPosition(transferServo_out); + //spindexPos = spindexer_intakePos1; + + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + + } + }; + } + + public Action shootAllAuto(double shootTime, double spindexSpeed) { + return new Action() { + int ticker = 1; + + double stamp = 0.0; + + double velo = 0.0; + + int shooterTicker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", velo); + TELE.addLine("shooting"); + TELE.update(); + + velo = flywheel.getVelo(); + + drive.updatePoseEstimate(); + + Poses_V2.teleStart = drive.localizer.getPose(); + + robot.intake.setPower(-0.3); + + if (ticker == 1) { + stamp = getRuntime(); + } + ticker++; + + robot.intake.setPower(0); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + if (getRuntime() - stamp < shootTime) { + + if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { + robot.spin1.setPosition(autoSpinStartPos); + robot.spin2.setPosition(1 - autoSpinStartPos); + } else { + robot.transferServo.setPosition(transferServo_in); + shooterTicker++; + double prevSpinPos = robot.spin1.getPosition(); + robot.spin1.setPosition(prevSpinPos + spindexSpeed); + robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + } + + return true; + + } else { + robot.transferServo.setPosition(transferServo_out); + //spindexPos = spindexer_intakePos1; + + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + + } + }; + } + + public Action intake(double intakeTime) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + ticker++; + + spindexer.processIntake(); + robot.intake.setPower(1); + + motif = turret.detectObelisk(); + + spindexer.ballCounterLight(); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); + + } + }; + } + + public Action detectObelisk( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance, + double turrPos + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + motif = turret.detectObelisk(); + + robot.turr1.setPosition(turrPos); + robot.turr2.setPosition(1 - turrPos); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + return !shouldFinish; + + } + }; + } + + public Action manageFlywheel( + double vel, + double hoodPos, + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + flywheel.manageFlywheel(vel); + robot.hood.setPosition(hoodPos); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + return !shouldFinish; + + } + }; + } + + public Action manageShooterAuto( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + double robotX = drive.localizer.getPose().position.x; + double robotY = drive.localizer.getPose().position.y; + + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robotX - goalX; // delta x from robot to goal + double dy = robotY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robotX, robotY, robotHeading, 0.0, false); + + turret.trackGoal(deltaPose); + + robot.hood.setPosition(targetingSettings.hoodAngle); + + flywheel.manageFlywheel(targetingSettings.flywheelRPM); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + return !shouldFinish; + + } + }; + } + + public Action manageFlywheelAuto( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + double robotX = drive.localizer.getPose().position.x; + double robotY = drive.localizer.getPose().position.y; + + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robotX - goalX; // delta x from robot to goal + double dy = robotY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robotX, robotY, robotHeading, 0.0, false); + + + robot.hood.setPosition(targetingSettings.hoodAngle); + + flywheel.manageFlywheel(targetingSettings.flywheelRPM); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + return !shouldFinish; + + } + }; + } + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); + + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + flywheel = new Flywheel(hardwareMap); + + targeting = new Targeting(); + targetingSettings = new Targeting.Settings(0.0, 0.0); + + spindexer = new Spindexer(hardwareMap); + + servos = new Servos(hardwareMap); + + robot.limelight.pipelineSwitch(1); + + turret = new Turret(robot, TELE, robot.limelight); + + turret.manualSetTurret(0.4); + + drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + robot.spin1.setPosition(autoSpinStartPos); + robot.spin2.setPosition(1 - autoSpinStartPos); + + robot.transferServo.setPosition(transferServo_out); + + TrajectoryActionBuilder shoot0 = null; + TrajectoryActionBuilder pickup1 = null; + TrajectoryActionBuilder shoot1 = null; + TrajectoryActionBuilder pickup2 = null; + TrajectoryActionBuilder shoot2 = null; + TrajectoryActionBuilder pickup3 = null; + TrajectoryActionBuilder shoot3 = null; + + robot.light.setPosition(1); + + while (opModeInInit()) { + + robot.hood.setPosition(shoot0Hood); + + if (gamepad2.crossWasPressed()) { + redAlliance = !redAlliance; + } + + if (redAlliance) { + robot.light.setPosition(0.28); + + // ---- FIRST SHOT ---- + x1 = rx1; + y1 = ry1; + h1 = rh1; + + // ---- PICKUP PATH ---- + x2a = rx2a; + y2a = ry2a; + h2a = rh2a; + x2b = rx2b; + y2b = ry2b; + h2b = rh2b; + x3a = rx3a; + y3a = ry3a; + h3a = rh3a; + x3b = rx3b; + y3b = ry3b; + h3b = rh3b; + x4a = rx4a; + y4a = ry4a; + h4a = rh4a; + x4b = rx4b; + y4b = ry4b; + h4b = rh4b; + xPrep = rxPrep; + yPrep = ryPrep; + hPrep = rhPrep; + xShoot = rShootX; + yShoot = rShootY; + hShoot = rShootH; + + + } else { + robot.light.setPosition(0.6); + + // ---- FIRST SHOT ---- + x1 = bx1; + y1 = by1; + h1 = bh1; + + // ---- PICKUP PATH ---- + x2a = bx2a; + y2a = by2a; + h2a = bh2a; + x2b = bx2b; + y2b = by2b; + h2b = bh2b; + x3a = bx3a; + y3a = by3a; + h3a = bh3a; + x3b = bx3b; + y3b = by3b; + h3b = bh3b; + x4a = bx4a; + y4a = by4a; + h4a = bh4a; + x4b = bx4b; + y4b = by4b; + h4b = bh4b; + + xPrep = bxPrep; + yPrep = byPrep; + hPrep = bhPrep; + xShoot = bShootX; + yShoot = bShootY; + hShoot = bShootH; + + } + + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1); + + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) + .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, + new TranslationalVelConstraint(pickup1Speed)); + + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) + .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + + pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) + .strafeToLinearHeading(new Vector2d(x3a, y3a), h3a) + .strafeToLinearHeading(new Vector2d(x3b, y3b), h3b, + new TranslationalVelConstraint(pickup1Speed)); + + shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b)) + .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + + pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + .strafeToLinearHeading(new Vector2d(x4a, y4a), h4a) + .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, + new TranslationalVelConstraint(pickup1Speed)); + shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) + .strafeToLinearHeading(new Vector2d(xPrep, yPrep), hPrep) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + + + + TELE.addData("Red?", redAlliance); + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + + if (opModeIsActive()) { + + robot.transfer.setPower(1); + + assert shoot0 != null; + + Actions.runBlocking( + new ParallelAction( + shoot0.build(), + manageFlywheel( + shoot0Vel, + shoot0Hood, + flywheel0Time, + x1, + 0.501, + shoot0XTolerance, + 0.501 + ) + + ) + ); + + Actions.runBlocking( + shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) + ); + + Actions.runBlocking( + new ParallelAction( + pickup1.build(), + manageFlywheel( + shootAllVelocity, + shootAllHood, + pickup1Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake1Time) + + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shootAllVelocity, + shootAllHood, + shoot1Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot1.build(), + prepareShootAll(shoot1Time) + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + + ); + + Actions.runBlocking( + new ParallelAction( + pickup2.build(), + manageShooterAuto( + normalIntakeTime, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(normalIntakeTime) + + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shootCycleTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot2.build(), + prepareShootAll(shootCycleTime) + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + + ); + + Actions.runBlocking( + new ParallelAction( + pickup3.build(), + manageShooterAuto( + normalIntakeTime, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(normalIntakeTime) + + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shootCycleTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot3.build(), + prepareShootAll(shootCycleTime) + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + + ); + + + TELE.addLine("finished"); + TELE.update(); + + sleep(2000); + + } + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index bf2fa34..94196a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -14,7 +14,7 @@ public class Poses { public static Pose2d goalPose = new Pose2d(-10, 0, 0); - public static double rx1 = 40, ry1 = -7, rh1 = 0; + public static double rx1 = 20, ry1 = 0, rh1 = 0; public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140); @@ -23,8 +23,8 @@ public class Poses { public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140); - public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140); - public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140); + public static double rx4a = 68, ry4a = 59, rh4a = Math.toRadians(140); + public static double rx4b = 40, ry4b = 87, rh4b = Math.toRadians(140); public static double bx1 = 40, by1 = 7, bh1 = 0; public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140); @@ -38,6 +38,13 @@ public class Poses { public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140); public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this + public static double rShootX = 40, rShootY = 0, rShootH = Math.toRadians(50); + public static double rxPrep = 50, ryPrep = 10, rhPrep = Math.toRadians(50); + + public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140); + public static double bxPrep = 50, byPrep = -10, bhPrep = Math.toRadians(140); + + public static Pose2d teleStart = new Pose2d(0, 0, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java index 40d48c8..d2f9547 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java @@ -201,9 +201,9 @@ public final class MecanumDrive { public double kA = 0.000046; // path profile parameters (in inches) - public double maxWheelVel = 250; + public double maxWheelVel = 180; public double minProfileAccel = -40; - public double maxProfileAccel = 250; + public double maxProfileAccel = 180; // turn profile parameters (in radians) public double maxAngVel = 4 * Math.PI; // shared with path @@ -496,4 +496,4 @@ public final class MecanumDrive { c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2); } } -} +} \ No newline at end of file From 66bb5c747f0d3f1d051e60eb49ef44e6052819e8 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Wed, 28 Jan 2026 19:42:08 -0600 Subject: [PATCH 12/18] before merge --- .../autonomous/ProtoAutoClose_V4.java | 4 +- .../ftc/teamcode/libs/RR/MecanumDrive.java | 277 +++++++++--------- 2 files changed, 142 insertions(+), 139 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java index 9e00607..b06cae2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java @@ -97,7 +97,7 @@ public class ProtoAutoClose_V4 extends LinearOpMode { public static double normalIntakeTime = 3.0; public static double shoot1Turr = 0.57; public static double shoot0XTolerance = 1.0; - public static double turretShootPos = 0.6; + public static double turretShootPos = 0.5; public static double shootAllTime = 1.8; public static double shoot0Time = 1.6; public static double intake1Time = 3.0; @@ -609,6 +609,8 @@ public class ProtoAutoClose_V4 extends LinearOpMode { TrajectoryActionBuilder pickup3 = null; TrajectoryActionBuilder shoot3 = null; + robot.limelight.start(); + robot.light.setPosition(1); while (opModeInInit()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java index d2f9547..3b465db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java @@ -5,9 +5,7 @@ import androidx.annotation.NonNull; import com.acmerobotics.dashboard.canvas.Canvas; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.AccelConstraint; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.Actions; +import com.acmerobotics.roadrunner.*; import com.acmerobotics.roadrunner.AngularVelConstraint; import com.acmerobotics.roadrunner.DualNum; import com.acmerobotics.roadrunner.HolonomicController; @@ -16,20 +14,12 @@ import com.acmerobotics.roadrunner.MinVelConstraint; import com.acmerobotics.roadrunner.MotorFeedforward; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2dDual; -import com.acmerobotics.roadrunner.PoseVelocity2d; -import com.acmerobotics.roadrunner.PoseVelocity2dDual; import com.acmerobotics.roadrunner.ProfileAccelConstraint; -import com.acmerobotics.roadrunner.ProfileParams; -import com.acmerobotics.roadrunner.Rotation2d; import com.acmerobotics.roadrunner.Time; import com.acmerobotics.roadrunner.TimeTrajectory; import com.acmerobotics.roadrunner.TimeTurn; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.TrajectoryBuilderParams; import com.acmerobotics.roadrunner.TurnConstraints; -import com.acmerobotics.roadrunner.Twist2d; -import com.acmerobotics.roadrunner.Twist2dDual; -import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.VelConstraint; import com.acmerobotics.roadrunner.ftc.DownsampledWriter; import com.acmerobotics.roadrunner.ftc.Encoder; @@ -56,131 +46,13 @@ import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumCommandMessage; import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage; import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage; +import java.lang.Math; import java.util.Arrays; import java.util.LinkedList; import java.util.List; @Config public final class MecanumDrive { - public static Params PARAMS = new Params(); - public final MecanumKinematics kinematics = new MecanumKinematics( - PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick); - public final TurnConstraints defaultTurnConstraints = new TurnConstraints( - PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); - public final VelConstraint defaultVelConstraint = - new MinVelConstraint(Arrays.asList( - kinematics.new WheelVelConstraint(PARAMS.maxWheelVel), - new AngularVelConstraint(PARAMS.maxAngVel) - )); - public final AccelConstraint defaultAccelConstraint = - new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); - public final DcMotorEx leftFront, leftBack, rightBack, rightFront; - public final VoltageSensor voltageSensor; - public final LazyImu lazyImu; - public final Localizer localizer; - private final LinkedList poseHistory = new LinkedList<>(); - private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000); - private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000); - private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000); - private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000); - public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { - LynxFirmware.throwIfModulesAreOutdated(hardwareMap); - - for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { - module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); - } - - // TODO: make sure your config has motors with these names (or change them) - // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html - leftFront = hardwareMap.get(DcMotorEx.class, "fl"); - leftBack = hardwareMap.get(DcMotorEx.class, "bl"); - rightBack = hardwareMap.get(DcMotorEx.class, "br"); - rightFront = hardwareMap.get(DcMotorEx.class, "fr"); - - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // TODO: reverse motor directions if needed - // - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - - leftBack.setDirection(DcMotorSimple.Direction.REVERSE); - - // TODO: make sure your config has an IMU with this name (can be BNO or BHI) - // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html - lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot( - PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); - - voltageSensor = hardwareMap.voltageSensor.iterator().next(); - - localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose); - - FlightRecorder.write("MECANUM_PARAMS", PARAMS); - } - - public void setDrivePowers(PoseVelocity2d powers) { - MecanumKinematics.WheelVelocities