From c42fce2e78e80848a44d7be8e1f957b85427343a Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 27 Feb 2026 19:01:33 -0600 Subject: [PATCH 01/24] untested edits of autos --- .../teamcode/autonomous/Auto_LT_Close.java | 25 ++- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 210 ++++++++++++++---- .../ftc/teamcode/constants/Back_Poses.java | 12 +- 3 files changed, 185 insertions(+), 62 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 34d7bef..36050fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -134,6 +134,7 @@ public class Auto_LT_Close extends LinearOpMode { double obeliskTurrPos3 = 0.0; double waitToPickupGate = 0; double obeliskTurrPosAutoStart = 0; + boolean limelightStart = false; // initialize path variables here TrajectoryActionBuilder shoot0 = null; @@ -185,7 +186,7 @@ public class Auto_LT_Close extends LinearOpMode { hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU(); while (opModeInInit()) { - if (limelightUsed && !gateCycle){ + if (limelightUsed && !gateCycle && limelightStart){ Actions.runBlocking( autoActions.detectObelisk( 0.1, @@ -207,14 +208,6 @@ public class Auto_LT_Close extends LinearOpMode { } } - if (!gateCycle) { - turret.pipelineSwitch(1); - } else if (redAlliance) { - turret.pipelineSwitch(4); - } else { - turret.pipelineSwitch(2); - } - if (gateCycle) { servos.setHoodPos(hoodGate0Start); } else { @@ -249,8 +242,10 @@ public class Auto_LT_Close extends LinearOpMode { if (gamepad2.squareWasPressed()) { drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); + sleep(100); robot.limelight.start(); limelightUsed = true; + limelightStart = true; gamepad2.rumble(500); } @@ -258,6 +253,12 @@ public class Auto_LT_Close extends LinearOpMode { if (redAlliance) { robot.light.setPosition(0.28); + if (gateCycle){ + turret.pipelineSwitch(1); + } else { + turret.pipelineSwitch(4); + } + // ---- FIRST SHOT ---- x1 = rx1; y1 = ry1; @@ -314,6 +315,12 @@ public class Auto_LT_Close extends LinearOpMode { } else { robot.light.setPosition(0.6); + if (gateCycle){ + turret.pipelineSwitch(5); + } else { + turret.pipelineSwitch(2); + } + // ---- FIRST SHOT ---- x1 = bx1; y1 = by1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index fbb7d67..eb7dd17 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -12,6 +12,9 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObelisk import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; @@ -57,12 +60,17 @@ public class Auto_LT_Far extends LinearOpMode { AutoActions autoActions; Light light; double xShoot, yShoot, hShoot; - double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; + double pickupGateXA = 0, pickupGateYA = 0, pickupGateHA = 0; + double pickupGateXB = 0, pickupGateYB = 0, pickupGateHB = 0; + double pickupGateXC = 0, pickupGateYC = 0, pickupGateHC = 0; public static double flywheel0Time = 1.5; boolean gatePickup = true; boolean stack3 = true; - double xStackPickupA, yStackPickupA, hStackPickupA; - double xStackPickupB, yStackPickupB, hStackPickupB; + boolean stack2 = true; + double xStackPickupFarA, yStackPickupFarA, hStackPickupFarA; + double xStackPickupFarB, yStackPickupFarB, hStackPickupFarB; + double xStackPickupMiddleA, yStackPickupMiddleA, hStackPickupMiddleA; + double xStackPickupMiddleB, yStackPickupMiddleB, hStackPickupMiddleB; public static int pickupStackSpeed = 17; public static int pickupGateSpeed = 25; int prevMotif = 0; @@ -88,6 +96,7 @@ public class Auto_LT_Far extends LinearOpMode { TrajectoryActionBuilder shoot3 = null; TrajectoryActionBuilder pickupGate = null; TrajectoryActionBuilder shootGate = null; + TrajectoryActionBuilder pickup2 = null; Pose2d autoStart = new Pose2d(0,0,0); @Override @@ -124,14 +133,40 @@ public class Auto_LT_Far extends LinearOpMode { servos.setTransferPos(transferServo_out); - while (opModeInInit()) { + limelightUsed = false; - if (gamepad2.leftBumperWasPressed()){ + while (opModeInInit()) { + if (limelightUsed){ + Actions.runBlocking( + autoActions.detectObelisk( + 0.1, + 0.501, + 0.501, + 0.501, + 0.501, + turrDefault + ) + ); + motif = turret.getObeliskID(); + + if (motif == 21){ + AutoActions.firstSpindexShootPos = spindexer_outtakeBall1; + } else if (motif == 22){ + AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b; + } else { + AutoActions.firstSpindexShootPos = spindexer_outtakeBall2; + } + } + + if (gamepad2.triangleWasPressed()){ gatePickup = !gatePickup; } if (gamepad2.rightBumperWasPressed()){ stack3 = !stack3; } + if (gamepad2.leftBumperWasPressed()){ + stack2 = !stack2; + } turret.setTurret(turretShootPos); @@ -164,17 +199,33 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = rShootY; hShoot = rShootH; - xStackPickupA = rStackPickupAX; - yStackPickupA = rStackPickupAY; - hStackPickupA = rStackPickupAH; + xStackPickupFarA = rStackPickupAX; + yStackPickupFarA = rStackPickupAY; + hStackPickupFarA = rStackPickupAH; - xStackPickupB = rStackPickupBX; - yStackPickupB = rStackPickupBY; - hStackPickupB = rStackPickupBH; + xStackPickupFarB = rStackPickupBX; + yStackPickupFarB = rStackPickupBY; + hStackPickupFarB = rStackPickupBH; - pickupGateX = rPickupGateX; - pickupGateY = rPickupGateY; - pickupGateH = rPickupGateH; + xStackPickupMiddleA = rStackPickupAX; + yStackPickupMiddleA = rStackPickupAY; + hStackPickupMiddleA = rStackPickupAH; + + xStackPickupMiddleB = rStackPickupBX; + yStackPickupMiddleB = rStackPickupBY; + hStackPickupMiddleB = rStackPickupBH; + + pickupGateXA = rPickupGateXA; + pickupGateYA = rPickupGateYA; + pickupGateHA = rPickupGateHA; + + pickupGateXB = rPickupGateXB; + pickupGateYB = rPickupGateYB; + pickupGateHB = rPickupGateHB; + + pickupGateXC = rPickupGateXC; + pickupGateYC = rPickupGateYC; + pickupGateHC = rPickupGateHC; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -182,10 +233,9 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + redTurretShootPos; if (gamepad2.squareWasPressed()){ - turret.pipelineSwitch(4); + turret.pipelineSwitch(1); robot.limelight.start(); - drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); - + limelightUsed = true; gamepad2.rumble(500); } } else { @@ -203,17 +253,33 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = bShootY; hShoot = bShootH; - xStackPickupA = bStackPickupAX; - yStackPickupA = bStackPickupAY; - hStackPickupA = bStackPickupAH; + xStackPickupFarA = bStackPickupAX; + yStackPickupFarA = bStackPickupAY; + hStackPickupFarA = bStackPickupAH; - xStackPickupB = bStackPickupBX; - yStackPickupB = bStackPickupBY; - hStackPickupB = bStackPickupBH; + xStackPickupFarB = bStackPickupBX; + yStackPickupFarB = bStackPickupBY; + hStackPickupFarB = bStackPickupBH; - pickupGateX = bPickupGateX; - pickupGateY = bPickupGateY; - pickupGateH = bPickupGateH; + xStackPickupMiddleA = bStackPickupAX; + yStackPickupMiddleA = bStackPickupAY; + hStackPickupMiddleA = bStackPickupAH; + + xStackPickupMiddleB = bStackPickupBX; + yStackPickupMiddleB = bStackPickupBY; + hStackPickupMiddleB = bStackPickupBH; + + pickupGateXA = bPickupGateXA; + pickupGateYA = bPickupGateYA; + pickupGateHA = bPickupGateHA; + + pickupGateXB = bPickupGateXB; + pickupGateYB = bPickupGateYB; + pickupGateHB = bPickupGateHB; + + pickupGateXC = bPickupGateXC; + pickupGateYC = bPickupGateYC; + pickupGateHC = bPickupGateHC; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; @@ -221,10 +287,9 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + blueTurretShootPos; if (gamepad2.squareWasPressed()){ - turret.pipelineSwitch(2); + turret.pipelineSwitch(5); robot.limelight.start(); - drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); - + limelightUsed = true; gamepad2.rumble(500); } } @@ -236,29 +301,33 @@ public class Auto_LT_Far extends LinearOpMode { .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA)) - .strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB), + .strafeToLinearHeading(new Vector2d(xStackPickupFarA, yStackPickupFarA), Math.toRadians(hStackPickupFarA)) + .strafeToLinearHeading(new Vector2d(xStackPickupFarB, yStackPickupFarB), Math.toRadians(hStackPickupFarB), new TranslationalVelConstraint(pickupStackSpeed)); - shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB))) + pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(xStackPickupMiddleA, yStackPickupMiddleA), Math.toRadians(hStackPickupMiddleA)) + .strafeToLinearHeading(new Vector2d(xStackPickupMiddleB, yStackPickupMiddleB), Math.toRadians(hStackPickupMiddleB), + new TranslationalVelConstraint(pickupStackSpeed)); + + shoot3 = drive.actionBuilder(new Pose2d(xStackPickupFarB, yStackPickupFarB, Math.toRadians(hStackPickupFarB))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH)) + .strafeToLinearHeading(new Vector2d(pickupGateXA, pickupGateYA), Math.toRadians(pickupGateHA)) .waitSeconds(0.2) .strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB)) .strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC), new TranslationalVelConstraint(pickupGateSpeed)); - shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) + shootGate = drive.actionBuilder(new Pose2d(pickupGateXC, pickupGateYC, Math.toRadians(pickupGateHC))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); - limelightUsed = true; - TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Gate Cycle?", gatePickup); - TELE.addData("Pickup Stack?", stack3); + TELE.addData("Pickup Stack Far?", stack3); + TELE.addData("Pickup Stack Middle?", stack2); TELE.addData("Start Position", autoStart); TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift TELE.update(); @@ -282,6 +351,11 @@ public class Auto_LT_Far extends LinearOpMode { shoot(); } + if (stack2){ + cycleStackMiddle(); + shoot(); + } + while (gatePickup && getRuntime() - stamp < endAutoTime){ cycleGatePickupBalls(); if (getRuntime() - stamp > endAutoTime){ @@ -358,14 +432,14 @@ public class Auto_LT_Far extends LinearOpMode { pickup3.build(), autoActions.intake( intakeStackTime, - xStackPickupB, - yStackPickupB, - hStackPickupB + xStackPickupFarB, + yStackPickupFarB, + hStackPickupFarB ), autoActions.detectObelisk( intakeStackTime, - xStackPickupB, - yStackPickupB, + xStackPickupFarB, + yStackPickupFarB, posXTolerance, posYTolerance, obeliskTurrPos3 @@ -393,20 +467,62 @@ public class Auto_LT_Far extends LinearOpMode { ); } + + void cycleStackMiddle(){ + Actions.runBlocking( + new ParallelAction( + pickup2.build(), + autoActions.intake( + intakeStackTime, + xStackPickupMiddleB, + yStackPickupMiddleB, + hStackPickupMiddleB + ), + autoActions.detectObelisk( + intakeStackTime, + xStackPickupMiddleB, + yStackPickupMiddleB, + posXTolerance, + posYTolerance, + obeliskTurrPos2 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + shoot3.build(), + autoActions.prepareShootAll( + colorSenseTime, + shootStackTime, + motif, + xShoot, + yShoot, + hShoot) + ) + ); + } + void cycleGatePickupBalls(){ Actions.runBlocking( new ParallelAction( pickupGate.build(), autoActions.intake( intakeStackTime, - pickupGateX, - pickupGateY, - pickupGateH + pickupGateXC, + pickupGateYC, + pickupGateHC ), autoActions.detectObelisk( intakeGateTime, - pickupGateX, - pickupGateY, + pickupGateXC, + pickupGateYC, posXTolerance, posYTolerance, obeliskTurrPos3 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index c5fcfd3..1b1af7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -16,12 +16,12 @@ public class Back_Poses { public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1; public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1; - public static double rPickupGateX = 50, rPickupGateY = 83, rPickupGateH = 140; - public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140; - public static double pickupGateXB = 84, pickupGateYB = 76, pickupGateHB = 140; - public static double pickupGateXC = 50, pickupGateYC = 83, pickupGateHC = 190; - - + public static double rPickupGateXA = 50, rPickupGateYA = 83, rPickupGateHA = 140; + public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -140; + public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 140; + public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -140; + public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190; + public static double bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190; public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50; From f9013f4d792b96b8c1e662498d331771e0c7cd57 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 27 Feb 2026 23:34:22 -0600 Subject: [PATCH 02/24] stash --- .../teamcode/autonomous/Auto_LT_Close.java | 1 + .../ftc/teamcode/autonomous/Auto_LT_Far.java | 268 ++++-------------- .../autonomous/actions/AutoActions.java | 42 ++- .../ftc/teamcode/constants/Back_Poses.java | 30 +- .../teamcode/constants/ServoPositions.java | 2 +- .../ftc/teamcode/teleop/TeleopV3.java | 29 +- .../ftc/teamcode/tests/SortingTest.java | 85 +++--- .../ftc/teamcode/utils/Drivetrain.java | 34 ++- .../ftc/teamcode/utils/Light.java | 12 +- .../ftc/teamcode/utils/Spindexer.java | 36 ++- .../ftc/teamcode/utils/Turret.java | 28 +- 11 files changed, 251 insertions(+), 316 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 36050fb..1ac064d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -180,6 +180,7 @@ public class Auto_LT_Close extends LinearOpMode { servos.setTransferPos(transferServo_out); limelightUsed = false; + Spindexer.teleop = false; robot.light.setPosition(1); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index eb7dd17..5d7a25d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -1,6 +1,11 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarAY; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBX; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bStackPickupFarBY; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1; @@ -12,9 +17,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObelisk import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; @@ -60,19 +62,14 @@ public class Auto_LT_Far extends LinearOpMode { AutoActions autoActions; Light light; double xShoot, yShoot, hShoot; - double pickupGateXA = 0, pickupGateYA = 0, pickupGateHA = 0; - double pickupGateXB = 0, pickupGateYB = 0, pickupGateHB = 0; - double pickupGateXC = 0, pickupGateYC = 0, pickupGateHC = 0; + double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; public static double flywheel0Time = 1.5; boolean gatePickup = true; boolean stack3 = true; - boolean stack2 = true; - double xStackPickupFarA, yStackPickupFarA, hStackPickupFarA; - double xStackPickupFarB, yStackPickupFarB, hStackPickupFarB; - double xStackPickupMiddleA, yStackPickupMiddleA, hStackPickupMiddleA; - double xStackPickupMiddleB, yStackPickupMiddleB, hStackPickupMiddleB; - public static int pickupStackSpeed = 17; - public static int pickupGateSpeed = 25; + double xStackPickupA, yStackPickupA, hStackPickupA; + double xStackPickupB, yStackPickupB, hStackPickupB; + public static int pickupStackSpeed = 12; + public static int pickupGateSpeed = 30; int prevMotif = 0; public static double spindexerSpeedIncrease = 0.014; public static double shootAllTime = 2; @@ -82,8 +79,8 @@ public class Auto_LT_Far extends LinearOpMode { public static double shootStackTime = 2; public static double shootGateTime = 2.5; public static double colorSenseTime = 1; - public static double intakeStackTime = 4.5; - public static double intakeGateTime = 8; + public static double intakeStackTime = 2.5; + public static double intakeGateTime = 2; double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; @@ -96,7 +93,6 @@ public class Auto_LT_Far extends LinearOpMode { TrajectoryActionBuilder shoot3 = null; TrajectoryActionBuilder pickupGate = null; TrajectoryActionBuilder shootGate = null; - TrajectoryActionBuilder pickup2 = null; Pose2d autoStart = new Pose2d(0,0,0); @Override @@ -127,46 +123,16 @@ public class Auto_LT_Far extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.setTurret(turrDefault); - - servos.setSpinPos(spinStartPos); - - servos.setTransferPos(transferServo_out); - - limelightUsed = false; + Spindexer.teleop = false; while (opModeInInit()) { - if (limelightUsed){ - Actions.runBlocking( - autoActions.detectObelisk( - 0.1, - 0.501, - 0.501, - 0.501, - 0.501, - turrDefault - ) - ); - motif = turret.getObeliskID(); - if (motif == 21){ - AutoActions.firstSpindexShootPos = spindexer_outtakeBall1; - } else if (motif == 22){ - AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b; - } else { - AutoActions.firstSpindexShootPos = spindexer_outtakeBall2; - } - } - - if (gamepad2.triangleWasPressed()){ + if (gamepad2.leftBumperWasPressed()){ gatePickup = !gatePickup; } if (gamepad2.rightBumperWasPressed()){ stack3 = !stack3; } - if (gamepad2.leftBumperWasPressed()){ - stack2 = !stack2; - } turret.setTurret(turretShootPos); @@ -199,33 +165,17 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = rShootY; hShoot = rShootH; - xStackPickupFarA = rStackPickupAX; - yStackPickupFarA = rStackPickupAY; - hStackPickupFarA = rStackPickupAH; + xStackPickupA = rStackPickupFarAX; + yStackPickupA = rStackPickupFarAY; + hStackPickupA = rStackPickupFarAH; - xStackPickupFarB = rStackPickupBX; - yStackPickupFarB = rStackPickupBY; - hStackPickupFarB = rStackPickupBH; + xStackPickupB = rStackPickupFarBX; + yStackPickupB = rStackPickupFarBY; + hStackPickupB = rStackPickupFarBH; - xStackPickupMiddleA = rStackPickupAX; - yStackPickupMiddleA = rStackPickupAY; - hStackPickupMiddleA = rStackPickupAH; - - xStackPickupMiddleB = rStackPickupBX; - yStackPickupMiddleB = rStackPickupBY; - hStackPickupMiddleB = rStackPickupBH; - - pickupGateXA = rPickupGateXA; - pickupGateYA = rPickupGateYA; - pickupGateHA = rPickupGateHA; - - pickupGateXB = rPickupGateXB; - pickupGateYB = rPickupGateYB; - pickupGateHB = rPickupGateHB; - - pickupGateXC = rPickupGateXC; - pickupGateYC = rPickupGateYC; - pickupGateHC = rPickupGateHC; + pickupGateX = rPickupGateXA; + pickupGateY = rPickupGateYA; + pickupGateH = rPickupGateHA; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -233,10 +183,15 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + redTurretShootPos; if (gamepad2.squareWasPressed()){ - turret.pipelineSwitch(1); + turret.pipelineSwitch(4); robot.limelight.start(); - limelightUsed = true; gamepad2.rumble(500); + sleep(1000); + turret.setTurret(turrDefault); + + servos.setSpinPos(spinStartPos); + + servos.setTransferPos(transferServo_out); } } else { robot.light.setPosition(0.6); @@ -253,33 +208,17 @@ public class Auto_LT_Far extends LinearOpMode { yShoot = bShootY; hShoot = bShootH; - xStackPickupFarA = bStackPickupAX; - yStackPickupFarA = bStackPickupAY; - hStackPickupFarA = bStackPickupAH; + xStackPickupA = bStackPickupFarAX; + yStackPickupA = bStackPickupFarAY; + hStackPickupA = bStackPickupFarAH; - xStackPickupFarB = bStackPickupBX; - yStackPickupFarB = bStackPickupBY; - hStackPickupFarB = bStackPickupBH; + xStackPickupB = bStackPickupFarBX; + yStackPickupB = bStackPickupFarBY; + hStackPickupB = bStackPickupFarBH; - xStackPickupMiddleA = bStackPickupAX; - yStackPickupMiddleA = bStackPickupAY; - hStackPickupMiddleA = bStackPickupAH; - - xStackPickupMiddleB = bStackPickupBX; - yStackPickupMiddleB = bStackPickupBY; - hStackPickupMiddleB = bStackPickupBH; - - pickupGateXA = bPickupGateXA; - pickupGateYA = bPickupGateYA; - pickupGateHA = bPickupGateHA; - - pickupGateXB = bPickupGateXB; - pickupGateYB = bPickupGateYB; - pickupGateHB = bPickupGateHB; - - pickupGateXC = bPickupGateXC; - pickupGateYC = bPickupGateYC; - pickupGateHC = bPickupGateHC; + pickupGateX = bPickupGateXA; + pickupGateY = bPickupGateYA; + pickupGateH = bPickupGateHA; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; @@ -287,10 +226,15 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + blueTurretShootPos; if (gamepad2.squareWasPressed()){ - turret.pipelineSwitch(5); + turret.pipelineSwitch(2); robot.limelight.start(); - limelightUsed = true; gamepad2.rumble(500); + sleep(1000); + turret.setTurret(turrDefault); + + servos.setSpinPos(spinStartPos); + + servos.setTransferPos(transferServo_out); } } @@ -301,33 +245,26 @@ public class Auto_LT_Far extends LinearOpMode { .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(xStackPickupFarA, yStackPickupFarA), Math.toRadians(hStackPickupFarA)) - .strafeToLinearHeading(new Vector2d(xStackPickupFarB, yStackPickupFarB), Math.toRadians(hStackPickupFarB), + .strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA)) + .strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB), new TranslationalVelConstraint(pickupStackSpeed)); - pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(xStackPickupMiddleA, yStackPickupMiddleA), Math.toRadians(hStackPickupMiddleA)) - .strafeToLinearHeading(new Vector2d(xStackPickupMiddleB, yStackPickupMiddleB), Math.toRadians(hStackPickupMiddleB), - new TranslationalVelConstraint(pickupStackSpeed)); - - shoot3 = drive.actionBuilder(new Pose2d(xStackPickupFarB, yStackPickupFarB, Math.toRadians(hStackPickupFarB))) + shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) - .strafeToLinearHeading(new Vector2d(pickupGateXA, pickupGateYA), Math.toRadians(pickupGateHA)) - .waitSeconds(0.2) - .strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB)) - .strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC), + .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH), new TranslationalVelConstraint(pickupGateSpeed)); - shootGate = drive.actionBuilder(new Pose2d(pickupGateXC, pickupGateYC, Math.toRadians(pickupGateHC))) + shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + limelightUsed = true; + TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Gate Cycle?", gatePickup); - TELE.addData("Pickup Stack Far?", stack3); - TELE.addData("Pickup Stack Middle?", stack2); + TELE.addData("Pickup Stack?", stack3); TELE.addData("Start Position", autoStart); TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift TELE.update(); @@ -351,11 +288,6 @@ public class Auto_LT_Far extends LinearOpMode { shoot(); } - if (stack2){ - cycleStackMiddle(); - shoot(); - } - while (gatePickup && getRuntime() - stamp < endAutoTime){ cycleGatePickupBalls(); if (getRuntime() - stamp > endAutoTime){ @@ -432,79 +364,16 @@ public class Auto_LT_Far extends LinearOpMode { pickup3.build(), autoActions.intake( intakeStackTime, - xStackPickupFarB, - yStackPickupFarB, - hStackPickupFarB - ), - autoActions.detectObelisk( - intakeStackTime, - xStackPickupFarB, - yStackPickupFarB, - posXTolerance, - posYTolerance, - obeliskTurrPos3 + xStackPickupB, + yStackPickupB, + hStackPickupB ) - ) ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = 22; - prevMotif = motif; - + servos.setSpinPos(spinStartPos); Actions.runBlocking( new ParallelAction( - shoot3.build(), - autoActions.prepareShootAll( - colorSenseTime, - shootStackTime, - motif, - xShoot, - yShoot, - hShoot) - ) - ); - } - - - void cycleStackMiddle(){ - Actions.runBlocking( - new ParallelAction( - pickup2.build(), - autoActions.intake( - intakeStackTime, - xStackPickupMiddleB, - yStackPickupMiddleB, - hStackPickupMiddleB - ), - autoActions.detectObelisk( - intakeStackTime, - xStackPickupMiddleB, - yStackPickupMiddleB, - posXTolerance, - posYTolerance, - obeliskTurrPos2 - ) - - ) - ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; - - Actions.runBlocking( - new ParallelAction( - shoot3.build(), - autoActions.prepareShootAll( - colorSenseTime, - shootStackTime, - motif, - xShoot, - yShoot, - hShoot) + shoot3.build() ) ); } @@ -515,25 +384,12 @@ public class Auto_LT_Far extends LinearOpMode { pickupGate.build(), autoActions.intake( intakeStackTime, - pickupGateXC, - pickupGateYC, - pickupGateHC - ), - autoActions.detectObelisk( - intakeGateTime, - pickupGateXC, - pickupGateYC, - posXTolerance, - posYTolerance, - obeliskTurrPos3 + pickupGateX, + pickupGateY, + pickupGateH ) ) ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; } void cycleGatePrepareShoot(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 3f07acd..e4db014 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -55,7 +55,7 @@ public class AutoActions { private boolean shootForward = true; public int motif = 0; double spinEndPos = ServoPositions.spinEndPos; - + private boolean intaking = false; public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig) { this.robot = rob; this.drive = dri; @@ -427,8 +427,10 @@ public class AutoActions { if ((System.currentTimeMillis() - stamp) > (time * 1000)) { servos.setSpinPos(spindexer_intakePos1); + intaking = false; return false; } else { + intaking = true; return true; } } @@ -672,6 +674,44 @@ public class AutoActions { } }; } + + public Action ShakeDrivetrain( + double time + ){ + return new Action() { + int ticker = 0; + double stamp = 0; + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0){ + stamp = System.currentTimeMillis(); + } + ticker++; + + double currentStamp = System.currentTimeMillis(); + if (currentStamp - stamp < time*1000 && (intaking || ticker < 50)) { + if (ticker % 10000 < 5000) { + robot.frontLeft.setPower(0.5); + robot.backLeft.setPower(0.5); + robot.frontRight.setPower(0.5); + robot.backRight.setPower(0.5); + } else { + robot.frontLeft.setPower(-0.5); + robot.backLeft.setPower(-0.5); + robot.frontRight.setPower(-0.5); + robot.backRight.setPower(-0.5); + } + return true; + } else { + robot.frontLeft.setPower(0); + robot.backLeft.setPower(0); + robot.frontRight.setPower(0); + robot.backRight.setPower(0); + return false; + } + } + }; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 1b1af7e..05190f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -7,23 +7,29 @@ public class Back_Poses { public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1; public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50; - public static double rShootX = 100, rShootY = 55, rShootH = 90; - public static double bShootX = 100, bShootY = -55, bShootH = -90; + public static double rShootX = 100, rShootY = 60, rShootH = 145.2; + public static double bShootX = 100, bShootY = -55, bShootH = -145.2; - public static double rStackPickupAX = 73, rStackPickupAY = 51, rStackPickupAH = 140; - public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; + public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145; + public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145; - public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1; - public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1; + public static double rStackPickupFarBX = 53, rStackPickupFarBY = 71, rStackPickupFarBH = 145.1; + public static double bStackPickupFarBX = 55, bStackPickupFarBY = -71, bStackPickupFarBH = -145.1; - public static double rPickupGateXA = 50, rPickupGateYA = 83, rPickupGateHA = 140; - public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -140; - public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 140; - public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -140; + public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145; + public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145; + + public static double rStackPickupMiddleBX = 45, rStackPickupMiddleBY = 49, rStackPickupMiddleBH = 145.1; + public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1; + + public static double rPickupGateXA = 60, rPickupGateYA = 83, rPickupGateHA = 145; + public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -145; + public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145; + public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145; public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190; public static double bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190; - public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; - public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50; + public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 54; + public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -54; } 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 775b583..3a05cdd 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 @@ -19,7 +19,7 @@ public class ServoPositions { public static double spinStartPos = 0.10; public static double spinEndPos = 0.95; - public static double shootAllSpindexerSpeedIncrease = 0.014; + public static double shootAllSpindexerSpeedIncrease = 0.013; 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 46b7949..40b3784 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 @@ -39,7 +39,7 @@ public class TeleopV3 extends LinearOpMode { public static double spinPow = 0.09; public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; public static double spinSpeedIncrease = 0.03; - public static int resetSpinTicks = 4; + public static int resetSpinTicks = 0; public static double hoodSpeedOffset = 0.01; public static double turretSpeedOffset = 0.01; public double vel = 3000; @@ -76,7 +76,7 @@ public class TeleopV3 extends LinearOpMode { int intakeTicker = 0; private boolean shootAll = false; - boolean relocalize = false; + public static boolean relocalize = false; @Override public void runOpMode() throws InterruptedException { @@ -112,6 +112,7 @@ public class TeleopV3 extends LinearOpMode { light.setState(StateEnums.LightState.MANUAL); limelightUsed = true; + Spindexer.teleop = true; while (opModeInInit()) { robot.limelight.start(); @@ -154,13 +155,13 @@ public class TeleopV3 extends LinearOpMode { shootAll = false; servo.setTransferPos(transferServo_out); - light.setState(StateEnums.LightState.BALL_COUNT); + //light.setState(StateEnums.LightState.BALL_COUNT); } else if (gamepad2.triangle){ - light.setState(StateEnums.LightState.BALL_COLOR); + //light.setState(StateEnums.LightState.BALL_COLOR); } else { - light.setState(StateEnums.LightState.GOAL_LOCK); + //light.setState(StateEnums.LightState.GOAL_LOCK); } //TURRET TRACKING @@ -354,14 +355,14 @@ public class TeleopV3 extends LinearOpMode { // // TELE.addData("shootall commanded", shootAll); // Targeting Debug - TELE.addData("robotX", robotX); - TELE.addData("robotY", robotY); - TELE.addData("robot H", robotHeading); +// TELE.addData("robotX", robotX); +// TELE.addData("robotY", robotY); +// TELE.addData("robot H", robotHeading); // 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 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); @@ -369,13 +370,13 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); - TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); - TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); - TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); +// TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); +// TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); +// TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); TELE.update(); - light.update(); + //light.update(); ticker++; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java index 0e30dbb..de6189d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java @@ -65,46 +65,51 @@ public class SortingTest extends LinearOpMode { waitForStart(); if (isStopRequested()) return; - while (opModeIsActive()){ - spindexer.setIntakePower(1); - robot.transfer.setPower(1); - - if (gamepad1.crossWasPressed()){ - motif = 21; - } else if (gamepad1.squareWasPressed()){ - motif = 22; - } else if (gamepad1.triangleWasPressed()){ - motif = 23; - } - flywheel.manageFlywheel(2500); - - if (gamepad1.leftBumperWasPressed()){ - intaking = false; - Actions.runBlocking( - autoActions.prepareShootAll( - 3, - 5, - motif, - 0.501, - 0.501, - 0.501 - ) - ); - } else if (gamepad1.rightBumperWasPressed()){ - intaking = false; - Actions.runBlocking( - autoActions.shootAllAuto( - 3.5, - 0.014, - 0.501, - 0.501, - 0.501 - ) - ); - intaking = true; - } else if (intaking){ - spindexer.processIntake(); - } + if (opModeIsActive()){ + Actions.runBlocking( + autoActions.ShakeDrivetrain( + 100 + ) + ); +// spindexer.setIntakePower(1); +// robot.transfer.setPower(1); +// +// if (gamepad1.crossWasPressed()){ +// motif = 21; +// } else if (gamepad1.squareWasPressed()){ +// motif = 22; +// } else if (gamepad1.triangleWasPressed()){ +// motif = 23; +// } +// flywheel.manageFlywheel(2500); +// +// if (gamepad1.leftBumperWasPressed()){ +// intaking = false; +// Actions.runBlocking( +// autoActions.prepareShootAll( +// 3, +// 5, +// motif, +// 0.501, +// 0.501, +// 0.501 +// ) +// ); +// } else if (gamepad1.rightBumperWasPressed()){ +// intaking = false; +// Actions.runBlocking( +// autoActions.shootAllAuto( +// 3.5, +// 0.014, +// 0.501, +// 0.501, +// 0.501 +// ) +// ); +// intaking = true; +// } else if (intaking){ +// spindexer.processIntake(); +// } } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java index accdc8b..f843d89 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java @@ -31,18 +31,40 @@ public class Drivetrain { } + private double prevY = 0; + private double prevX = 0; + private double prevRX = 0; + private double prevBrake = 0; public void drive(double y, double x, double rx, double brake){ + int countConstant = 0; + boolean brakeChange = false; + if (Math.abs(prevY - y) > 0.05){ + prevY = y; + countConstant++; + } + if (Math.abs(prevX - x) > 0.05){ + prevX = x; + countConstant++; + } + if (Math.abs(prevRX - rx) > 0.05){ + prevRX = rx; + countConstant++; + } + if (Math.abs(prevBrake - brake) > 0.05){ + prevBrake = brake; + brakeChange = true; + } - if (!autoDrive) { + if (!autoDrive && countConstant > 0) { x = x* 1.1; // Counteract imperfect strafing - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator; - double backLeftPower = (y - x + rx) / denominator; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; + double denominator = Math.max(Math.abs(prevY) + Math.abs(prevX) + Math.abs(prevRX), 1); + double frontLeftPower = (prevY + prevX + prevRX) / denominator; + double backLeftPower = (prevY - prevX + prevRX) / denominator; + double frontRightPower = (prevY - prevX - prevRX) / denominator; + double backRightPower = (prevY + prevX - prevRX) / denominator; robot.frontLeft.setPower(frontLeftPower); robot.backLeft.setPower(backLeftPower); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java index 6a5f09d..6fdd2cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Light.java @@ -64,17 +64,17 @@ public final class Light { break; case BALL_COLOR: - - if ((System.currentTimeMillis() % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) { + double currentTime = System.currentTimeMillis(); + if ((currentTime % ballColorCycleTime) < ((ballColorCycleTime / 3) - restingTime)) { lightColor = spindexer.getRearCenterLight(); - } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime / 3)) { + } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime / 3)) { lightColor = 0; - } else if ((System.currentTimeMillis() % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) { + } else if ((currentTime % ballColorCycleTime) < ((2 * ballColorCycleTime / 3) - restingTime)) { lightColor = spindexer.getDriverLight(); - } else if ((System.currentTimeMillis() % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) { + } else if ((currentTime % ballColorCycleTime) < (2 * ballColorCycleTime / 3)) { lightColor = 0; - } else if ((System.currentTimeMillis() % ballColorCycleTime) < (ballColorCycleTime - restingTime)) { + } else if ((currentTime % ballColorCycleTime) < (ballColorCycleTime - restingTime)) { lightColor = spindexer.getPassengerLight(); } 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 43c8a7d..f7d707d 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 @@ -173,10 +173,23 @@ public class Spindexer { // Detects if a ball is found and what color. // Returns true is there was a new ball found in Position 1 // FIXIT: Reduce number of times that we read the color sensors for loop times. + public static boolean teleop = false; public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) { boolean newPos1Detection = false; int spindexerBallPos = 0; + double rearDistance; + double frontDriverDistance; + double frontPassengerDistance; + if (teleop){ + rearDistance = 48; + frontDriverDistance = 50; + frontPassengerDistance = 29; + } else { + rearDistance = 48; + frontDriverDistance = 56; + frontPassengerDistance = 29; + } // Read Distances double dRearCenter = robot.color1.getDistance(DistanceUnit.MM); @@ -187,7 +200,7 @@ public class Spindexer { distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger); // Position 1 - if (distanceRearCenter < 48) { + if (distanceRearCenter < rearDistance) { // Mark Ball Found newPos1Detection = true; @@ -209,7 +222,7 @@ public class Spindexer { // Position 2 // Find which ball position this is in the spindexer spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]; - if (distanceFrontDriver < 50) { + if (distanceFrontDriver < frontDriverDistance) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { @@ -235,7 +248,7 @@ public class Spindexer { // Position 3 spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]; - if (distanceFrontPassenger < 29) { + if (distanceFrontPassenger < frontPassengerDistance) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; @@ -512,6 +525,7 @@ public class Spindexer { break; case SHOOTWAIT: + double shootWaitMax = 4; // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { if (commandedIntakePosition==2) { @@ -535,33 +549,27 @@ public class Spindexer { break; case SHOOT_PREP_CONTINOUS: - if (shootTicks > waitFirstBallTicks){ + if (servos.spinEqual(spinStartPos)){ currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; - shootTicks++; - } else if (servos.spinEqual(spinStartPos)){ - shootTicks++; - servos.setTransferPos(transferServo_in); } else { servos.setSpinPos(spinStartPos); } break; case SHOOT_CONTINOUS: - whileShooting = true; + servos.setTransferPos(transferServo_in); ballPositions[0].isEmpty = false; ballPositions[1].isEmpty = false; ballPositions[2].isEmpty = false; if (servos.getSpinPos() > spinEndPos){ - whileShooting = false; - servos.setTransferPos(transferServo_out); - shootTicks = 0; currentIntakeState = IntakeState.FINDNEXT; + servos.setTransferPos(transferServo_out); } else { - double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; + double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; if (spinPos > spinEndPos + 0.03){ spinPos = spinEndPos + 0.03; } - servos.setSpinPos(spinPos); + moveSpindexerToPos(spinPos); } break; 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 55df38d..471cc1d 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 @@ -13,6 +13,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.teleop.TeleopV3; import java.util.ArrayList; import java.util.List; @@ -79,7 +80,7 @@ public class Turret { } public double getTurrPos() { - return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault; + return robot.turr1.getPosition(); } private double prevTurrPos = 0; @@ -114,23 +115,18 @@ public class Turret { if (result.isValid()) { tx = result.getTx(); ty = result.getTy(); - // MegaTag1 code for receiving position - Pose3D botpose = result.getBotpose(); - if (botpose != null) { - limelightPosX = botpose.getPosition().x; - limelightPosY = botpose.getPosition().y; - } - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - limelightTagPose = fiducial.getRobotPoseTargetSpace(); - if (limelightTagPose != null){ - xPos = limelightTagPose.getPosition().x; - yPos = limelightTagPose.getPosition().y; - zPos = limelightTagPose.getPosition().z; - hPos = limelightTagPose.getOrientation().getYaw(); + if (TeleopV3.relocalize){ + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + limelightTagPose = fiducial.getRobotPoseTargetSpace(); + if (limelightTagPose != null){ + xPos = limelightTagPose.getPosition().x; + yPos = limelightTagPose.getPosition().y; + zPos = limelightTagPose.getPosition().z; + hPos = limelightTagPose.getOrientation().getYaw(); + } } } - } } if (xPos != null){ From e8d28b9e5ff55a83a7d03cfd4602f25e1e0a020a Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 28 Feb 2026 00:10:33 -0600 Subject: [PATCH 03/24] revert --- .../ftc/teamcode/autonomous/Auto_LT_Close.java | 2 +- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 2 +- .../ftc/teamcode/constants/Back_Poses.java | 4 ++-- .../ftc/teamcode/constants/Front_Poses.java | 2 +- .../ftc/teamcode/teleop/TeleopV3.java | 4 ++-- .../ftc/teamcode/utils/Spindexer.java | 15 ++++++++------- 6 files changed, 15 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 1ac064d..fb9ca7f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -180,7 +180,7 @@ public class Auto_LT_Close extends LinearOpMode { servos.setTransferPos(transferServo_out); limelightUsed = false; - Spindexer.teleop = false; +// Spindexer.teleop = false; robot.light.setPosition(1); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 5d7a25d..1ee944f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -123,7 +123,7 @@ public class Auto_LT_Far extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - Spindexer.teleop = false; +// Spindexer.teleop = false; while (opModeInInit()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 05190f4..654d02d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -8,7 +8,7 @@ public class Back_Poses { public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50; public static double rShootX = 100, rShootY = 60, rShootH = 145.2; - public static double bShootX = 100, bShootY = -55, bShootH = -145.2; + public static double bShootX = 100, bShootY = -60, bShootH = -145.2; public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145; public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145; @@ -23,7 +23,7 @@ public class Back_Poses { public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1; public static double rPickupGateXA = 60, rPickupGateYA = 83, rPickupGateHA = 145; - public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -145; + public static double bPickupGateXA = 60, bPickupGateYA = -83, bPickupGateHA = -145; public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145; public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145; public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index 9a8274f..10d140f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -14,7 +14,7 @@ public class Front_Poses { public static double bx2a = 41, by2a = -18, bh2a = -140; public static double rx2b = 21, ry2b = 34, rh2b = 140.1; - public static double bx2b = 23, by2b = -36, bh2b = -140.1; + public static double bx2b = 23, by2b = -34, bh2b = -140.1; public static double rx3a = 55, ry3a = 39, rh3a = 140; public static double bx3a = 55, by3a = -39, bh3a = -140; 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 40b3784..6dc4a9b 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 @@ -155,13 +155,13 @@ public class TeleopV3 extends LinearOpMode { shootAll = false; servo.setTransferPos(transferServo_out); - //light.setState(StateEnums.LightState.BALL_COUNT); + light.setState(StateEnums.LightState.BALL_COUNT); } else if (gamepad2.triangle){ //light.setState(StateEnums.LightState.BALL_COLOR); } else { - //light.setState(StateEnums.LightState.GOAL_LOCK); + light.setState(StateEnums.LightState.BALL_COUNT); } //TURRET TRACKING 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 f7d707d..7b11bb8 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 @@ -525,7 +525,6 @@ public class Spindexer { break; case SHOOTWAIT: - double shootWaitMax = 4; // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { if (commandedIntakePosition==2) { @@ -550,26 +549,30 @@ public class Spindexer { case SHOOT_PREP_CONTINOUS: if (servos.spinEqual(spinStartPos)){ + servos.setTransferPos(transferServo_in); currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; } else { + servos.setTransferPos(transferServo_out); servos.setSpinPos(spinStartPos); } break; case SHOOT_CONTINOUS: - servos.setTransferPos(transferServo_in); + whileShooting = true; ballPositions[0].isEmpty = false; ballPositions[1].isEmpty = false; ballPositions[2].isEmpty = false; if (servos.getSpinPos() > spinEndPos){ - currentIntakeState = IntakeState.FINDNEXT; + whileShooting = false; servos.setTransferPos(transferServo_out); + shootTicks = 0; + currentIntakeState = IntakeState.FINDNEXT; } else { double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; if (spinPos > spinEndPos + 0.03){ spinPos = spinEndPos + 0.03; } - moveSpindexerToPos(spinPos); + servos.setSpinPos(spinPos); } break; @@ -679,10 +682,8 @@ public class Spindexer { return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor; } private double prevPow = 0.501; - private boolean firstIntakePow = true; public void setIntakePower(double pow){ - if (firstIntakePow || prevPow != pow){ - firstIntakePow = false; + if (prevPow != 0.501 && prevPow != pow){ robot.intake.setPower(pow); } prevPow = pow; From a1340c53884ec3381e134d83f49343da94a8579c Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 28 Feb 2026 01:33:26 -0600 Subject: [PATCH 04/24] night before regionals --- .../teamcode/autonomous/Auto_LT_Close.java | 3 +- .../ftc/teamcode/constants/Front_Poses.java | 8 ++-- .../teamcode/constants/ServoPositions.java | 4 +- .../ftc/teamcode/teleop/TeleopV3.java | 6 +-- .../ftc/teamcode/utils/Targeting.java | 42 ++++++++++++------- 5 files changed, 38 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index fb9ca7f..cc5af40 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos0; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3; @@ -371,7 +372,7 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = bPickupGateBY; pickupGateBH = bPickupGateBH; - obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0; + obeliskTurrPosAutoStart = turrDefault + blueObeliskTurrPos0; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index 10d140f..ccfb8d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -7,8 +7,8 @@ import com.acmerobotics.roadrunner.Pose2d; public class Front_Poses { - public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; - public static double bx1 = 20, by1 = -0.5, bh1 = -0.1; + public static double rx1 = 30, ry1 = 5, rh1 = 0.1; + public static double bx1 = 30, by1 = -5, bh1 = -0.1; public static double rx2a = 41, ry2a = 18, rh2a = 140; public static double bx2a = 41, by2a = -18, bh2a = -140; @@ -33,8 +33,8 @@ public class Front_Poses { public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; - public static double rShootX = 40, rShootY = 10, rShootH = 50; - public static double bShootX = 40, bShootY = -10, bShootH = -50; + public static double rShootX = 60, rShootY = 10, rShootH = 50; + public static double bShootX = 60, bShootY = -10, bShootH = -50; public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; public static double bxPrep = 45, byPrep = -10, bhPrep = -50; 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 3a05cdd..465d608 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 @@ -16,10 +16,10 @@ public class ServoPositions { public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6; public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4; - public static double spinStartPos = 0.10; + public static double spinStartPos = 0.05; public static double spinEndPos = 0.95; - public static double shootAllSpindexerSpeedIncrease = 0.013; + public static double shootAllSpindexerSpeedIncrease = 0.008; 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 6dc4a9b..a519e0a 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 @@ -361,8 +361,8 @@ public class TeleopV3 extends LinearOpMode { // 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 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); @@ -376,7 +376,7 @@ public class TeleopV3 extends LinearOpMode { TELE.update(); - //light.update(); + light.update(); ticker++; } 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 b738fac..bec1745 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 @@ -87,35 +87,47 @@ public class Targeting { public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); + int gridX; + int gridY; if (!redAlliance){ sin54 = Math.sin(Math.toRadians(54)); + double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; + double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; + + // Convert robot coordinates to inches + robotInchesX = rotatedX * unitConversionFactor + 20; + robotInchesY = rotatedY * unitConversionFactor + 20; + + // Find approximate location in the grid + gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize)); + gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); } else { sin54 = Math.sin(Math.toRadians(-54)); + double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; + double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; + + // Convert robot coordinates to inches + robotInchesX = rotatedX * unitConversionFactor; + robotInchesY = rotatedY * unitConversionFactor; + + // Find approximate location in the grid + gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); + gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); } - // TODO: test these values determined from the fmap - double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; - double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; - // Convert robot coordinates to inches - robotInchesX = rotatedX * unitConversionFactor; - robotInchesY = rotatedY * unitConversionFactor; - - // Find approximate location in the grid - int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); - int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); int remX = Math.floorMod((int) robotInchesX, tileSize); int remY = Math.floorMod((int) robotInchesY, tileSize); //clamp - //if (redAlliance) { + if (redAlliance) { robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); - //} else { -// robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); -// robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); - //} + } else { + robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); + robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); + } // Determine if we need to interpolate based on tile position. // if near upper or lower quarter or tile interpolate with next tile. From 76dc6b12bf409596841a0b13b48fe9d95b6ce6cb Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Sat, 28 Feb 2026 02:49:50 -0600 Subject: [PATCH 05/24] Open up SpinEqual position test. Eliminate pause after moving spindexer before going back to intake mode. --- .../firstinspires/ftc/teamcode/utils/Servos.java | 2 +- .../ftc/teamcode/utils/Spindexer.java | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 8f6d885..232ed35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -79,6 +79,6 @@ public class Servos { } public boolean spinEqual(double pos) { - return Math.abs(pos - this.getSpinPos()) < 0.03; + return Math.abs(pos - this.getSpinPos()) < 0.05; } } 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 7b11bb8..75bb5f1 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 @@ -48,7 +48,7 @@ public class Spindexer { public double distanceFrontDriver = 0.0; public double distanceFrontPassenger = 0.0; - public double spindexerWiggle = 0.01; + public double spindexerWiggle = 0.03; public double spindexerOuttakeWiggle = 0.01; private double prevPos = 0.0; @@ -205,7 +205,7 @@ public class Spindexer { // Mark Ball Found newPos1Detection = true; - if (detectRearColor) { + if (detectRearColor && !teleop) { // Detect which color NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors(); @@ -225,7 +225,7 @@ public class Spindexer { if (distanceFrontDriver < frontDriverDistance) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; - if (detectFrontColor) { + if (detectFrontColor && !teleop) { NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors(); double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue); @@ -252,7 +252,7 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; - if (detectFrontColor) { + if (detectFrontColor && !teleop) { NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors(); double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue); @@ -446,13 +446,13 @@ public class Spindexer { case MOVING: // Stopping when we get to the new position if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - if (intakeTicker > 1){ + //if (intakeTicker > 1){ currentIntakeState = Spindexer.IntakeState.INTAKE; stopSpindexer(); intakeTicker = 0; - } else { - intakeTicker++; - } + //} else { + // intakeTicker++; + //} //detectBalls(false, false); } else { // Keep moving the spindexer From 6743481440d8cddf323b8018305519effeb9acfb Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 28 Feb 2026 11:31:32 -0600 Subject: [PATCH 06/24] added control to spindexer speed --- .../ftc/teamcode/constants/ServoPositions.java | 2 +- .../ftc/teamcode/teleop/TeleopV3.java | 18 ++++++++++++++---- 2 files changed, 15 insertions(+), 5 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 465d608..b1e9eb1 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 @@ -19,7 +19,7 @@ public class ServoPositions { public static double spinStartPos = 0.05; public static double spinEndPos = 0.95; - public static double shootAllSpindexerSpeedIncrease = 0.008; + public static double shootAllSpindexerSpeedIncrease = 0.009; 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 a519e0a..a92bc8b 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 @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; @@ -16,6 +17,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.constants.StateEnums; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Drivetrain; @@ -157,10 +159,11 @@ public class TeleopV3 extends LinearOpMode { light.setState(StateEnums.LightState.BALL_COUNT); - } else if (gamepad2.triangle){ + //} else if (gamepad2.triangle){ //light.setState(StateEnums.LightState.BALL_COLOR); - } else { + //} + } else { light.setState(StateEnums.LightState.BALL_COUNT); } @@ -188,7 +191,7 @@ public class TeleopV3 extends LinearOpMode { //RELOCALIZATION - if (gamepad2.squareWasPressed()){ + if (gamepad2.triangleWasPressed()){ relocalize = !relocalize; gamepad2.rumble(500); } @@ -197,7 +200,7 @@ public class TeleopV3 extends LinearOpMode { turret.relocalize(); xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; yOffset = (turret.getLimelightX() * 39.3701) - robY; - hOffset = (Math.toRadians(turret.getLimelightH())) - robH; + //hOffset = (Math.toRadians(turret.getLimelightH())) - robH; } else { turret.trackGoal(deltaPose); } @@ -265,8 +268,15 @@ public class TeleopV3 extends LinearOpMode { if (gamepad2.crossWasPressed()) { drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + gamepad2.rumble(200); + sleep(500); } + if (gamepad2.squareWasPressed()){ + shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease-0.01; + } else if (gamepad2.circleWasPressed()){ + shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease+0.01; + } if (enableSpindexerManager) { From b342c9814937928eb2ca982089a2b7dd3c57d7e6 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 28 Feb 2026 12:02:38 -0600 Subject: [PATCH 07/24] gate auto coded --- .../teamcode/autonomous/Auto_LT_Close.java | 76 +++++++++---------- .../ftc/teamcode/constants/Front_Poses.java | 6 ++ 2 files changed, 44 insertions(+), 38 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index cc5af40..4aef555 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -90,10 +90,6 @@ public class Auto_LT_Close extends LinearOpMode { public static double intake1GateTime = 3.3; public static double lastShootTime = 27; - public static double openGateX = 26; - public static double openGateY = 48; - public static double openGateH = Math.toRadians(155); - Robot robot; MultipleTelemetry TELE; MecanumDrive drive; @@ -125,6 +121,8 @@ public class Auto_LT_Close extends LinearOpMode { double xShootGate, yShootGate, hShootGate; double xLeave, yLeave, hLeave; double xLeaveGate, yLeaveGate, hLeaveGate; + double openGateCloseX = 0, openGateCloseY = 0, openGateCloseH = 0; + double openGateMiddleX = 0, openGateMiddleY = 0, openGateMiddleH = 0; int ballCycles = 3; int prevMotif = 0; @@ -310,6 +308,14 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = rPickupGateBY; pickupGateBH = rPickupGateBH; + openGateCloseX = rOpenGateCloseX; + openGateCloseY = rOpenGateCloseY; + openGateCloseH = rOpenGateCloseH; + + openGateMiddleX = rOpenGateMiddleX; + openGateMiddleY = rOpenGateMiddleY; + openGateMiddleH = rOpenGateMiddleH; + obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -372,25 +378,34 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = bPickupGateBY; pickupGateBH = bPickupGateBH; + openGateCloseX = bOpenGateCloseX; + openGateCloseY = bOpenGateCloseY; + openGateCloseH = bOpenGateCloseH; + + openGateMiddleX = bOpenGateMiddleX; + openGateMiddleY = bOpenGateMiddleY; + openGateMiddleH = bOpenGateMiddleH; + obeliskTurrPosAutoStart = turrDefault + blueObeliskTurrPos0; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; } - if (gateCycle) { - shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0)); - } else { +// if (gateCycle) { +// shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) +// .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0)); +// } else { shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); - } +// } if (gateCycle) { pickup2 = shoot0.endTrajectory().fresh() .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), - new TranslationalVelConstraint(pickupStackGateSpeed)); + new TranslationalVelConstraint(pickupStackGateSpeed)) + .strafeToLinearHeading(new Vector2d(openGateMiddleX, openGateMiddleY), Math.toRadians(openGateMiddleH)); } else { pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) @@ -398,13 +413,14 @@ public class Auto_LT_Close extends LinearOpMode { new TranslationalVelConstraint(pickup1Speed)); } - if (gateCycle&& withPartner) { +// if (gateCycle && withPartner) { +// shoot2 = pickup2.endTrajectory().fresh() +// .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH)) +// .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH)); +// } else + if (gateCycle) { shoot2 = pickup2.endTrajectory().fresh() - .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH)) - .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH)); - } else if (gateCycle) { - shoot2 = pickup2.endTrajectory().fresh() - .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); } else if (ballCycles < 3) { shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); @@ -426,10 +442,11 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { - pickup1 = gateCycleShoot.endTrajectory().fresh() + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), - new TranslationalVelConstraint(pickupStackGateSpeed)); + new TranslationalVelConstraint(pickupStackGateSpeed)) + .strafeToLinearHeading(new Vector2d(openGateCloseX, openGateCloseY), Math.toRadians(openGateCloseH)); } else { pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) @@ -440,7 +457,7 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { shoot1 = pickup1.endTrajectory().fresh() - .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate)); + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); } else if (ballCycles < 2) { shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); @@ -488,28 +505,11 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { startAutoGate(); shoot(0.501, 0.501, 0.501); - cycleStackMiddleGate(); + cycleStackClose(); shoot(0.501,0.501, 0.501); - - while (getRuntime() - stamp < endGateTime) { - cycleGateIntake(); - if (getRuntime() - stamp < lastShootTime) { - cycleGateShoot(); - shoot(0.501, 0.501, 0.501); - } - } - cycleStackCloseIntakeGate(); - - if (getRuntime() - stamp < lastShootTime) { - cycleStackCloseShootGate(); - } - + cycleStackMiddle(); shoot(0.501, 0.501, 0.501); - } else { - - - startAuto(); shoot(0.501, 0.501,0.501); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index ccfb8d3..51d4117 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -58,5 +58,11 @@ public class Front_Poses { public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180; public static double pickupGateCX = 34, pickupGateCY = 58, pickupGateCH = 220; + public static double rOpenGateCloseX = 20, rOpenGateCloseY = 35, rOpenGateCloseH = 230; + public static double bOpenGateCloseX = 20, bOpenGateCloseY = -35, bOpenGateCloseH = -230; + + public static double rOpenGateMiddleX = 36, rOpenGateMiddleY = 59, rOpenGateMiddleH = 50; + public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -500; + public static Pose2d teleStart = new Pose2d(0, 0, 0); } From fb9cbb1c71b6adff186166c887064bba66b5ab17 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 28 Feb 2026 12:08:13 -0600 Subject: [PATCH 08/24] some misc fixes --- .../org/firstinspires/ftc/teamcode/constants/Front_Poses.java | 2 +- .../firstinspires/ftc/teamcode/constants/ServoPositions.java | 2 +- .../java/org/firstinspires/ftc/teamcode/utils/Spindexer.java | 2 ++ .../main/java/org/firstinspires/ftc/teamcode/utils/Turret.java | 2 +- 4 files changed, 5 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index 51d4117..d0dbe76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -62,7 +62,7 @@ public class Front_Poses { public static double bOpenGateCloseX = 20, bOpenGateCloseY = -35, bOpenGateCloseH = -230; public static double rOpenGateMiddleX = 36, rOpenGateMiddleY = 59, rOpenGateMiddleH = 50; - public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -500; + public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -50; public static Pose2d teleStart = new Pose2d(0, 0, 0); } 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 b1e9eb1..671d02f 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 @@ -19,7 +19,7 @@ public class ServoPositions { public static double spinStartPos = 0.05; public static double spinEndPos = 0.95; - public static double shootAllSpindexerSpeedIncrease = 0.009; + public static double shootAllSpindexerSpeedIncrease = 0.0095; public static double transferServo_out = 0.15; 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 7b11bb8..01dd93a 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 @@ -185,6 +185,8 @@ public class Spindexer { rearDistance = 48; frontDriverDistance = 50; frontPassengerDistance = 29; + detectFrontColor = false; + detectRearColor = false; } else { rearDistance = 48; frontDriverDistance = 56; 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 471cc1d..b2cdfff 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 @@ -27,7 +27,7 @@ public class Turret { public static double turret180Range = 0.54; public static double turrDefault = 0.35; public static double turrMin = 0; - public static double turrMax = 1; + public static double turrMax = 0.69; public static boolean limelightUsed = true; public static double limelightPosOffset = 5; public static double manualOffset = 0.0; From 37dca729f0b3fd6632f8a95760af5f13d9f2835f Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 28 Feb 2026 12:27:53 -0600 Subject: [PATCH 09/24] spindex fix --- .../teamcode/autonomous/actions/AutoActions.java | 2 +- .../ftc/teamcode/constants/ServoPositions.java | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index e4db014..968545d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -54,7 +54,7 @@ public class AutoActions { public static double firstSpindexShootPos = spinStartPos; private boolean shootForward = true; public int motif = 0; - double spinEndPos = ServoPositions.spinEndPos; + double spinEndPos = 0.95; private boolean intaking = false; public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig) { this.robot = rob; 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 671d02f..51b533a 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,19 +5,19 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.18; //0.13; + public static double spindexer_intakePos1 = 0.26; //0.13; - public static double spindexer_intakePos2 = 0.37; //0.33;//0.5; + public static double spindexer_intakePos2 = 0.45; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.56; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.63; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.84; //0.65; //0.24; - public static double spindexer_outtakeBall3b = 0.27; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.91; //0.65; //0.24; + public static double spindexer_outtakeBall3b = 0.35; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6; - public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4; + public static double spindexer_outtakeBall2 = 0.73; //0.46; //0.6; + public static double spindexer_outtakeBall1 = 0.54; //0.27; //0.4; public static double spinStartPos = 0.05; - public static double spinEndPos = 0.95; + public static double spinEndPos = 0.6; public static double shootAllSpindexerSpeedIncrease = 0.0095; From 128637e8a10c6b64b354baeb90bcb323841cd456 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 28 Feb 2026 12:30:36 -0600 Subject: [PATCH 10/24] for you @Matt --- .../org/firstinspires/ftc/teamcode/constants/Back_Poses.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 654d02d..1f6a8a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -7,8 +7,8 @@ public class Back_Poses { public static double rLeaveX = 90, rLeaveY = 50, rLeaveH = 50.1; public static double bLeaveX = 90, bLeaveY = -50, bLeaveH = -50; - public static double rShootX = 100, rShootY = 60, rShootH = 145.2; - public static double bShootX = 100, bShootY = -60, bShootH = -145.2; + public static double rShootX = 100, rShootY = 60, rShootH = 125.2; + public static double bShootX = 100, bShootY = -60, bShootH = -125.2; public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145; public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145; From 08ba099d5ba0e1ae30e2a619cf5e0ce3138d6ea5 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 7 Apr 2026 19:12:34 -0500 Subject: [PATCH 11/24] area code --- .../teamcode/autonomous/Auto_LT_Close.java | 2 +- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 87 ++++++++----- .../ftc/teamcode/constants/Back_Poses.java | 12 +- .../ftc/teamcode/teleop/TeleopV3.java | 118 +++++++++--------- .../ftc/teamcode/utils/Drivetrain.java | 8 +- .../ftc/teamcode/utils/Turret.java | 5 +- 6 files changed, 129 insertions(+), 103 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 4aef555..8b8b5a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -503,7 +503,7 @@ public class Auto_LT_Close extends LinearOpMode { if (gateCycle) { - startAutoGate(); + startAuto(); shoot(0.501, 0.501, 0.501); cycleStackClose(); shoot(0.501,0.501, 0.501); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 1ee944f..763d938 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -79,8 +79,8 @@ public class Auto_LT_Far extends LinearOpMode { public static double shootStackTime = 2; public static double shootGateTime = 2.5; public static double colorSenseTime = 1; - public static double intakeStackTime = 2.5; - public static double intakeGateTime = 2; + public static double intakeStackTime = 5; + public static double intakeGateTime = 3.75; double obeliskTurrPos1 = 0.0; double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; @@ -122,6 +122,8 @@ public class Auto_LT_Far extends LinearOpMode { robot.limelight.pipelineSwitch(1); turret = new Turret(robot, TELE, robot.limelight); + limelightUsed = false; + // Spindexer.teleop = false; @@ -190,6 +192,7 @@ public class Auto_LT_Far extends LinearOpMode { turret.setTurret(turrDefault); servos.setSpinPos(spinStartPos); + limelightUsed = true; servos.setTransferPos(transferServo_out); } @@ -231,6 +234,7 @@ public class Auto_LT_Far extends LinearOpMode { gamepad2.rumble(500); sleep(1000); turret.setTurret(turrDefault); + limelightUsed = true; servos.setSpinPos(spinStartPos); @@ -259,10 +263,10 @@ public class Auto_LT_Far extends LinearOpMode { shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); - limelightUsed = true; TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); + TELE.addData("Limelight On?",limelightUsed); TELE.addData("Gate Cycle?", gatePickup); TELE.addData("Pickup Stack?", stack3); TELE.addData("Start Position", autoStart); @@ -281,11 +285,17 @@ public class Auto_LT_Far extends LinearOpMode { robot.transfer.setPower(1); startAuto(); - shoot(); + if (redAlliance){ + shoot(autoStartRX, autoStartRY, autoStartRH); + + } else { + shoot(autoStartBX, autoStartBY, autoStartBH); + + } if (stack3){ cycleStackFar(); - shoot(); + shoot(xShoot, yShoot, hShoot); } while (gatePickup && getRuntime() - stamp < endAutoTime){ @@ -294,10 +304,7 @@ public class Auto_LT_Far extends LinearOpMode { break; } cycleGatePrepareShoot(); - if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){ - break; - } - shoot(); + shoot(xShoot, yShoot, hShoot); } if (gatePickup || stack3){ @@ -324,28 +331,46 @@ public class Auto_LT_Far extends LinearOpMode { } - void shoot(){ + void shoot(double x, double y, double z){ Actions.runBlocking( new ParallelAction( - autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, 0.501, 0.501, 0.501) + autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, x, y, z) ) ); } void startAuto(){ - Actions.runBlocking( - new ParallelAction( - autoActions.manageShooterAuto( - flywheel0Time, - 0.501, - 0.501, - 0.501, - true - ) - ) - ); + if (redAlliance){ + Actions.runBlocking( + new ParallelAction( + autoActions.manageShooterAuto( + flywheel0Time, + autoStartRX, + autoStartRY, + autoStartRH, + true + ) + + ) + ); + + } else { + Actions.runBlocking( + new ParallelAction( + autoActions.manageShooterAuto( + flywheel0Time, + autoStartBX, + autoStartBY, + autoStartBH, + true + ) + + ) + ); + } + } void leave3Ball(){ @@ -371,9 +396,13 @@ public class Auto_LT_Far extends LinearOpMode { ) ); servos.setSpinPos(spinStartPos); + spindexer.setIntakePower(-0.1); Actions.runBlocking( new ParallelAction( - shoot3.build() + shoot3.build(), + autoActions.manageShooterAuto( + shootStackTime,xShoot, yShoot, hShoot, false + ) ) ); } @@ -383,7 +412,7 @@ public class Auto_LT_Far extends LinearOpMode { new ParallelAction( pickupGate.build(), autoActions.intake( - intakeStackTime, + intakeGateTime, pickupGateX, pickupGateY, pickupGateH @@ -393,16 +422,16 @@ public class Auto_LT_Far extends LinearOpMode { } void cycleGatePrepareShoot(){ + spindexer.setIntakePower(-0.1); Actions.runBlocking( new ParallelAction( shootGate.build(), - autoActions.prepareShootAll( - colorSenseTime, - shootGateTime, - motif, + autoActions.manageShooterAuto( + shootGateTime, xShoot, yShoot, - hShoot + hShoot, + false ) ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 1f6a8a4..472fd95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -10,11 +10,11 @@ public class Back_Poses { public static double rShootX = 100, rShootY = 60, rShootH = 125.2; public static double bShootX = 100, bShootY = -60, bShootH = -125.2; - public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145; - public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145; + public static double rStackPickupFarAX = 75, rStackPickupFarAY = 45, rStackPickupFarAH = 150; + public static double bStackPickupFarAX = 75, bStackPickupFarAY = -45, bStackPickupFarAH = -150; - public static double rStackPickupFarBX = 53, rStackPickupFarBY = 71, rStackPickupFarBH = 145.1; - public static double bStackPickupFarBX = 55, bStackPickupFarBY = -71, bStackPickupFarBH = -145.1; + public static double rStackPickupFarBX = 45, rStackPickupFarBY = 80, rStackPickupFarBH = 145.1; + public static double bStackPickupFarBX = 45, bStackPickupFarBY = -80, bStackPickupFarBH = -145.1; public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145; public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145; @@ -22,8 +22,8 @@ public class Back_Poses { public static double rStackPickupMiddleBX = 45, rStackPickupMiddleBY = 49, rStackPickupMiddleBH = 145.1; public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1; - public static double rPickupGateXA = 60, rPickupGateYA = 83, rPickupGateHA = 145; - public static double bPickupGateXA = 60, bPickupGateYA = -83, bPickupGateHA = -145; + public static double rPickupGateXA = 60, rPickupGateYA = 90, rPickupGateHA = 140; + public static double bPickupGateXA = 60, bPickupGateYA = -90, bPickupGateHA = -140; public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145; public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145; public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190; 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 a92bc8b..4fd769b 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 @@ -142,6 +142,62 @@ public class TeleopV3 extends LinearOpMode { while (opModeIsActive()) { //TELE.addData("Is limelight on?", robot.limelight.getStatus()); + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (enableSpindexerManager) { + //if (!shootAll) { + spindexer.processIntake(); + //} + + // RIGHT_BUMPER + if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { + spindexer.setIntakePower(1); + } else if (gamepad1.cross) { + spindexer.setIntakePower(-1); + } else { + spindexer.setIntakePower(0); + } + + // LEFT_BUMPER + if (!shootAll && gamepad1.leftBumperWasReleased()) { + shootStamp = getRuntime(); + shootAll = true; + + shooterTicker = 0; + + } + intakeTicker++; + if (shootAll) { + intakeTicker = 0; + intake = false; + reject = false; + + if (shooterTicker == 0) { + spindexer.prepareShootAllContinous(); + //TELE.addLine("preparing to shoot"); +// } else if (shooterTicker == 2) { +// //servo.setTransferPos(transferServo_in); +// spindexer.shootAll(); +// TELE.addLine("starting to shoot"); + } else if (spindexer.shootAllComplete()) { + //spindexPos = spindexer_intakePos1; + shootAll = false; + spindexer.resetSpindexer(); + //spindexer.processIntake(); + //TELE.addLine("stop shooting"); + } + shooterTicker++; + //spindexer.processIntake(); + } + + if (gamepad1.left_stick_button) { + servo.setTransferPos(transferServo_out); + //spindexPos = spindexer_intakePos1; + shootAll = false; + spindexer.resetSpindexer(); + } + } //DRIVETRAIN: @@ -169,9 +225,9 @@ public class TeleopV3 extends LinearOpMode { //TURRET TRACKING - double robX = drive.localizer.getPose().position.x; - double robY = drive.localizer.getPose().position.y; - double robH = drive.localizer.getPose().heading.toDouble(); + double robX = currentPose.position.x; + double robY = currentPose.position.y; + double robH = currentPose.heading.toDouble(); double robotX = robX + xOffset; double robotY = robY + yOffset; @@ -278,63 +334,7 @@ public class TeleopV3 extends LinearOpMode { shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease+0.01; } - - if (enableSpindexerManager) { - //if (!shootAll) { - spindexer.processIntake(); - //} - - // RIGHT_BUMPER - if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { - spindexer.setIntakePower(1); - } else if (gamepad1.cross) { - spindexer.setIntakePower(-1); - } else { - spindexer.setIntakePower(0); - } - - // LEFT_BUMPER - if (!shootAll && gamepad1.leftBumperWasReleased()) { - shootStamp = getRuntime(); - shootAll = true; - - shooterTicker = 0; - - } - intakeTicker++; - if (shootAll) { - intakeTicker = 0; - intake = false; - reject = false; - - if (shooterTicker == 0) { - spindexer.prepareShootAllContinous(); - //TELE.addLine("preparing to shoot"); -// } else if (shooterTicker == 2) { -// //servo.setTransferPos(transferServo_in); -// spindexer.shootAll(); -// TELE.addLine("starting to shoot"); - } else if (spindexer.shootAllComplete()) { - //spindexPos = spindexer_intakePos1; - shootAll = false; - spindexer.resetSpindexer(); - //spindexer.processIntake(); - //TELE.addLine("stop shooting"); - } - shooterTicker++; - //spindexer.processIntake(); - } - - if (gamepad1.left_stick_button) { - servo.setTransferPos(transferServo_out); - //spindexPos = spindexer_intakePos1; - shootAll = false; - spindexer.resetSpindexer(); - } - } - //EXTRA STUFFINESS: - drive.updatePoseEstimate(); for (LynxModule hub : allHubs) { hub.clearBulkCache(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java index f843d89..edd598a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java @@ -71,6 +71,8 @@ public class Drivetrain { robot.frontRight.setPower(frontRightPower); robot.backRight.setPower(backRightPower); } + Pose2d currentPos = drive.localizer.getPose(); + brakePos = currentPos; if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) { robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -78,15 +80,9 @@ public class Drivetrain { robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - drive.updatePoseEstimate(); - - brakePos = drive.localizer.getPose(); autoDrive = true; } else if (brake > 0.4) { - drive.updatePoseEstimate(); - - Pose2d currentPos = drive.localizer.getPose(); TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos) .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT); 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 b2cdfff..96ddb29 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 @@ -242,6 +242,7 @@ public class Turret { return bearingOffset; } + double targetTurretPos; public void trackGoal(Pose2d deltaPos) { /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ @@ -273,7 +274,7 @@ public class Turret { limelightRead(); // Active correction if we see the target - if (result.isValid() && !lockOffset && limelightUsed) { + if (result.isValid() && !lockOffset && limelightUsed && targetTurretPos > turrMin && targetTurretPos < turrMax) { currentTrackOffset += bearingAlign(result); currentTrackCount++; @@ -329,7 +330,7 @@ public class Turret { /* ---------------- ANGLE → SERVO POSITION ---------------- */ - double targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); + targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); // Clamp to physical servo limits targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax)); From e40695b4f60c340bd467b339a6d4a4a21434693f Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 7 Apr 2026 20:55:53 -0500 Subject: [PATCH 12/24] pedropathing added: tuning progress: forward and lateral velocities done --- .../teamcode/libs/pedroPathing/Constants.java | 19 - .../ftc/teamcode/pedroPathing/Constants.java | 50 ++ .../{libs => }/pedroPathing/Tuning.java | 585 ++++++++++++++++-- build.dependencies.gradle | 6 +- 4 files changed, 581 insertions(+), 79 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{libs => }/pedroPathing/Tuning.java (69%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java deleted file mode 100644 index d596ce1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Constants.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.teamcode.libs.pedroPathing; - -import com.pedropathing.follower.Follower; -import com.pedropathing.follower.FollowerConstants; -import com.pedropathing.ftc.FollowerBuilder; -import com.pedropathing.paths.PathConstraints; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants(); - - public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); - - public static Follower createFollower(HardwareMap hardwareMap) { - return new FollowerBuilder(followerConstants, hardwareMap) - .pathConstraints(pathConstraints) - .build(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java new file mode 100644 index 0000000..7144cad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.pedropathing.follower.Follower; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; +import com.pedropathing.paths.PathConstraints; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +public class Constants { + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(15.5); + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(1) + .rightFrontMotorName("fr") + .rightRearMotorName("br") + .leftRearMotorName("bl") + .leftFrontMotorName("fl") + .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE) + .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .xVelocity(64.675) + .yVelocity(49.583); + + public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + + public static PinpointConstants localizerConstants = new PinpointConstants() + .forwardPodY(7.5) + .strafePodX(3.75) + .distanceUnit(DistanceUnit.INCH) + .hardwareMapName("pinpoint") + .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) + .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED) + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD); + + public static Follower createFollower(HardwareMap hardwareMap) { + return new FollowerBuilder(followerConstants, hardwareMap) + .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) + .pinpointLocalizer(localizerConstants) + .build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java similarity index 69% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index da2b0d6..7dbf3e9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -1,12 +1,14 @@ -package org.firstinspires.ftc.teamcode.libs.pedroPathing; +package org.firstinspires.ftc.teamcode.pedroPathing; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.changes; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrent; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.drawCurrentAndHistory; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.follower; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.stopRobot; -import static org.firstinspires.ftc.teamcode.libs.pedroPathing.Tuning.telemetryM; +import static com.pedropathing.math.MathFunctions.quadraticFit; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawCurrent; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawCurrentAndHistory; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM; +import android.annotation.SuppressLint; import com.bylazar.configurables.PanelsConfigurables; import com.bylazar.configurables.annotations.Configurable; @@ -17,13 +19,20 @@ import com.bylazar.field.Style; import com.bylazar.telemetry.PanelsTelemetry; import com.bylazar.telemetry.TelemetryManager; import com.pedropathing.follower.Follower; -import com.pedropathing.geometry.*; -import com.pedropathing.math.*; -import com.pedropathing.paths.*; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.math.Vector; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; import com.pedropathing.telemetry.SelectableOpMode; -import com.pedropathing.util.*; +import com.pedropathing.util.PoseHistory; +import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.util.ElapsedTime; import java.util.ArrayList; import java.util.List; @@ -52,6 +61,7 @@ public class Tuning extends SelectableOpMode { super("Select a Tuning OpMode", s -> { s.folder("Localization", l -> { l.add("Localization Test", LocalizationTest::new); + l.add("Offsets Tuner", OffsetsTuner::new); l.add("Forward Tuner", ForwardTuner::new); l.add("Lateral Tuner", LateralTuner::new); l.add("Turn Tuner", TurnTuner::new); @@ -61,6 +71,7 @@ public class Tuning extends SelectableOpMode { a.add("Lateral Velocity Tuner", LateralVelocityTuner::new); a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new); a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new); + a.add("Predictive Braking Tuner", PredictiveBrakingTuner::new); }); s.folder("Manual", p -> { p.add("Translational Tuner", TranslationalTuner::new); @@ -73,6 +84,11 @@ public class Tuning extends SelectableOpMode { p.add("Triangle", Triangle::new); p.add("Circle", Circle::new); }); + s.folder("Swerve", p-> { + p.add("Analog Min / Max Tuner", AnalogMinMaxTuner::new); + p.add("Swerve Offsets Test", SwerveOffsetsTest::new); + p.add("Swerve Turn Test", SwerveTurnTest::new); + }); }); } @@ -117,23 +133,35 @@ public class Tuning extends SelectableOpMode { } /** - * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a + * This is the LocalizationTest OpMode. This is basically just a simple drive attached to a * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot. * You should use this to check the robot's localization. * * @author Anyi Lin - 10158 Scott's Bots * @author Baron Henderson - 20077 The Indubitables + * @author Kabir Goyal * @version 1.0, 5/6/2024 */ class LocalizationTest extends OpMode { - @Override - public void init() {} + boolean debugStringEnabled = false; - /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */ + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + } + + /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ @Override public void init_loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + telemetryM.debug("This will print your robot's position to telemetry while " - + "allowing robot control through a basic mecanum drive on gamepad 1."); + + "allowing robot control through a basic drive on gamepad 1."); + telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + " (press gamepad a to toggle)"); telemetryM.update(telemetry); follower.update(); drawCurrent(); @@ -146,11 +174,15 @@ class LocalizationTest extends OpMode { } /** - * This updates the robot's pose estimate, the simple mecanum drive, and updates the + * This updates the robot's pose estimate, the simple drive, and updates the * Panels telemetry with the robot's position as well as draws the robot's position. */ @Override public void loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); follower.update(); @@ -158,6 +190,10 @@ class LocalizationTest extends OpMode { telemetryM.debug("y:" + follower.getPose().getY()); telemetryM.debug("heading:" + follower.getPose().getHeading()); telemetryM.debug("total heading:" + follower.getTotalHeading()); + if (debugStringEnabled) { + telemetryM.debug("Drivetrain Debug String:\n" + + follower.getDrivetrain().debugString()); + } telemetryM.update(telemetry); drawCurrentAndHistory(); @@ -182,6 +218,7 @@ class ForwardTuner extends OpMode { @Override public void init() { + follower.setStartingPose(new Pose(72,72)); follower.update(); drawCurrent(); } @@ -229,6 +266,7 @@ class LateralTuner extends OpMode { @Override public void init() { + follower.setStartingPose(new Pose(72,72)); follower.update(); drawCurrent(); } @@ -276,6 +314,7 @@ class TurnTuner extends OpMode { @Override public void init() { + follower.setStartingPose(new Pose(72,72)); follower.update(); drawCurrent(); } @@ -312,7 +351,7 @@ class TurnTuner extends OpMode { * reaching the end of the distance, it averages them and prints out the velocity obtained. It is * recommended to run this multiple times on a full battery to get the best results. What this does * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that - * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * empirically represents the direction your wheels actually prefer to go in, allowing for * more accurate following. * * @author Anyi Lin - 10158 Scott's Bots @@ -323,13 +362,15 @@ class TurnTuner extends OpMode { */ class ForwardVelocityTuner extends OpMode { private final ArrayList velocities = new ArrayList<>(); - public static double DISTANCE = 48; + public static double DISTANCE = 120; public static double RECORD_NUMBER = 10; private boolean end; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ @Override @@ -374,7 +415,7 @@ class ForwardVelocityTuner extends OpMode { if (!end) { - if (Math.abs(follower.getPose().getX()) > DISTANCE) { + if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) { end = true; stopRobot(); } else { @@ -417,7 +458,7 @@ class ForwardVelocityTuner extends OpMode { * reaching the end of the distance, it averages them and prints out the velocity obtained. It is * recommended to run this multiple times on a full battery to get the best results. What this does * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that - * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * empirically represents the direction your wheels actually prefer to go in, allowing for * more accurate following. * * @author Anyi Lin - 10158 Scott's Bots @@ -429,13 +470,15 @@ class ForwardVelocityTuner extends OpMode { class LateralVelocityTuner extends OpMode { private final ArrayList velocities = new ArrayList<>(); - public static double DISTANCE = 48; + public static double DISTANCE = 120; public static double RECORD_NUMBER = 10; private boolean end; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** * This initializes the drive motors as well as the cache of velocities and the Panels @@ -480,7 +523,7 @@ class LateralVelocityTuner extends OpMode { drawCurrentAndHistory(); if (!end) { - if (Math.abs(follower.getPose().getY()) > DISTANCE) { + if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) { end = true; stopRobot(); } else { @@ -537,7 +580,9 @@ class ForwardZeroPowerAccelerationTuner extends OpMode { private boolean end; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** This initializes the drive motors as well as the Panels telemetryM. */ @Override @@ -639,7 +684,9 @@ class LateralZeroPowerAccelerationTuner extends OpMode { private boolean end; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** This initializes the drive motors as well as the Panels telemetry. */ @Override @@ -717,6 +764,185 @@ class LateralZeroPowerAccelerationTuner extends OpMode { } } +/** + * This is the Predictive Braking Tuner. It runs the robot forward and backward at various power + * levels, recording the robot’s velocity and position immediately before braking. The motors are + * then set to a reverse power, which represents the fastest theoretical braking the robot + * can achieve. Once the robot comes to a complete stop, the tuner measures the stopping distance. + * Using the collected data, it generates a velocity-vs-stopping-distance graph and fits a + * quadratic curve to model the braking behavior. + * + * @author Ashay Sarda - 19745 Turtle Walkers + * @author Jacob Ophoven - 18535 Frozen Code + * @version 1.0, 12/26/2025 + */ +class PredictiveBrakingTuner extends OpMode { + private static final double[] TEST_POWERS = + {1, 1, 1, 0.9, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2}; + private static final double BRAKING_POWER = -0.2; + + private static final int DRIVE_TIME_MS = 1000; + + private enum State { + START_MOVE, + WAIT_DRIVE_TIME, + APPLY_BRAKE, + WAIT_BRAKE_TIME, + RECORD, + DONE + } + + private static class BrakeRecord { + double timeMs; + Pose pose; + double velocity; + + BrakeRecord(double timeMs, Pose pose, double velocity) { + this.timeMs = timeMs; + this.pose = pose; + this.velocity = velocity; + } + } + + private State state = State.START_MOVE; + + private final ElapsedTime timer = new ElapsedTime(); + + private int iteration = 0; + + private Vector startPosition; + private double measuredVelocity; + + private final List velocityToBrakingDistance = new ArrayList<>(); + private final List brakeData = new ArrayList<>(); + + @Override + public void init() {} + + @Override + public void init_loop() { + telemetryM.debug("The robot will move forwards and backwards starting at max speed and slowing down."); + telemetryM.debug("Make sure you have enough room. Leave at least 4-5 feet."); + telemetryM.debug("After stopping, kFriction and kBraking will be displayed."); + telemetryM.debug("Make sure to turn the timer off."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawCurrent(); + } + + @Override + public void start() { + timer.reset(); + follower.update(); + follower.startTeleOpDrive(true); + } + + @SuppressLint("DefaultLocale") + @Override + public void loop() { + follower.update(); + + if (gamepad1.b) { + stopRobot(); + requestOpModeStop(); + return; + } + + double direction = (iteration % 2 == 0) ? 1 : -1; + + switch (state) { + case START_MOVE: { + if (iteration >= TEST_POWERS.length) { + state = State.DONE; + break; + } + + double currentPower = TEST_POWERS[iteration]; + follower.setMaxPower(currentPower); + follower.setTeleOpDrive(direction, 0, 0, true); + + timer.reset(); + state = State.WAIT_DRIVE_TIME; + break; + } + + case WAIT_DRIVE_TIME: { + if (timer.milliseconds() >= DRIVE_TIME_MS) { + measuredVelocity = follower.getVelocity().getMagnitude(); + startPosition = follower.getPose().getAsVector(); + state = State.APPLY_BRAKE; + } + break; + } + + case APPLY_BRAKE: { + follower.setTeleOpDrive(BRAKING_POWER * direction, 0, 0, true); + + timer.reset(); + state = State.WAIT_BRAKE_TIME; + break; + } + + case WAIT_BRAKE_TIME: { + double t = timer.milliseconds(); + Pose currentPose = follower.getPose(); + double currentVelocity = follower.getVelocity().getMagnitude(); + + brakeData.add(new BrakeRecord(t, currentPose, currentVelocity)); + + if (follower.getVelocity().dot(new Vector(direction, + follower.getHeading())) <= 0) { + state = State.RECORD; + } + break; + } + + case RECORD: { + Vector endPosition = follower.getPose().getAsVector(); + double brakingDistance = endPosition.minus(startPosition).getMagnitude(); + + velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); + + telemetryM.debug("Test " + iteration, + String.format("v=%.3f d=%.3f", measuredVelocity, + brakingDistance)); + telemetryM.update(telemetry); + + iteration++; + state = State.START_MOVE; + + break; + } + + case DONE: { + stopRobot(); + + double[] coefficients = quadraticFit(velocityToBrakingDistance); + + telemetryM.debug("Tuning Complete"); + telemetryM.debug("Braking Profile:"); + telemetryM.debug("kQuadratic", coefficients[1]); + telemetryM.debug("kLinear", coefficients[0]); + telemetryM.update(telemetry); + telemetryM.debug("Tuning Complete"); + telemetryM.debug("Braking Profile:"); + telemetryM.debug("kQuadraticFriction", coefficients[1]); + telemetryM.debug("kLinearBraking", coefficients[0]); + for (BrakeRecord record : brakeData) { + Pose p = record.pose; + telemetryM.debug(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f", + record.timeMs, p.getX(), p.getY(), + p.getHeading(), + record.velocity)); + } + telemetryM.update(); + break; + } + } + } +} + /** * This is the Translational PIDF Tuner OpMode. It will keep the robot in place. * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly. @@ -735,7 +961,9 @@ class TranslationalTuner extends OpMode { private Path backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** This initializes the Follower and creates the forward and backward Paths. */ @Override @@ -752,9 +980,9 @@ class TranslationalTuner extends OpMode { public void start() { follower.deactivateAllPIDFs(); follower.activateTranslational(); - forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); backwards.setConstantHeadingInterpolation(0); follower.followPath(forwards); } @@ -776,6 +1004,9 @@ class TranslationalTuner extends OpMode { } telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent()); + telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent()); telemetryM.update(telemetry); } } @@ -799,7 +1030,9 @@ class HeadingTuner extends OpMode { private Path backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** * This initializes the Follower and creates the forward and backward Paths. Additionally, this @@ -819,9 +1052,9 @@ class HeadingTuner extends OpMode { public void start() { follower.deactivateAllPIDFs(); follower.activateHeading(); - forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); backwards.setConstantHeadingInterpolation(0); follower.followPath(forwards); } @@ -846,6 +1079,8 @@ class HeadingTuner extends OpMode { } telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getHeadingError()); telemetryM.update(telemetry); } } @@ -867,7 +1102,9 @@ class DriveTuner extends OpMode { private PathChain backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** * This initializes the Follower and creates the forward and backward Paths. Additionally, this @@ -890,13 +1127,13 @@ class DriveTuner extends OpMode { forwards = follower.pathBuilder() .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))) + .addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))) .setConstantHeadingInterpolation(0) .build(); backwards = follower.pathBuilder() .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))) + .addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))) .setConstantHeadingInterpolation(0) .build(); @@ -923,6 +1160,8 @@ class DriveTuner extends OpMode { } telemetryM.debug("Driving forward?: " + forward); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]); telemetryM.update(telemetry); } } @@ -945,7 +1184,9 @@ class Line extends OpMode { private Path backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** This initializes the Follower and creates the forward and backward Paths. */ @Override @@ -961,9 +1202,9 @@ class Line extends OpMode { @Override public void start() { follower.activateAllPIDFs(); - forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); backwards.setConstantHeadingInterpolation(0); follower.followPath(forwards); } @@ -1010,7 +1251,9 @@ class CentripetalTuner extends OpMode { private Path backwards; @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** * This initializes the Follower and creates the forward and backward Paths. @@ -1029,8 +1272,8 @@ class CentripetalTuner extends OpMode { @Override public void start() { follower.activateAllPIDFs(); - forwards = new Path(new BezierCurve(new Pose(), new Pose(Math.abs(DISTANCE),0), new Pose(Math.abs(DISTANCE),DISTANCE))); - backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE),DISTANCE), new Pose(Math.abs(DISTANCE),0), new Pose(0,0))); + forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72))); + backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72))); backwards.setTangentHeadingInterpolation(); backwards.reverseHeadingInterpolation(); @@ -1071,9 +1314,9 @@ class CentripetalTuner extends OpMode { */ class Triangle extends OpMode { - private final Pose startPose = new Pose(0, 0, Math.toRadians(0)); - private final Pose interPose = new Pose(24, -24, Math.toRadians(90)); - private final Pose endPose = new Pose(24, 24, Math.toRadians(45)); + private final Pose startPose = new Pose(72, 72, Math.toRadians(0)); + private final Pose interPose = new Pose(24 + 72, -24 + 72, Math.toRadians(90)); + private final Pose endPose = new Pose(24 + 72, 24 + 72, Math.toRadians(45)); private PathChain triangle; @@ -1092,7 +1335,9 @@ class Triangle extends OpMode { } @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } @Override public void init_loop() { @@ -1138,14 +1383,14 @@ class Circle extends OpMode { public void start() { circle = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(0, 0), new Pose(RADIUS, 0), new Pose(RADIUS, RADIUS))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .addPath(new BezierCurve(new Pose(RADIUS, RADIUS), new Pose(RADIUS, 2 * RADIUS), new Pose(0, 2 * RADIUS))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .addPath(new BezierCurve(new Pose(0, 2 * RADIUS), new Pose(-RADIUS, 2 * RADIUS), new Pose(-RADIUS, RADIUS))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) - .addPath(new BezierCurve(new Pose(-RADIUS, RADIUS), new Pose(-RADIUS, 0), new Pose(0, 0))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(0, RADIUS)) + .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) .build(); follower.followPath(circle); } @@ -1161,7 +1406,9 @@ class Circle extends OpMode { } @Override - public void init() {} + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } /** * This runs the OpMode, updating the Follower as well as printing out the debug statements to @@ -1178,6 +1425,230 @@ class Circle extends OpMode { } } +/** + * Tuning OpMode to get the min and max encoder values for swerve pods + * @author Kabir Goyal + */ +class AnalogMinMaxTuner extends OpMode { + //populate the below with your names for the servos and encoders + public String[] encoderNames = {"leftFrontEncoder", "rightFrontEncoder", "leftBackEncoder", "rightBackEncoder"}; + public AnalogInput[] encoders = new AnalogInput[encoderNames.length]; + public double[] minVoltages = new double[encoderNames.length]; + public double[] maxVoltages = new double[encoderNames.length]; + + public List lynxModules; //js to improve loop times a bit yk + + public void start() { + } + + @Override + public void init_loop() { + telemetryM.debug("Press START. Then, Spin each pod slowly for 4 to 5 full rotations.\n" + + "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry."); + telemetryM.update(telemetry); + } + + @Override + public void init() { + lynxModules = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : lynxModules) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + + for (int i = 0; i < encoders.length; i++) { + encoders[i] = hardwareMap.get(AnalogInput.class, encoderNames[i]); + minVoltages[i] = 5; //bigger value than should ever be read + } + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + for (LynxModule hub : lynxModules) { + hub.clearBulkCache(); + } + + telemetryM.debug("Spin each pod slowly for 4 to 5 full rotations.\n" + + "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry.\n\n"); + + for (int i = 0; i < encoders.length; i++) { + double currentVoltage = encoders[i].getVoltage(); + minVoltages[i] = Math.min(minVoltages[i], currentVoltage); + maxVoltages[i] = Math.max(maxVoltages[i], currentVoltage); + telemetryM.addData(encoderNames[i] + "min value:", minVoltages[i]); + telemetryM.addData(encoderNames[i] + "max value:", maxVoltages[i]); + telemetryM.addLine(""); + } + + telemetryM.update(); + } +} + +/** + * This is the SwerveOffsetsTest + * You should use this to check how good your swerve angle offsets are and if your motor directions are correct + * @author Kabir Goyal + * + */ +class SwerveOffsetsTest extends OpMode { + boolean debugStringEnabled = false; + + @Override + public void init() {} + + /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ + @Override + public void init_loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + + telemetryM.debug("This OpMode will run all four swerve pods in the direction they think is forward" + + "\nensure your bot is not on the ground while running"); + telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + " (press gamepad a to toggle)"); + telemetryM.update(telemetry); + follower.update(); + drawCurrent(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + follower.update(); + } + + /** + * This updates the robot's pose estimate, the simple drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + follower.setTeleOpDrive(0.25, 0, 0, true); + follower.update(); + + if (debugStringEnabled) { + telemetryM.debug("Drivetrain Debug String:\n" + + follower.getDrivetrain().debugString()); + } + telemetryM.update(telemetry); + + drawCurrentAndHistory(); + } +} + +/** + * This is the SwerveTurnTest + * You should use this to check your encoder directions and x/y pod offsets + * @author Kabir Goyal + * + */ +class SwerveTurnTest extends OpMode { + boolean debugStringEnabled = false; + + @Override + public void init() {} + + /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ + @Override + public void init_loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + + telemetryM.debug("This OpMode will run all four swerve pods in their turning direction (perpendicular to the center of the robot) " + + "\nrun this once off the ground to check servo directions and motor directions before testing on the ground"); + telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + " (press gamepad a to toggle)"); + telemetryM.update(telemetry); + follower.update(); + drawCurrent(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + follower.update(); + } + + /** + * This updates the robot's pose estimate, the simple drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { + debugStringEnabled = !debugStringEnabled; + } + + follower.setTeleOpDrive(0, 0, 0.25, true); + follower.update(); + + if (debugStringEnabled) { + telemetryM.debug("Drivetrain Debug String:\n" + + follower.getDrivetrain().debugString()); + } + telemetryM.update(telemetry); + + drawCurrentAndHistory(); + } +} + +/** + * This is the OffsetsTuner OpMode. This tracks the movement of the robot as it turns 180 degrees, + * and calculates what the robot's strafeX and forwardY offsets should be. Ensure that your strafeX and forwardY offsets + * are set to 0 before running this OpMode. After running, input the displayed offsets into your localizer constants. + * + * @author Havish Sripada - 12808 RevAmped Robotics + * @author Baron Henderson + */ +class OffsetsTuner extends OpMode { + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants."); + telemetryM.debug("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry."); + telemetryM.update(telemetry); + + drawCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated offsets and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Total Angle: " + follower.getTotalHeading()); + + telemetryM.debug("The following values are the offsets in inches that should be applied to your localizer."); + telemetryM.debug("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); + telemetryM.debug("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); + telemetryM.update(telemetry); + + drawCurrentAndHistory(); + } +} + + /** * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. * @@ -1189,10 +1660,10 @@ class Drawing { private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); private static final Style robotLook = new Style( - "", "#3F51B5", 0.0 + "", "#3F51B5", 0.75 ); private static final Style historyLook = new Style( - "", "#4CAF50", 0.0 + "", "#4CAF50", 0.75 ); /** diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 70755bd..bd7c756 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -21,9 +21,9 @@ dependencies { implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'com.pedropathing:ftc:2.0.6' //PedroCore - implementation 'com.pedropathing:telemetry:1.0.0' //PedroTele - implementation 'com.bylazar:fullpanels:1.0.2' //Panels + implementation 'com.pedropathing:ftc:2.1.1' + implementation 'com.pedropathing:telemetry:1.0.0' + implementation 'com.bylazar:fullpanels:1.0.12' implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB implementation 'com.rowanmcalpin.nextftc:core:0.6.2' //NEXT FTC From 6a3f65d4c52857aa3e31d18f6432b1c658e5b291 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 10 Apr 2026 19:53:54 -0500 Subject: [PATCH 13/24] before adding pidf constants --- .../ftc/teamcode/pedroPathing/Constants.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index 7144cad..f1e2af8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -14,7 +14,9 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class Constants { public static FollowerConstants followerConstants = new FollowerConstants() - .mass(15.5); + .mass(15.5) + .forwardZeroPowerAcceleration(-29.512) + .lateralZeroPowerAcceleration(-72.872); public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) @@ -32,8 +34,8 @@ public class Constants { public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); public static PinpointConstants localizerConstants = new PinpointConstants() - .forwardPodY(7.5) - .strafePodX(3.75) + .forwardPodY(-7.5) + .strafePodX(-3.75) .distanceUnit(DistanceUnit.INCH) .hardwareMapName("pinpoint") .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) From 1723f6f85dd52c3ccf4005217c1cb3fa85affc21 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 10 Apr 2026 21:52:56 -0500 Subject: [PATCH 14/24] tuned translational pidf --- .../ftc/teamcode/pedroPathing/Constants.java | 9 +++++++-- .../firstinspires/ftc/teamcode/pedroPathing/Tuning.java | 2 ++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index f1e2af8..b9063d5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing; +import com.acmerobotics.dashboard.config.Config; +import com.pedropathing.control.PIDFCoefficients; import com.pedropathing.follower.Follower; import com.pedropathing.follower.FollowerConstants; import com.pedropathing.ftc.FollowerBuilder; @@ -11,12 +13,15 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; - +@Config public class Constants { public static FollowerConstants followerConstants = new FollowerConstants() .mass(15.5) .forwardZeroPowerAcceleration(-29.512) - .lateralZeroPowerAcceleration(-72.872); + .lateralZeroPowerAcceleration(-72.872) + .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)); + + public static int DEBUG_VAR = 0; public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 7dbf3e9..1258ea1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -10,6 +10,7 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM; import android.annotation.SuppressLint; +import com.acmerobotics.dashboard.config.Config; import com.bylazar.configurables.PanelsConfigurables; import com.bylazar.configurables.annotations.Configurable; import com.bylazar.configurables.annotations.IgnoreConfigurable; @@ -44,6 +45,7 @@ import java.util.List; * @version 1.0, 6/26/2025 */ @Configurable +@Config @TeleOp(name = "Tuning", group = "Pedro Pathing") public class Tuning extends SelectableOpMode { public static Follower follower; From 6c905f2506c6ba257ea3fc13ecb52bc605ca870c Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 10 Apr 2026 21:56:55 -0500 Subject: [PATCH 15/24] ignore --- .../org/firstinspires/ftc/teamcode/pedroPathing/Constants.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index b9063d5..5a30fe1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -21,8 +21,6 @@ public class Constants { .lateralZeroPowerAcceleration(-72.872) .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)); - public static int DEBUG_VAR = 0; - public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) .rightFrontMotorName("fr") From f3efc132e78e562aae111158cf4829ad5884240a Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 14 Apr 2026 20:26:19 -0500 Subject: [PATCH 16/24] tuned heading pidf and fixed dash --- .../ftc/teamcode/pedroPathing/Constants.java | 6 +- .../ftc/teamcode/pedroPathing/Tuning.java | 516 +++++++++--------- 2 files changed, 250 insertions(+), 272 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index 5a30fe1..2190bff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -19,7 +19,8 @@ public class Constants { .mass(15.5) .forwardZeroPowerAcceleration(-29.512) .lateralZeroPowerAcceleration(-72.872) - .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)); + .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)) + .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)); public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) @@ -34,7 +35,8 @@ public class Constants { .xVelocity(64.675) .yVelocity(49.583); - public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + public static double breakingStrength = 1.25; + public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1); public static PinpointConstants localizerConstants = new PinpointConstants() .forwardPodY(-7.5) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 1258ea1..0de897d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -8,12 +8,15 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower; import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot; import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM; +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; + import android.annotation.SuppressLint; -import com.acmerobotics.dashboard.config.Config; import com.bylazar.configurables.PanelsConfigurables; -import com.bylazar.configurables.annotations.Configurable; -import com.bylazar.configurables.annotations.IgnoreConfigurable; import com.bylazar.field.FieldManager; import com.bylazar.field.PanelsField; import com.bylazar.field.Style; @@ -44,19 +47,15 @@ import java.util.List; * @author Baron Henderson - 20077 The Indubitables * @version 1.0, 6/26/2025 */ -@Configurable @Config @TeleOp(name = "Tuning", group = "Pedro Pathing") public class Tuning extends SelectableOpMode { public static Follower follower; - @IgnoreConfigurable static PoseHistory poseHistory; - @IgnoreConfigurable static TelemetryManager telemetryM; - @IgnoreConfigurable static ArrayList changes = new ArrayList<>(); public Tuning() { @@ -115,7 +114,7 @@ public class Tuning extends SelectableOpMode { public static void drawCurrent() { try { - Drawing.drawRobot(follower.getPose()); + Drawing.drawRobot(follower.getPose(), "blue"); Drawing.sendPacket(); } catch (Exception e) { throw new RuntimeException("Drawing failed " + e); @@ -123,7 +122,7 @@ public class Tuning extends SelectableOpMode { } public static void drawCurrentAndHistory() { - Drawing.drawPoseHistory(poseHistory); + Drawing.drawPoseHistory(poseHistory, "blue"); drawCurrent(); } @@ -160,11 +159,11 @@ class LocalizationTest extends OpMode { } - telemetryM.debug("This will print your robot's position to telemetry while " + telemetry.addLine("This will print your robot's position to telemetry while " + "allowing robot control through a basic drive on gamepad 1."); - telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + " (press gamepad a to toggle)"); - telemetryM.update(telemetry); + telemetry.update(); follower.update(); drawCurrent(); } @@ -188,15 +187,15 @@ class LocalizationTest extends OpMode { follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); follower.update(); - telemetryM.debug("x:" + follower.getPose().getX()); - telemetryM.debug("y:" + follower.getPose().getY()); - telemetryM.debug("heading:" + follower.getPose().getHeading()); - telemetryM.debug("total heading:" + follower.getTotalHeading()); + telemetry.addLine("x:" + follower.getPose().getX()); + telemetry.addLine("y:" + follower.getPose().getY()); + telemetry.addLine("heading:" + follower.getPose().getHeading()); + telemetry.addLine("total heading:" + follower.getTotalHeading()); if (debugStringEnabled) { - telemetryM.debug("Drivetrain Debug String:\n" + + telemetry.addLine("Drivetrain Debug String:\n" + follower.getDrivetrain().debugString()); } - telemetryM.update(telemetry); + telemetry.update(); drawCurrentAndHistory(); } @@ -228,8 +227,8 @@ class ForwardTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); + telemetry.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -241,10 +240,10 @@ class ForwardTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("Distance Moved: " + follower.getPose().getX()); - telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); - telemetryM.update(telemetry); + telemetry.addLine("Distance Moved: " + follower.getPose().getX()); + telemetry.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetry.addLine("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); + telemetry.update(); drawCurrentAndHistory(); } @@ -276,8 +275,8 @@ class LateralTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); + telemetry.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -289,10 +288,10 @@ class LateralTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("Distance Moved: " + follower.getPose().getY()); - telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); - telemetryM.update(telemetry); + telemetry.addLine("Distance Moved: " + follower.getPose().getY()); + telemetry.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetry.addLine("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); + telemetry.update(); drawCurrentAndHistory(); } @@ -324,8 +323,8 @@ class TurnTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); + telemetry.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -338,10 +337,10 @@ class TurnTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("Total Angle: " + follower.getTotalHeading()); - telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); - telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); - telemetryM.update(telemetry); + telemetry.addLine("Total Angle: " + follower.getTotalHeading()); + telemetry.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetry.addLine("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); + telemetry.update(); drawCurrentAndHistory(); } @@ -377,12 +376,12 @@ class ForwardVelocityTuner extends OpMode { /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); - telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); - telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.debug("pose", follower.getPose()); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); + telemetry.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetry.addLine("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); + telemetry.addLine("Press B on game pad 1 to stop."); + telemetry.addData("pose", follower.getPose()); + telemetry.update(); follower.update(); drawCurrent(); @@ -434,15 +433,15 @@ class ForwardVelocityTuner extends OpMode { average += velocity; } average /= velocities.size(); - telemetryM.debug("Forward Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); + telemetry.addLine("Forward Velocity: " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Forward Velocity temporarily (while robot remains on)."); for (int i = 0; i < velocities.size(); i++) { telemetry.addData(String.valueOf(i), velocities.get(i)); } - telemetryM.update(telemetry); + telemetry.update(); telemetry.update(); if (gamepad1.aWasPressed()) { @@ -488,11 +487,11 @@ class LateralVelocityTuner extends OpMode { */ @Override public void init_loop() { - telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right."); - telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); - telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); - telemetryM.debug("Press B on Gamepad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the right."); + telemetry.addLine("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetry.addLine("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); + telemetry.addLine("Press B on Gamepad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); @@ -542,10 +541,10 @@ class LateralVelocityTuner extends OpMode { } average /= velocities.size(); - telemetryM.debug("Strafe Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); - telemetryM.update(telemetry); + telemetry.addLine("Strafe Velocity: " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Lateral Velocity temporarily (while robot remains on)."); + telemetry.update(); if (gamepad1.aWasPressed()) { follower.setYVelocity(average); @@ -589,12 +588,12 @@ class ForwardZeroPowerAccelerationTuner extends OpMode { /** This initializes the drive motors as well as the Panels telemetryM. */ @Override public void init_loop() { - telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second."); - telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on Gamepad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run forward until it reaches " + VELOCITY + " inches per second."); + telemetry.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetry.addLine("Make sure you have enough room."); + telemetry.addLine("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); + telemetry.addLine("Press B on Gamepad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -648,10 +647,10 @@ class ForwardZeroPowerAccelerationTuner extends OpMode { } average /= accelerations.size(); - telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); + telemetry.addLine("Forward Zero Power Acceleration (Deceleration): " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); + telemetry.update(); if (gamepad1.aWasPressed()) { follower.getConstants().setForwardZeroPowerAcceleration(average); @@ -693,12 +692,12 @@ class LateralZeroPowerAccelerationTuner extends OpMode { /** This initializes the drive motors as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); - telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will run to the right until it reaches " + VELOCITY + " inches per second."); + telemetry.addLine("Then, it will cut power from the drivetrain and roll to a stop."); + telemetry.addLine("Make sure you have enough room."); + telemetry.addLine("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); + telemetry.addLine("Press B on game pad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -752,10 +751,10 @@ class LateralZeroPowerAccelerationTuner extends OpMode { } average /= accelerations.size(); - telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); + telemetry.addLine("Lateral Zero Power Acceleration (Deceleration): " + average); + telemetry.addLine("\n"); + telemetry.addLine("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); + telemetry.update(); if (gamepad1.aWasPressed()) { follower.getConstants().setLateralZeroPowerAcceleration(average); @@ -823,12 +822,12 @@ class PredictiveBrakingTuner extends OpMode { @Override public void init_loop() { - telemetryM.debug("The robot will move forwards and backwards starting at max speed and slowing down."); - telemetryM.debug("Make sure you have enough room. Leave at least 4-5 feet."); - telemetryM.debug("After stopping, kFriction and kBraking will be displayed."); - telemetryM.debug("Make sure to turn the timer off."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.update(telemetry); + telemetry.addLine("The robot will move forwards and backwards starting at max speed and slowing down."); + telemetry.addLine("Make sure you have enough room. Leave at least 4-5 feet."); + telemetry.addLine("After stopping, kFriction and kBraking will be displayed."); + telemetry.addLine("Make sure to turn the timer off."); + telemetry.addLine("Press B on game pad 1 to stop."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -906,10 +905,8 @@ class PredictiveBrakingTuner extends OpMode { velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); - telemetryM.debug("Test " + iteration, - String.format("v=%.3f d=%.3f", measuredVelocity, - brakingDistance)); - telemetryM.update(telemetry); + telemetry.addData("Test " + iteration, String.format("v=%.3f d=%.3f", measuredVelocity, brakingDistance)); + telemetry.update(); iteration++; state = State.START_MOVE; @@ -922,23 +919,23 @@ class PredictiveBrakingTuner extends OpMode { double[] coefficients = quadraticFit(velocityToBrakingDistance); - telemetryM.debug("Tuning Complete"); - telemetryM.debug("Braking Profile:"); - telemetryM.debug("kQuadratic", coefficients[1]); - telemetryM.debug("kLinear", coefficients[0]); - telemetryM.update(telemetry); - telemetryM.debug("Tuning Complete"); - telemetryM.debug("Braking Profile:"); - telemetryM.debug("kQuadraticFriction", coefficients[1]); - telemetryM.debug("kLinearBraking", coefficients[0]); + telemetry.addLine("Tuning Complete"); + telemetry.addLine("Braking Profile:"); + telemetry.addData("kQuadratic", coefficients[1]); + telemetry.addData("kLinear", coefficients[0]); + telemetry.update(); + telemetry.addLine("Tuning Complete"); + telemetry.addLine("Braking Profile:"); + telemetry.addData("kQuadraticFriction", coefficients[1]); + telemetry.addData("kLinearBraking", coefficients[0]); for (BrakeRecord record : brakeData) { Pose p = record.pose; - telemetryM.debug(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f", + telemetry.addLine(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f", record.timeMs, p.getX(), p.getY(), p.getHeading(), record.velocity)); } - telemetryM.update(); + telemetry.update(); break; } } @@ -970,10 +967,10 @@ class TranslationalTuner extends OpMode { /** This initializes the Follower and creates the forward and backward Paths. */ @Override public void init_loop() { - telemetryM.debug("This will activate the translational PIDF(s)"); - telemetryM.debug("The robot will try to stay in place while you push it laterally."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("This will activate the translational PIDF(s)"); + telemetry.addLine("The robot will try to stay in place while you push it laterally."); + telemetry.addLine("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1005,11 +1002,11 @@ class TranslationalTuner extends OpMode { } } - telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); + telemetry.addLine("Push the robot laterally to test the Translational PIDF(s)."); telemetryM.addData("Zero Line", 0); telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent()); telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent()); - telemetryM.update(telemetry); + telemetry.update(); } } @@ -1042,10 +1039,10 @@ class HeadingTuner extends OpMode { */ @Override public void init_loop() { - telemetryM.debug("This will activate the heading PIDF(s)."); - telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("This will activate the heading PIDF(s)."); + telemetry.addLine("The robot will try to stay at a constant heading while you try to turn it."); + telemetry.addLine("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1080,10 +1077,10 @@ class HeadingTuner extends OpMode { } } - telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); + telemetry.addLine("Turn the robot manually to test the Heading PIDF(s)."); telemetryM.addData("Zero Line", 0); telemetryM.addData("Error", follower.errorCalculator.getHeadingError()); - telemetryM.update(telemetry); + telemetry.update(); } } @@ -1114,10 +1111,10 @@ class DriveTuner extends OpMode { */ @Override public void init_loop() { - telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward."); - telemetryM.debug("The robot will go forward and backward continuously along the path."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.update(telemetry); + telemetry.addLine("This will run the robot in a straight line going " + DISTANCE + "inches forward."); + telemetry.addLine("The robot will go forward and backward continuously along the path."); + telemetry.addLine("Make sure you have enough room."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1161,10 +1158,10 @@ class DriveTuner extends OpMode { } } - telemetryM.debug("Driving forward?: " + forward); + telemetry.addLine("Driving forward?: " + forward); telemetryM.addData("Zero Line", 0); telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]); - telemetryM.update(telemetry); + telemetry.update(); } } @@ -1193,10 +1190,10 @@ class Line extends OpMode { /** This initializes the Follower and creates the forward and backward Paths. */ @Override public void init_loop() { - telemetryM.debug("This will activate all the PIDF(s)"); - telemetryM.debug("The robot will go forward and backward continuously along the path while correcting."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); - telemetryM.update(telemetry); + telemetry.addLine("This will activate all the PIDF(s)"); + telemetry.addLine("The robot will go forward and backward continuously along the path while correcting."); + telemetry.addLine("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1227,8 +1224,8 @@ class Line extends OpMode { } } - telemetryM.debug("Driving Forward?: " + forward); - telemetryM.update(telemetry); + telemetry.addLine("Driving Forward?: " + forward); + telemetry.update(); } } @@ -1263,10 +1260,10 @@ class CentripetalTuner extends OpMode { */ @Override public void init_loop() { - telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); - telemetryM.debug("The robot will go continuously along the path."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.update(telemetry); + telemetry.addLine("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); + telemetry.addLine("The robot will go continuously along the path."); + telemetry.addLine("Make sure you have enough room."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1301,8 +1298,8 @@ class CentripetalTuner extends OpMode { } } - telemetryM.debug("Driving away from the origin along the curve?: " + forward); - telemetryM.update(telemetry); + telemetry.addLine("Driving away from the origin along the curve?: " + forward); + telemetry.update(); } } @@ -1343,9 +1340,9 @@ class Triangle extends OpMode { @Override public void init_loop() { - telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point."); - telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode."); - telemetryM.update(telemetry); + telemetry.addLine("This will run in a roughly triangular shape, starting on the bottom-middle point."); + telemetry.addLine("So, make sure you have enough space to the left, front, and right to run the OpMode."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1399,10 +1396,10 @@ class Circle extends OpMode { @Override public void init_loop() { - telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); - telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode."); - telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); - telemetryM.update(telemetry); + telemetry.addLine("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); + telemetry.addLine("So, make sure you have enough space to the left, front, and back to run the OpMode."); + telemetry.addLine("It will also continuously face the center of the circle to test your heading and centripetal correction."); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1445,9 +1442,9 @@ class AnalogMinMaxTuner extends OpMode { @Override public void init_loop() { - telemetryM.debug("Press START. Then, Spin each pod slowly for 4 to 5 full rotations.\n" + + telemetry.addLine("Press START. Then, Spin each pod slowly for 4 to 5 full rotations.\n" + "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry."); - telemetryM.update(telemetry); + telemetry.update(); } @Override @@ -1473,7 +1470,7 @@ class AnalogMinMaxTuner extends OpMode { hub.clearBulkCache(); } - telemetryM.debug("Spin each pod slowly for 4 to 5 full rotations.\n" + + telemetry.addLine("Spin each pod slowly for 4 to 5 full rotations.\n" + "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry.\n\n"); for (int i = 0; i < encoders.length; i++) { @@ -1485,7 +1482,7 @@ class AnalogMinMaxTuner extends OpMode { telemetryM.addLine(""); } - telemetryM.update(); + telemetry.update(); } } @@ -1509,11 +1506,11 @@ class SwerveOffsetsTest extends OpMode { } - telemetryM.debug("This OpMode will run all four swerve pods in the direction they think is forward" + telemetry.addLine("This OpMode will run all four swerve pods in the direction they think is forward" + "\nensure your bot is not on the ground while running"); - telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + " (press gamepad a to toggle)"); - telemetryM.update(telemetry); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1538,10 +1535,10 @@ class SwerveOffsetsTest extends OpMode { follower.update(); if (debugStringEnabled) { - telemetryM.debug("Drivetrain Debug String:\n" + + telemetry.addLine("Drivetrain Debug String:\n" + follower.getDrivetrain().debugString()); } - telemetryM.update(telemetry); + telemetry.update(); drawCurrentAndHistory(); } @@ -1567,11 +1564,11 @@ class SwerveTurnTest extends OpMode { } - telemetryM.debug("This OpMode will run all four swerve pods in their turning direction (perpendicular to the center of the robot) " + telemetry.addLine("This OpMode will run all four swerve pods in their turning direction (perpendicular to the center of the robot) " + "\nrun this once off the ground to check servo directions and motor directions before testing on the ground"); - telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + + telemetry.addLine("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + " (press gamepad a to toggle)"); - telemetryM.update(telemetry); + telemetry.update(); follower.update(); drawCurrent(); } @@ -1596,10 +1593,10 @@ class SwerveTurnTest extends OpMode { follower.update(); if (debugStringEnabled) { - telemetryM.debug("Drivetrain Debug String:\n" + + telemetry.addLine("Drivetrain Debug String:\n" + follower.getDrivetrain().debugString()); } - telemetryM.update(telemetry); + telemetry.update(); drawCurrentAndHistory(); } @@ -1624,9 +1621,9 @@ class OffsetsTuner extends OpMode { /** This initializes the PoseUpdater as well as the Panels telemetry. */ @Override public void init_loop() { - telemetryM.debug("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants."); - telemetryM.debug("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry."); - telemetryM.update(telemetry); + telemetry.addLine("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants."); + telemetry.addLine("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry."); + telemetry.update(); drawCurrent(); } @@ -1639,12 +1636,12 @@ class OffsetsTuner extends OpMode { public void loop() { follower.update(); - telemetryM.debug("Total Angle: " + follower.getTotalHeading()); + telemetry.addLine("Total Angle: " + follower.getTotalHeading()); - telemetryM.debug("The following values are the offsets in inches that should be applied to your localizer."); - telemetryM.debug("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); - telemetryM.debug("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); - telemetryM.update(telemetry); + telemetry.addLine("The following values are the offsets in inches that should be applied to your localizer."); + telemetry.addLine("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); + telemetry.addLine("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); + telemetry.update(); drawCurrentAndHistory(); } @@ -1652,147 +1649,126 @@ class OffsetsTuner extends OpMode { /** - * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. + * This is the Drawing class. It handles the drawing of stuff on FTC Dashboard, like the robot. * - * @author Lazar - 19234 - * @version 1.1, 5/19/2025 + * @author Logan Nash + * @author Anyi Lin - 10158 Scott's Bots + * @version 2.0, 11/03/2025 */ class Drawing { - public static final double ROBOT_RADIUS = 9; // woah - private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); - - private static final Style robotLook = new Style( - "", "#3F51B5", 0.75 - ); - private static final Style historyLook = new Style( - "", "#4CAF50", 0.75 - ); - - /** - * This prepares Panels Field for using Pedro Offsets - */ - public static void init() { - panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING()); - } + public static final double ROBOT_RADIUS = 9; + private static TelemetryPacket packet; /** * This draws everything that will be used in the Follower's telemetryDebug() method. This takes - * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * a Follower as an input, so an instance of the DashboardDrawingHandler class is not needed. * - * @param follower Pedro Follower instance. + * @param follower */ public static void drawDebug(Follower follower) { if (follower.getCurrentPath() != null) { - drawPath(follower.getCurrentPath(), robotLook); + drawPath(follower.getCurrentPath(), "#3F51B5"); Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); - drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook); + drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), "#3F51B5"); } - drawPoseHistory(follower.getPoseHistory(), historyLook); - drawRobot(follower.getPose(), historyLook); - + drawPoseHistory(follower.getPoseHistory(), "#4CAF50"); + drawRobot(follower.getPose(), "#4CAF50"); sendPacket(); } /** - * This draws a robot at a specified Pose with a specified - * look. The heading is represented as a line. + * This adds instructions to the current packet to draw a robot at a specified Pose with a specified + * color. If no packet exists, then a new one is created. * - * @param pose the Pose to draw the robot at - * @param style the parameters used to draw the robot with + * @param pose the Pose to draw the robot at + * @param color the color to draw the robot with */ - public static void drawRobot(Pose pose, Style style) { - if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { + public static void drawRobot(Pose pose, String color) { + if (packet == null) packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke(color); + Drawing.drawRobotOnCanvas(packet.fieldOverlay(), pose.copy()); + } + + /** + * This adds instructions to the current packet to draw a Path with a specified color. If no + * packet exists, then a new one is created. + * + * @param path the Path to draw + * @param color the color to draw the Path with + */ + public static void drawPath(Path path, String color) { + if (packet == null) packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke(color); + Drawing.drawPath(packet.fieldOverlay(), path.getPanelsDrawingPoints()); + } + + /** + * This adds instructions to the current packet to draw all the Paths in a PathChain with a + * specified color. If no packet exists, then a new one is created. + * + * @param pathChain the PathChain to draw + * @param color the color to draw the PathChain with + */ + public static void drawPath(PathChain pathChain, String color) { + for (int i = 0; i < pathChain.size(); i++) { + drawPath(pathChain.getPath(i), color); + } + } + + /** + * This adds instructions to the current packet to draw the pose history of the robot. If no + * packet exists, then a new one is created. + * + * @param poseTracker the DashboardPoseTracker to get the pose history from + * @param color the color to draw the pose history with + */ + public static void drawPoseHistory(PoseHistory poseTracker, String color) { + if (packet == null) packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke(color); + packet.fieldOverlay().strokePolyline(poseTracker.getXPositionsArray(), poseTracker.getYPositionsArray()); + } + + /** + * This tries to send the current packet to FTC Dashboard. + * + * @return returns if the operation was successful. + */ + public static boolean sendPacket() { + if (packet != null) { + FtcDashboard.getInstance().sendTelemetryPacket(packet); + packet = null; + return true; + } + return false; + } + + /** + * This draws a robot on the Dashboard at a specified Pose. This is more useful for drawing the + * actual robot, since the Pose contains the direction the robot is facing as well as its position. + * + * @param c the Canvas on the Dashboard on which this will draw at + * @param t the Pose to draw at + */ + public static void drawRobotOnCanvas(Canvas c, Pose t) { + if (t == null || Double.isNaN(t.getX()) || Double.isNaN(t.getY()) || Double.isNaN(t.getHeading())) { return; } - panelsField.setStyle(style); - panelsField.moveCursor(pose.getX(), pose.getY()); - panelsField.circle(ROBOT_RADIUS); - - Vector v = pose.getHeadingAsUnitVector(); + c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); + Vector v = t.getHeadingAsUnitVector(); v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); - double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; - double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); - - panelsField.setStyle(style); - panelsField.moveCursor(x1, y1); - panelsField.line(x2, y2); + double x1 = t.getX() + v.getXComponent() / 2, y1 = t.getY() + v.getYComponent() / 2; + double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent(); + c.strokeLine(x1, y1, x2, y2); } /** - * This draws a robot at a specified Pose. The heading is represented as a line. + * This draws a Path on the Dashboard from a specified Array of Points. * - * @param pose the Pose to draw the robot at + * @param c the Canvas on the Dashboard on which this will draw + * @param points the Points to draw */ - public static void drawRobot(Pose pose) { - drawRobot(pose, robotLook); - } - - /** - * This draws a Path with a specified look. - * - * @param path the Path to draw - * @param style the parameters used to draw the Path with - */ - public static void drawPath(Path path, Style style) { - double[][] points = path.getPanelsDrawingPoints(); - - for (int i = 0; i < points[0].length; i++) { - for (int j = 0; j < points.length; j++) { - if (Double.isNaN(points[j][i])) { - points[j][i] = 0; - } - } - } - - panelsField.setStyle(style); - panelsField.moveCursor(points[0][0], points[0][1]); - panelsField.line(points[1][0], points[1][1]); - } - - /** - * This draws all the Paths in a PathChain with a - * specified look. - * - * @param pathChain the PathChain to draw - * @param style the parameters used to draw the PathChain with - */ - public static void drawPath(PathChain pathChain, Style style) { - for (int i = 0; i < pathChain.size(); i++) { - drawPath(pathChain.getPath(i), style); - } - } - - /** - * This draws the pose history of the robot. - * - * @param poseTracker the PoseHistory to get the pose history from - * @param style the parameters used to draw the pose history with - */ - public static void drawPoseHistory(PoseHistory poseTracker, Style style) { - panelsField.setStyle(style); - - int size = poseTracker.getXPositionsArray().length; - for (int i = 0; i < size - 1; i++) { - - panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]); - panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]); - } - } - - /** - * This draws the pose history of the robot. - * - * @param poseTracker the PoseHistory to get the pose history from - */ - public static void drawPoseHistory(PoseHistory poseTracker) { - drawPoseHistory(poseTracker, historyLook); - } - - /** - * This tries to send the current packet to FTControl Panels. - */ - public static void sendPacket() { - panelsField.update(); + public static void drawPath(Canvas c, double[][] points) { + c.strokePolyline(points[0], points[1]); } } \ No newline at end of file From a8d28928e2055575d4d75dee451c52ac75fee161 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 14 Apr 2026 20:53:29 -0500 Subject: [PATCH 17/24] tuned drive pidf and adjusted center to 0 --- .../ftc/teamcode/pedroPathing/Constants.java | 4 +- .../ftc/teamcode/pedroPathing/Tuning.java | 56 +++++++++---------- 2 files changed, 31 insertions(+), 29 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index 2190bff..c1c18ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing; import com.acmerobotics.dashboard.config.Config; +import com.pedropathing.control.FilteredPIDFCoefficients; import com.pedropathing.control.PIDFCoefficients; import com.pedropathing.follower.Follower; import com.pedropathing.follower.FollowerConstants; @@ -20,7 +21,8 @@ public class Constants { .forwardZeroPowerAcceleration(-29.512) .lateralZeroPowerAcceleration(-72.872) .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)) - .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)); + .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03)); public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 0de897d..1de9ad4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -148,7 +148,7 @@ class LocalizationTest extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72,72)); + follower.setStartingPose(new Pose(0,0)); } /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ @@ -219,7 +219,7 @@ class ForwardTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72,72)); + follower.setStartingPose(new Pose(0,0)); follower.update(); drawCurrent(); } @@ -267,7 +267,7 @@ class LateralTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72,72)); + follower.setStartingPose(new Pose(0,0)); follower.update(); drawCurrent(); } @@ -315,7 +315,7 @@ class TurnTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72,72)); + follower.setStartingPose(new Pose(0,0)); follower.update(); drawCurrent(); } @@ -370,7 +370,7 @@ class ForwardVelocityTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ @@ -478,7 +478,7 @@ class LateralVelocityTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } /** @@ -582,7 +582,7 @@ class ForwardZeroPowerAccelerationTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } /** This initializes the drive motors as well as the Panels telemetryM. */ @@ -686,7 +686,7 @@ class LateralZeroPowerAccelerationTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } /** This initializes the drive motors as well as the Panels telemetry. */ @@ -961,7 +961,7 @@ class TranslationalTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } /** This initializes the Follower and creates the forward and backward Paths. */ @@ -979,9 +979,9 @@ class TranslationalTuner extends OpMode { public void start() { follower.deactivateAllPIDFs(); follower.activateTranslational(); - forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE,0))); forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards = new Path(new BezierLine(new Pose(DISTANCE,0), new Pose(0,0))); backwards.setConstantHeadingInterpolation(0); follower.followPath(forwards); } @@ -1030,7 +1030,7 @@ class HeadingTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } /** @@ -1051,9 +1051,9 @@ class HeadingTuner extends OpMode { public void start() { follower.deactivateAllPIDFs(); follower.activateHeading(); - forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE ,0))); forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards = new Path(new BezierLine(new Pose(DISTANCE ,0), new Pose(0,0))); backwards.setConstantHeadingInterpolation(0); follower.followPath(forwards); } @@ -1102,7 +1102,7 @@ class DriveTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } /** @@ -1126,13 +1126,13 @@ class DriveTuner extends OpMode { forwards = follower.pathBuilder() .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))) + .addPath(new BezierLine(new Pose(0,0), new Pose(DISTANCE ,0))) .setConstantHeadingInterpolation(0) .build(); backwards = follower.pathBuilder() .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))) + .addPath(new BezierLine(new Pose(DISTANCE ,0), new Pose(0,0))) .setConstantHeadingInterpolation(0) .build(); @@ -1184,7 +1184,7 @@ class Line extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } /** This initializes the Follower and creates the forward and backward Paths. */ @@ -1201,9 +1201,9 @@ class Line extends OpMode { @Override public void start() { follower.activateAllPIDFs(); - forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards = new Path(new BezierLine(new Pose(0,0), new Pose(DISTANCE ,0))); forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards = new Path(new BezierLine(new Pose(DISTANCE ,0), new Pose(0,0))); backwards.setConstantHeadingInterpolation(0); follower.followPath(forwards); } @@ -1251,7 +1251,7 @@ class CentripetalTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } /** @@ -1271,8 +1271,8 @@ class CentripetalTuner extends OpMode { @Override public void start() { follower.activateAllPIDFs(); - forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72))); - backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72))); + forwards = new Path(new BezierCurve(new Pose(0,0), new Pose(Math.abs(DISTANCE) ,0), new Pose(Math.abs(DISTANCE) ,DISTANCE ))); + backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) ,0), new Pose(0,0))); backwards.setTangentHeadingInterpolation(); backwards.reverseHeadingInterpolation(); @@ -1313,9 +1313,9 @@ class CentripetalTuner extends OpMode { */ class Triangle extends OpMode { - private final Pose startPose = new Pose(72, 72, Math.toRadians(0)); - private final Pose interPose = new Pose(24 + 72, -24 + 72, Math.toRadians(90)); - private final Pose endPose = new Pose(24 + 72, 24 + 72, Math.toRadians(45)); + private final Pose startPose = new Pose(0,0, Math.toRadians(0)); + private final Pose interPose = new Pose(24, -24, Math.toRadians(90)); + private final Pose endPose = new Pose(24, 24, Math.toRadians(45)); private PathChain triangle; @@ -1335,7 +1335,7 @@ class Triangle extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72, 72)); + follower.setStartingPose(new Pose(0,0)); } @Override @@ -1613,7 +1613,7 @@ class SwerveTurnTest extends OpMode { class OffsetsTuner extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72,72)); + follower.setStartingPose(new Pose(0,0)); follower.update(); drawCurrent(); } From 4cbb09e0884cb2d8b6855f6d7a0dfbad837b7bfc Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 14 Apr 2026 21:21:28 -0500 Subject: [PATCH 18/24] done tuning pedro pathing --- .../org/firstinspires/ftc/teamcode/pedroPathing/Constants.java | 3 ++- .../org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index c1c18ae..dd67303 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -22,7 +22,8 @@ public class Constants { .lateralZeroPowerAcceleration(-72.872) .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)) .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)) - .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03)); + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03)) + .centripetalScaling(0.0005); public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 1de9ad4..20f8497 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -114,7 +114,7 @@ public class Tuning extends SelectableOpMode { public static void drawCurrent() { try { - Drawing.drawRobot(follower.getPose(), "blue"); + Drawing.drawRobot(follower.getPose(), "green"); Drawing.sendPacket(); } catch (Exception e) { throw new RuntimeException("Drawing failed " + e); From 6b092bdaebec5b3fd9621ea88df5af57b3990f18 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 18 Apr 2026 00:03:07 -0500 Subject: [PATCH 19/24] Redid turret math to adjust to pedropathing field. --- .../autonomous/actions/AutoActions.java | 20 ++++++----- .../disabled/Auto_LT_Close_12Ball.java | 3 +- .../disabled/Auto_LT_Close_GateOpen.java | 3 +- .../ftc/teamcode/teleop/TeleopV3.java | 3 +- .../ftc/teamcode/tests/ShooterTest.java | 3 +- .../ftc/teamcode/tests/TurretTest.java | 34 ++++++++++++------- .../ftc/teamcode/utils/Targeting.java | 2 +- .../ftc/teamcode/utils/Turret.java | 28 ++++++++++----- 8 files changed, 60 insertions(+), 36 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 968545d..d696af1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -18,6 +18,8 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; +import com.pedropathing.geometry.Pose; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.hardware.NormalizedRGBA; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -525,7 +527,7 @@ public class AutoActions { drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); + Pose2d currentPose = null; //drive.localizer.getPose(); if (ticker == 0) { stamp = System.currentTimeMillis(); @@ -542,10 +544,10 @@ public class AutoActions { ticker++; - double robotX = currentPose.position.x; - double robotY = currentPose.position.y; + double robotX = 0.0;//currentPose.position.x; + double robotY = 0.0;//currentPose.position.y; - double robotHeading = currentPose.heading.toDouble(); + double robotHeading = 0.0;//currentPose.heading.toDouble(); double goalX = -15; double goalY = 0; @@ -554,11 +556,11 @@ public class AutoActions { double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose; + Pose deltaPose; if (posX != 0.501) { - deltaPose = new Pose2d(posX, posY, Math.toRadians(posH)); + deltaPose = new Pose(posX, posY, Math.toRadians(posH)); } else { - deltaPose = new Pose2d(dx, dy, robotHeading); + deltaPose = new Pose(dx, dy, robotHeading); } Turret.limelightUsed = true; @@ -646,9 +648,9 @@ public class AutoActions { double dx = robotX - goalX; // delta x from robot to goal double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose; + Pose deltaPose; if (turr == 0.501) { - deltaPose = new Pose2d(dx, dy, robotHeading); + deltaPose = new Pose(dx, dy, robotHeading); if (!detectingObelisk) { turret.trackGoal(deltaPose); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java index b4ca67a..9d0f8c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java @@ -72,6 +72,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -489,7 +490,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { 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); + Pose deltaPose = new Pose(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java index c32dc38..1ff9d28 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java @@ -77,6 +77,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -628,7 +629,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { 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); + Pose deltaPose = new Pose(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); 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 af73da3..8ab4d34 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 @@ -12,6 +12,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDFController; +import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -238,7 +239,7 @@ public class TeleopV3 extends LinearOpMode { 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); + Pose deltaPose = new Pose(dx, dy, robotHeading); // double distanceToGoal = Math.sqrt(dx * dx + dy * dy); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 12a3c8b..b058ea9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -15,6 +15,7 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -107,7 +108,7 @@ public class ShooterTest extends LinearOpMode { double dx = robX - goalX; // delta x from robot to goal double dy = robY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + Pose deltaPose = new Pose(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 34c34e3..0b3fcaf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -4,11 +4,14 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Turret; @@ -25,26 +28,31 @@ public class TurretTest extends LinearOpMode { ); Turret turret = new Turret(robot, TELE, robot.limelight); + + Follower follower; + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(72, 72, 0); + follower.setStartingPose(start); + follower.update(); waitForStart(); - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0)); + Turret.limelightUsed = false; while(opModeIsActive()){ + follower.update(); + turret.trackGoal(follower.getPose()); - drive.updatePoseEstimate(); - turret.trackGoal(drive.localizer.getPose()); +// TELE.addData("tpos", turret.getTurrPos()); +// TELE.addData("Limelight tx", turret.getBearing()); +// TELE.addData("Limelight ty", turret.getTy()); +// TELE.addData("Limelight X", turret.getLimelightX()); +// TELE.addData("Limelight Y", turret.getLimelightY()); - TELE.addData("tpos", turret.getTurrPos()); - TELE.addData("Limelight tx", turret.getBearing()); - TELE.addData("Limelight ty", turret.getTy()); - TELE.addData("Limelight X", turret.getLimelightX()); - TELE.addData("Limelight Y", turret.getLimelightY()); +// if(zeroTurr){ +// turret.zeroTurretEncoder(); +// } - if(zeroTurr){ - turret.zeroTurretEncoder(); - } - - TELE.update(); +// TELE.update(); } } 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 bec1745..c9edac2 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 @@ -84,7 +84,7 @@ public class Targeting { double cos54 = Math.cos(Math.toRadians(-54)); double sin54 = Math.sin(Math.toRadians(-54)); - + //TODO: change code so it uses pedropathing paths public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); int gridX; 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 96ddb29..ceb2432 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 @@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDController; +import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; @@ -243,17 +244,25 @@ public class Turret { } double targetTurretPos; - public void trackGoal(Pose2d deltaPos) { + public void trackGoal(Pose deltaPos) { /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ + double posX; + if (Color.redAlliance){ + posX = 144 - deltaPos.getX(); + } else { + posX = deltaPos.getX(); + } + double posY = 144 - deltaPos.getY(); + double posH = Math.toDegrees(deltaPos.getHeading()); + while (posH > 180) posH -= 360; + while (posH < -180) posH += 360; // Angle from robot to goal in robot frame - double desiredTurretAngleDeg = Math.toDegrees( - Math.atan2(deltaPos.position.y, deltaPos.position.x) - ); + double desiredTurretAngleDeg = Math.toDegrees(Math.atan2(posY, posX)) - 45; // Robot heading (field → robot) - double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble()); + double robotHeadingDeg = posH + 135; // Turret angle needed relative to robot double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; @@ -353,15 +362,16 @@ public class Turret { /* ---------------- TELEMETRY ---------------- */ -// TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); -// TELE.addData("Target Pos", "%.3f", targetTurretPos); -// TELE.addData("Current Pos", "%.3f", currentPos); -// TELE.addData("Commanded Pos", "%.3f", turretPos); + TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); + TELE.addData("Target Pos", "%.3f", targetTurretPos); + TELE.addData("Current Localization Pos", "%.3f", deltaPos); + TELE.addData("Commanded Pos", "%.3f", turretPos); // TELE.addData("LL Valid", result.isValid()); // TELE.addData("LL getTx", result.getTx()); // TELE.addData("LL Offset", offset); // TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET"); // TELE.addData("Learned Offset", "%.2f", offset); + TELE.update(); } } From 2a29e8181b8d36ab4ad758d381d39a41cd5013f5 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 18 Apr 2026 20:36:48 -0500 Subject: [PATCH 20/24] teleop is back up and running --- .../ftc/teamcode/constants/Front_Poses.java | 3 + .../teamcode/constants/ServoPositions.java | 18 ++-- .../ftc/teamcode/pedroPathing/Tuning.java | 2 +- .../ftc/teamcode/teleop/TeleopV3.java | 86 +++++++++++------- .../ftc/teamcode/tests/ShooterTest.java | 1 - .../ftc/teamcode/utils/Drivetrain.java | 36 ++++---- .../ftc/teamcode/utils/Targeting.java | 88 +++++++++++-------- .../ftc/teamcode/utils/Turret.java | 19 ++-- 8 files changed, 149 insertions(+), 104 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index d0dbe76..f75a111 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -65,4 +65,7 @@ public class Front_Poses { public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -50; public static Pose2d teleStart = new Pose2d(0, 0, 0); + + //For PedroPathing TODO: figure out how to change start poses in auto + public static double teleStartPoseX = 72, teleStartPoseY = 72, teleStartPoseH = 0; } 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 51b533a..8f54aca 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,21 +5,21 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.26; //0.13; + public static double spindexer_intakePos1 = 0.18; //0.13; - public static double spindexer_intakePos2 = 0.45; //0.33;//0.5; + public static double spindexer_intakePos2 = 0.37; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.63; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.55; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.91; //0.65; //0.24; - public static double spindexer_outtakeBall3b = 0.35; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.83; //0.65; //0.24; + public static double spindexer_outtakeBall3b = 0.27; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.73; //0.46; //0.6; - public static double spindexer_outtakeBall1 = 0.54; //0.27; //0.4; - public static double spinStartPos = 0.05; + 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 = 0; public static double spinEndPos = 0.6; - public static double shootAllSpindexerSpeedIncrease = 0.0095; + public static double shootAllSpindexerSpeedIncrease = 0.01; public static double transferServo_out = 0.15; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 20f8497..55cc2fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -148,7 +148,7 @@ class LocalizationTest extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(0,0)); + follower.setStartingPose(new Pose(72,72, 0)); } /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ 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 8ab4d34..937758b 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 @@ -2,6 +2,9 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; @@ -12,6 +15,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDFController; +import com.pedropathing.follower.Follower; import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -21,6 +25,7 @@ import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.constants.StateEnums; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utils.Drivetrain; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Light; @@ -38,10 +43,11 @@ import java.util.List; public class TeleopV3 extends LinearOpMode { public static double manualVel = 3000; public static double hoodDefaultPos = 0.5; - + private double predictedResetX, predictedResetY, predictedResetH; + public static double redPredictedResetX = 9, redPredictedResetY = 10.25, redPredictedResetH = 0; + public static double bluePredictedResetX = 135.0, bluePredictedResetY = 9, bluePredictedResetH = 180; public static double spinPow = 0.09; public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; - public static double spinSpeedIncrease = 0.03; public static int resetSpinTicks = 0; public static double hoodSpeedOffset = 0.01; public static double turretSpeedOffset = 0.01; @@ -56,12 +62,13 @@ public class TeleopV3 extends LinearOpMode { Light light; Servos servo; Flywheel flywheel; - MecanumDrive drive; +// MecanumDrive drive; Spindexer spindexer; Targeting targeting; Targeting.Settings targetingSettings; Drivetrain drivetrain; MeasuringLoopTimes loopTimes; + Follower follower; double autoHoodOffset = 0.0; int shooterTicker = 0; boolean intake = false; @@ -84,7 +91,6 @@ public class TeleopV3 extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); - robot.light.setPosition(0); List allHubs = hardwareMap.getAll(LynxModule.class); for (LynxModule hub : allHubs) { @@ -94,12 +100,15 @@ public class TeleopV3 extends LinearOpMode { TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); servo = new Servos(hardwareMap); flywheel = new Flywheel(hardwareMap); - drive = new MecanumDrive(hardwareMap, teleStart); +// drive = new MecanumDrive(hardwareMap, teleStart); + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); + follower.setStartingPose(start); spindexer = new Spindexer(hardwareMap); targeting = new Targeting(); targetingSettings = new Targeting.Settings(0.0, 0.0); - drivetrain = new Drivetrain(robot, drive); + drivetrain = new Drivetrain(robot, follower); loopTimes = new MeasuringLoopTimes(); loopTimes.init(); @@ -118,21 +127,33 @@ public class TeleopV3 extends LinearOpMode { Spindexer.teleop = true; while (opModeInInit()) { + //ONLY FOR TESTING: COMMENT OUT FOR COMPETITIONS + if (gamepad1.crossWasPressed()){ + redAlliance = !redAlliance; + } + robot.limelight.start(); if (redAlliance) { turret.pipelineSwitch(4); light.setManualLightColor(Color.LightRed); + predictedResetX = redPredictedResetX; + predictedResetY = redPredictedResetY; + predictedResetH = Math.toRadians(redPredictedResetH); } else { turret.pipelineSwitch(2); light.setManualLightColor(Color.LightBlue); - + predictedResetX = bluePredictedResetX; + predictedResetY = bluePredictedResetY; + predictedResetH = Math.toRadians(bluePredictedResetH); } - robot.light.setPosition(1); + limelightUsed = true; + + TELE.addData("Red Alliance?", redAlliance); + TELE.update(); light.update(); } - limelightUsed = true; waitForStart(); if (isStopRequested()) return; @@ -143,8 +164,8 @@ public class TeleopV3 extends LinearOpMode { while (opModeIsActive()) { //TELE.addData("Is limelight on?", robot.limelight.getStatus()); - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); + follower.update(); + Pose currentPose = follower.getPose(); if (enableSpindexerManager) { //if (!shootAll) { @@ -226,20 +247,21 @@ public class TeleopV3 extends LinearOpMode { //TURRET TRACKING - double robX = currentPose.position.x; - double robY = currentPose.position.y; - double robH = currentPose.heading.toDouble(); + double robX = currentPose.getX(); + double robY = currentPose.getY(); + double robH = currentPose.getHeading(); double robotX = robX + xOffset; double robotY = robY + yOffset; double robotHeading = robH + hOffset; - 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 - Pose deltaPose = new Pose(dx, dy, robotHeading); +// 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 +// Pose deltaPose = new Pose(dx, dy, robotHeading); + Pose deltaPose = new Pose(robotX, robotY, robotHeading); // double distanceToGoal = Math.sqrt(dx * dx + dy * dy); @@ -253,14 +275,15 @@ public class TeleopV3 extends LinearOpMode { gamepad2.rumble(500); } - if (relocalize){ - turret.relocalize(); - xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; - yOffset = (turret.getLimelightX() * 39.3701) - robY; - hOffset = (Math.toRadians(turret.getLimelightH())) - robH; - } else { + //TODO: relocalize using limelight +// if (relocalize){ +// turret.relocalize(); +// xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; +// yOffset = (turret.getLimelightX() * 39.3701) - robY; +// hOffset = (Math.toRadians(turret.getLimelightH())) - robH; +// } else { turret.trackGoal(deltaPose); - } + //} //VELOCITY AUTOMATIC if (autoVel) { @@ -324,7 +347,8 @@ public class TeleopV3 extends LinearOpMode { } if (gamepad2.crossWasPressed()) { - drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); +// drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + follower.setPose(new Pose(predictedResetX, predictedResetY, predictedResetH)); gamepad2.rumble(200); sleep(500); } @@ -381,9 +405,9 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); - TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); - TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); - TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); +// TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); +// TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); +// TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index b058ea9..9c4f5bf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -8,7 +8,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; 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.teleop.TeleopV3.spinSpeedIncrease; import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import com.acmerobotics.dashboard.FtcDashboard; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java index edd598a..cb5fc64 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java @@ -1,33 +1,31 @@ package org.firstinspires.ftc.teamcode.utils; -import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.ProfileAccelConstraint; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Gamepad; - -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; public class Drivetrain { - Robot robot; + Robot robot; boolean autoDrive = false; - Pose2d brakePos = new Pose2d(0, 0, 0); + Pose brakePos = new Pose(0, 0, 0); - MecanumDrive drive; +// MecanumDrive drive; + Follower follower; private final TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); - public Drivetrain (Robot rob, MecanumDrive mecanumDrive){ + public Drivetrain (Robot rob, Follower follower){ this.robot = rob; - this.drive = mecanumDrive; + this.follower = follower; } @@ -71,7 +69,7 @@ public class Drivetrain { robot.frontRight.setPower(frontRightPower); robot.backRight.setPower(backRightPower); } - Pose2d currentPos = drive.localizer.getPose(); + Pose currentPos = follower.getPose(); brakePos = currentPos; if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) { @@ -84,13 +82,13 @@ public class Drivetrain { } else if (brake > 0.4) { - TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos) - .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT); + PathChain traj2 = follower.pathBuilder() + .addPath(new BezierLine(currentPos, brakePos)) + .setLinearHeadingInterpolation(currentPos.getHeading(), brakePos.getHeading()) + .build(); - if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) { - Actions.runBlocking( - traj2.build() - ); + if (Math.abs(currentPos.getX() - brakePos.getX()) > 1 || Math.abs(currentPos.getY() - brakePos.getY()) > 1) { + follower.followPath(traj2); } } else { autoDrive = false; 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 c9edac2..83521ed 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 @@ -3,7 +3,9 @@ package org.firstinspires.ftc.teamcode.utils; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import java.lang.Math; +import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; public class Targeting { @@ -71,7 +73,7 @@ public class Targeting { public final int TILE_UPPER_QUARTILE = 18; public final int TILE_LOWER_QUARTILE = 6; public double robotInchesX, robotInchesY = 0.0; - public int robotGridX, robotGridY = 0; + public int robotGridX = 0, robotGridY = 0; MultipleTelemetry TELE; double cancelOffsetX = 0.0; // was -40.0 double cancelOffsetY = 0.0; // was 7.0 @@ -89,45 +91,59 @@ public class Targeting { Settings recommendedSettings = new Settings(0.0, 0.0); int gridX; int gridY; - if (!redAlliance){ - sin54 = Math.sin(Math.toRadians(54)); - double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; - double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; + int remX = 0; + int remY = 0; + // Old code +// if (!redAlliance){ +// sin54 = Math.sin(Math.toRadians(54)); +// double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; +// double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; +// +// // Convert robot coordinates to inches +// robotInchesX = rotatedX * unitConversionFactor + 20; +// robotInchesY = rotatedY * unitConversionFactor + 20; +// +// // Find approximate location in the grid +// gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize)); +// gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); +// } else { +// sin54 = Math.sin(Math.toRadians(-54)); +// double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; +// double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; +// +// // Convert robot coordinates to inches +// robotInchesX = rotatedX * unitConversionFactor; +// robotInchesY = rotatedY * unitConversionFactor; +// +// // Find approximate location in the grid +// gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); +// gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); +// } +// +// + remX = Math.floorMod((int) robotX, tileSize); + remY = Math.floorMod((int) robotY, tileSize); +// +// //clamp +// +// if (redAlliance) { +// robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); +// robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); +// } else { +// robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); +// robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); +// } - // Convert robot coordinates to inches - robotInchesX = rotatedX * unitConversionFactor + 20; - robotInchesY = rotatedY * unitConversionFactor + 20; - - // Find approximate location in the grid - gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize)); - gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); + // New code + if (redAlliance){ + gridY = Math.round((float) (((144-robotX)-12) / 24)); } else { - sin54 = Math.sin(Math.toRadians(-54)); - double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; - double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; - - // Convert robot coordinates to inches - robotInchesX = rotatedX * unitConversionFactor; - robotInchesY = rotatedY * unitConversionFactor; - - // Find approximate location in the grid - gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); - gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); + gridY = Math.round((float) ((robotX-12) / 24)); } + gridX = Math.round((float) (((144-robotY)-12) / 24)); - - int remX = Math.floorMod((int) robotInchesX, tileSize); - int remY = Math.floorMod((int) robotInchesY, tileSize); - - //clamp - - if (redAlliance) { - robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); - robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); - } else { - robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); - robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); - } + robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); + robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); // Determine if we need to interpolate based on tile position. // if near upper or lower quarter or tile interpolate with next tile. 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 ceb2432..48969da 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 @@ -25,7 +25,7 @@ public class Turret { public static double turretTolerance = 0.02; public static double turrPosScalar = 0.00011264432; - public static double turret180Range = 0.54; + public static double turret180Range = 0.55; public static double turrDefault = 0.35; public static double turrMin = 0; public static double turrMax = 0.69; @@ -262,7 +262,12 @@ public class Turret { double desiredTurretAngleDeg = Math.toDegrees(Math.atan2(posY, posX)) - 45; // Robot heading (field → robot) - double robotHeadingDeg = posH + 135; + double robotHeadingDeg; + if (Color.redAlliance){ + robotHeadingDeg = posH + 135; + } else { + robotHeadingDeg = posH + 45; + } // Turret angle needed relative to robot double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; @@ -362,16 +367,16 @@ public class Turret { /* ---------------- TELEMETRY ---------------- */ - TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); - TELE.addData("Target Pos", "%.3f", targetTurretPos); - TELE.addData("Current Localization Pos", "%.3f", deltaPos); - TELE.addData("Commanded Pos", "%.3f", turretPos); +// TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); +// TELE.addData("Target Pos", "%.3f", targetTurretPos); +// TELE.addData("Current Localization Pos", deltaPos); +// TELE.addData("Commanded Pos", "%.3f", turretPos); // TELE.addData("LL Valid", result.isValid()); // TELE.addData("LL getTx", result.getTx()); // TELE.addData("LL Offset", offset); // TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET"); // TELE.addData("Learned Offset", "%.2f", offset); - TELE.update(); +// TELE.update(); } } From 7eebd42ea274f075233a7e21c8821af8d219df28 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 18 Apr 2026 21:34:08 -0500 Subject: [PATCH 21/24] limelight relocalization in progress --- .../ftc/teamcode/teleop/TeleopV3.java | 19 +++++++------ .../ftc/teamcode/utils/Turret.java | 28 ++++++------------- 2 files changed, 20 insertions(+), 27 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 937758b..c7b175e 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 @@ -41,6 +41,7 @@ import java.util.List; @Config @TeleOp public class TeleopV3 extends LinearOpMode { + private double metersToInches = 39.3700787402; public static double manualVel = 3000; public static double hoodDefaultPos = 0.5; private double predictedResetX, predictedResetY, predictedResetH; @@ -275,15 +276,17 @@ public class TeleopV3 extends LinearOpMode { gamepad2.rumble(500); } - //TODO: relocalize using limelight -// if (relocalize){ -// turret.relocalize(); -// xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; -// yOffset = (turret.getLimelightX() * 39.3701) - robY; -// hOffset = (Math.toRadians(turret.getLimelightH())) - robH; -// } else { + if (relocalize){ + turret.relocalize(); + xOffset = ((turret.getLimelightY()*metersToInches)+72) - robX; + yOffset = (72-(turret.getLimelightX()*metersToInches)) - robY; + hOffset = (Math.toRadians(turret.getLimelightH() + 90)); + while (hOffset > 180) {hOffset-=360;} + while (hOffset < -180) {hOffset+=360;} + hOffset = hOffset - robH; + } else { turret.trackGoal(deltaPose); - //} + } //VELOCITY AUTOMATIC if (autoVel) { 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 48969da..e8837b9 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 @@ -109,7 +109,7 @@ public class Turret { private void limelightRead() { // only for tracking purposes, not general reads Double xPos = null; Double yPos = null; - Double zPos = null; + double zPos; Double hPos = null; result = webcam.getLatestResult(); if (result != null) { @@ -117,27 +117,18 @@ public class Turret { tx = result.getTx(); ty = result.getTy(); if (TeleopV3.relocalize){ - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - limelightTagPose = fiducial.getRobotPoseTargetSpace(); - if (limelightTagPose != null){ - xPos = limelightTagPose.getPosition().x; - yPos = limelightTagPose.getPosition().y; - zPos = limelightTagPose.getPosition().z; - hPos = limelightTagPose.getOrientation().getYaw(); - } + zPos = result.getBotpose().getPosition().z; + if (zPos < 0.15){ + xPos = result.getBotpose().getPosition().x; + yPos = result.getBotpose().getPosition().y; + hPos = result.getBotpose().getOrientation().getYaw(); + limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX); + limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY); + limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH); } } } } - if (xPos != null){ - if (zPos<0) { - limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX); - limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY); - limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ); - limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH); - } - } } public double getBearing() { @@ -150,7 +141,6 @@ public class Turret { return ty; } - Pose3D limelightTagPose; double limelightTagX = 0.0; double limelightTagY = 0.0; double limelightTagZ = 0.0; From 81e0e80f620788d40792e0b49a8ea241cdb73b80 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 21 Apr 2026 21:22:39 -0500 Subject: [PATCH 22/24] auto in progress --- .../autonomous/Auto12BallPedroPathing.java | 187 ++++++++++++++++++ .../ftc/teamcode/utils/Turret.java | 6 +- 2 files changed, 190 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java new file mode 100644 index 0000000..80d7042 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java @@ -0,0 +1,187 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; +import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +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 com.pedropathing.util.Timer; + +import java.util.List; + +@Config +@Autonomous (preselectTeleOp = "TeleopV3") +public class Auto12BallPedroPathing extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + Flywheel flywheel; + Targeting targeting; + Targeting.Settings targetingSettings; + Follower follower; + Turret turret; + Spindexer spindexer; + Servos servos; + Timer pathTimer, opModeTimer; + + // Initialize path state machine + enum PathState { + DRIVE_SHOOT0, + WAIT_SHOOT0, + DRIVE_PICKUP1, + PICKUP1, + DRIVE_SHOOT1, + WAIT_SHOOT1, + DRIVE_PICKUP2, + PICKUP2, + DRIVE_SHOOT2, + WAIT_SHOOT2, + DRIVE_PICKUP3, + PICKUP3, + DRIVE_SHOOT3, + WAIT_SHOOT3 + } + PathState pathState = PathState.DRIVE_SHOOT0; + + // Poses + public static double startPoseX = 110, startPoseY = 130, startPoseH = -90; + public static double shoot0X = 88, shoot0Y = 82, shoot0H = -45; + Pose startPose; + Pose shoot0; + + //Building Paths + PathChain startPose_shoot0; + void buildPaths(){ + startPose_shoot0 = follower.pathBuilder() + .addPath(new BezierLine(startPose, shoot0)) + .setLinearHeadingInterpolation(startPose.getHeading(), shoot0.getHeading()) + .build(); + } + + //Path State Machine + void pathStateMachine(){ + switch(pathState){ + case DRIVE_SHOOT0: + follower.followPath(startPose_shoot0); + pathState = PathState.WAIT_SHOOT0; + break; + case WAIT_SHOOT0: + boolean shootStart = false; + if (!follower.isBusy() && !shootStart){ + spindexer.prepareShootAllContinous(); + shootStart = true; + } + if (shootStart && spindexer.shootAllComplete()){ + //pathState = PathState.DRIVE_PICKUP1; + TELE.addLine("End Auto"); + TELE.update(); + } + break; + default: + break; + } + } + + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + List allHubs = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : allHubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + flywheel = new Flywheel(hardwareMap); + targeting = new Targeting(); + targetingSettings = new Targeting.Settings(0,0); + follower = Constants.createFollower(hardwareMap); + turret = new Turret(robot, TELE, robot.limelight); + spindexer = new Spindexer(hardwareMap); + opModeTimer = new Timer(); + pathTimer = new Timer(); + hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU(); + + robot.light.setPosition(Color.LightRed); + + int allianceTicks = 0; + while (opModeInInit()){ + boolean initalizeRobot = false; + + if (gamepad1.crossWasPressed() && !initalizeRobot){ + allianceTicks++; + Color.redAlliance = !Color.redAlliance; + if (Color.redAlliance){ + robot.light.setPosition(Color.LightRed); + } else { + robot.light.setPosition(Color.LightBlue); + } + startPoseX = 144 - startPoseX; + startPoseH = 180 - startPoseH; + shoot0X = 144 - shoot0X; + shoot0H = 180 - shoot0H; + + while (startPoseH > 180) {startPoseH-=360;} + while (startPoseH < -180) {startPoseH+=360;} + + while (shoot0H > 180) {shoot0H-=360;} + while (shoot0H < -180) {shoot0H+=360;} + } + + if (gamepad1.triangleWasPressed()){ + initalizeRobot = true; + } + + if (initalizeRobot){ + follower.setStartingPose(startPose); + buildPaths(); + sleep(2000); + turret.setTurret(turrDefault); + servos.setSpinPos(spinStartPos); + } + + TELE.addData("Red Alliance?", Color.redAlliance); + TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initalizeRobot); + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()){ + follower.update(); + pathStateMachine(); + Pose currentPose = follower.getPose(); + teleStartPoseX = currentPose.getX(); + teleStartPoseY = currentPose.getY(); + teleStartPoseH = Math.toDegrees(currentPose.getHeading()); + turret.trackGoal(currentPose); + targetingSettings = targeting.calculateSettings(currentPose.getX(), currentPose.getY(), currentPose.getHeading(), 0, turretInterpolate); + + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); + flywheel.manageFlywheel(targetingSettings.flywheelRPM); + servos.setHoodPos(targetingSettings.hoodAngle); + } + } +} 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 e8837b9..d76ab5e 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 @@ -239,11 +239,11 @@ public class Turret { /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ double posX; if (Color.redAlliance){ - posX = 144 - deltaPos.getX(); + posX = 134 - deltaPos.getX(); } else { - posX = deltaPos.getX(); + posX = deltaPos.getX() - 10; } - double posY = 144 - deltaPos.getY(); + double posY = 140 - deltaPos.getY(); double posH = Math.toDegrees(deltaPos.getHeading()); while (posH > 180) posH -= 360; while (posH < -180) posH += 360; From 222b20156140bea06f1a55897ad46f9b6043e13b Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 25 Apr 2026 22:15:26 -0500 Subject: [PATCH 23/24] fixed hood offset --- .../firstinspires/ftc/teamcode/constants/ServoPositions.java | 2 +- .../java/org/firstinspires/ftc/teamcode/utils/Targeting.java | 5 ++--- 2 files changed, 3 insertions(+), 4 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 8f54aca..24ad1be 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 @@ -27,7 +27,7 @@ public class ServoPositions { public static double hoodAuto = 0.27; - public static double hoodOffset = -0.04; // offset from 0.93 + public static double hoodOffset = -0.05; // offset from 0.93 (or position at 0,0 in targeting class) public static double turret_redClose = 0; public static double turret_blueClose = 0; 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 83521ed..95ce968 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.utils; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import java.lang.Math; @@ -84,8 +85,6 @@ public class Targeting { public Targeting() { } - double cos54 = Math.cos(Math.toRadians(-54)); - double sin54 = Math.sin(Math.toRadians(-54)); //TODO: change code so it uses pedropathing paths public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); @@ -220,7 +219,7 @@ public class Targeting { if (true) { //!interpolate) { if ((robotGridY < 6) && (robotGridX < 6)) { recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; - recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; + recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + hoodOffset; } return recommendedSettings; } else { From 3f2d54065f145df287e1a4b1d6d2c96c42873c68 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 25 Apr 2026 22:18:54 -0500 Subject: [PATCH 24/24] autoPathing complete --- .../autonomous/Auto12BallPedroPathing.java | 342 +++++++++++++++--- .../ftc/teamcode/pedroPathing/Constants.java | 4 +- 2 files changed, 300 insertions(+), 46 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java index 80d7042..8d10658 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; -import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; +import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; @@ -11,10 +11,10 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; import com.pedropathing.geometry.BezierLine; import com.pedropathing.geometry.Pose; import com.pedropathing.paths.PathChain; -import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -43,10 +43,16 @@ public class Auto12BallPedroPathing extends LinearOpMode { Turret turret; Spindexer spindexer; Servos servos; - Timer pathTimer, opModeTimer; + Timer opModeTimer, shootingTimer; + + // Wait Times + public static double shootTime = 2; + + // Extra Variables + public static double intakePower = 0.3; // Initialize path state machine - enum PathState { + private enum PathState { DRIVE_SHOOT0, WAIT_SHOOT0, DRIVE_PICKUP1, @@ -65,42 +71,240 @@ public class Auto12BallPedroPathing extends LinearOpMode { PathState pathState = PathState.DRIVE_SHOOT0; // Poses - public static double startPoseX = 110, startPoseY = 130, startPoseH = -90; - public static double shoot0X = 88, shoot0Y = 82, shoot0H = -45; + public static double startPoseX = 112, startPoseY = 132.5, startPoseH = -90; + public static double shoot0X = 106, shoot0Y = 106, shoot0H = -40; + public static double drivePickup1X = 102, drivePickup1Y = 82, drivePickup1H = 0; + public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; + public static double shoot1X = 86, shoot1Y = 82, shoot1H = -80; + public static double drivePickup2ControlX = 91.69828844730904, drivePickup2ControlY = 66.724457099909; + public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; + public static double pickup2X = 132, pickup2Y = 57, pickup2H = 0; + public static double shoot2ControlX = 86, shoot2ControlY = 57; + public static double shoot2X = 86, shoot2Y = 82, shoot2H = -90; + public static double drivePickup3ControlX = 97.97800291788306, drivePickup3ControlY = 50.10765863138859; + public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; + public static double pickup3X = 132, pickup3Y = 34.5, pickup3H = 0; + public static double shoot3ControlX = 86, shoot3ControlY = 34.5; + public static double shoot3X = 84, shoot3Y = 102, shoot3H = -90; Pose startPose; Pose shoot0; + Pose drivePickup1; + Pose pickup1; + Pose shoot1; + Pose drivePickup2Control; + Pose drivePickup2; + Pose pickup2; + Pose shoot2Control; + Pose shoot2; + Pose drivePickup3Control; + Pose drivePickup3; + Pose pickup3; + Pose shoot3Control; + Pose shoot3; + private void initializePoses(){ + startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); + shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); + drivePickup1 = new Pose(drivePickup1X, drivePickup1Y, Math.toRadians(drivePickup1H)); + pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); + shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); + drivePickup2Control = new Pose(drivePickup2ControlX, drivePickup2ControlY); + drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H)); + pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H)); + shoot2Control = new Pose(shoot2ControlX, shoot2ControlY); + shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H)); + drivePickup3Control = new Pose(drivePickup3ControlX, drivePickup3ControlY); + drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H)); + pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H)); + shoot3Control = new Pose(shoot3ControlX, shoot3ControlY); + shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H)); + } // add poses to void //Building Paths PathChain startPose_shoot0; - void buildPaths(){ + PathChain shoot0_drivePickup1; + PathChain drivePickup1_pickup1; + PathChain pickup1_shoot1; + PathChain shoot1_drivePickup2; + PathChain drivePickup2_pickup2; + PathChain pickup2_shoot2; + PathChain shoot2_drivePickup3; + PathChain drivePickup3_pickup3; + PathChain pickup3_shoot3; + private void buildPaths(){ startPose_shoot0 = follower.pathBuilder() .addPath(new BezierLine(startPose, shoot0)) .setLinearHeadingInterpolation(startPose.getHeading(), shoot0.getHeading()) .build(); + + shoot0_drivePickup1 = follower.pathBuilder() + .addPath(new BezierLine(shoot0, drivePickup1)) + .setLinearHeadingInterpolation(shoot0.getHeading(), drivePickup1.getHeading()) + .build(); + + drivePickup1_pickup1 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup1, pickup1)) + .setTangentHeadingInterpolation() + .build(); + + pickup1_shoot1 = follower.pathBuilder() + .addPath(new BezierLine(pickup1, shoot1)) + .setLinearHeadingInterpolation(pickup1.getHeading(), shoot1.getHeading()) + .build(); + + shoot1_drivePickup2 = follower.pathBuilder() + .addPath(new BezierCurve(shoot1, drivePickup2Control, drivePickup2)) + .setLinearHeadingInterpolation(shoot1.getHeading(), drivePickup2.getHeading()) + .build(); + + drivePickup2_pickup2 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup2, pickup2)) + .setConstantHeadingInterpolation(pickup2.getHeading()) + .build(); + + pickup2_shoot2 = follower.pathBuilder() + .addPath(new BezierCurve(pickup2, shoot2Control, shoot2)) + .setLinearHeadingInterpolation(pickup2.getHeading(), shoot2.getHeading()) + .build(); + + shoot2_drivePickup3 = follower.pathBuilder() + .addPath(new BezierCurve(shoot2, drivePickup3Control, drivePickup3)) + .setLinearHeadingInterpolation(shoot2.getHeading(), drivePickup3.getHeading()) + .build(); + + drivePickup3_pickup3 = follower.pathBuilder() + .addPath(new BezierLine(drivePickup3, pickup3)) + .setTangentHeadingInterpolation() + .build(); + + pickup3_shoot3 = follower.pathBuilder() + .addPath(new BezierCurve(pickup3, shoot3Control, shoot3)) + .setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading()) + .build(); } //Path State Machine - void pathStateMachine(){ + private boolean startAuto = true; + private double timeStamp = 0; + private void pathStateMachine(){ + double currentTime = (double) System.currentTimeMillis() / 1000; switch(pathState){ case DRIVE_SHOOT0: - follower.followPath(startPose_shoot0); - pathState = PathState.WAIT_SHOOT0; + if (startAuto){ + follower.followPath(startPose_shoot0, true); + startAuto = false; + } + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT0; + timeStamp = currentTime; +// spindexer.prepareShootAllContinous(); + } break; case WAIT_SHOOT0: - boolean shootStart = false; - if (!follower.isBusy() && !shootStart){ - spindexer.prepareShootAllContinous(); - shootStart = true; + if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ +// spindexer.resetSpindexer(); + pathState = PathState.DRIVE_PICKUP1; + follower.followPath(shoot0_drivePickup1, true); } - if (shootStart && spindexer.shootAllComplete()){ - //pathState = PathState.DRIVE_PICKUP1; - TELE.addLine("End Auto"); + break; + case DRIVE_PICKUP1: + if (!follower.isBusy()){ + pathState = PathState.PICKUP1; + follower.followPath(drivePickup1_pickup1, intakePower, false); + } + break; + case PICKUP1: + if (!follower.isBusy()){ + pathState = PathState.DRIVE_SHOOT1; + follower.followPath(pickup1_shoot1, true); + } + break; + case DRIVE_SHOOT1: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT1; + timeStamp = currentTime; +// spindexer.prepareShootAllContinous(); + } + break; + case WAIT_SHOOT1: + if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ +// spindexer.resetSpindexer(); + pathState = PathState.DRIVE_PICKUP2; + follower.followPath(shoot1_drivePickup2, true); + } + break; + case DRIVE_PICKUP2: + if (!follower.isBusy()){ + pathState = PathState.PICKUP2; + follower.followPath(drivePickup2_pickup2, intakePower, false); + } + break; + case PICKUP2: + if (!follower.isBusy()){ + pathState = PathState.DRIVE_SHOOT2; + follower.followPath(pickup2_shoot2, true); + } + break; + case DRIVE_SHOOT2: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT2; + timeStamp = currentTime; +// spindexer.prepareShootAllContinous(); + } + break; + case WAIT_SHOOT2: + if (spindexer.shootAllComplete() || currentTime - timeStamp > shootTime){ +// spindexer.resetSpindexer(); + pathState = PathState.DRIVE_PICKUP3; + follower.followPath(shoot2_drivePickup3, true); + } + break; + case DRIVE_PICKUP3: + if (!follower.isBusy()){ + pathState = PathState.PICKUP3; + follower.followPath(drivePickup3_pickup3, intakePower, false); + } + break; + case PICKUP3: + if (!follower.isBusy()){ + pathState = PathState.DRIVE_SHOOT3; + follower.followPath(pickup3_shoot3, true); + } + break; + case DRIVE_SHOOT3: + if (!follower.isBusy()){ + pathState = PathState.WAIT_SHOOT3; +// spindexer.prepareShootAllContinous(); + } + break; + case WAIT_SHOOT3: + if (spindexer.shootAllComplete()){ +// spindexer.resetSpindexer(); + TELE.addLine("Done Auto"); TELE.update(); } break; default: break; } + TELE.update(); + } + + private boolean driveToShoot(){ + return pathState == PathState.DRIVE_SHOOT0 || + pathState == PathState.DRIVE_SHOOT1 || + pathState == PathState.DRIVE_SHOOT2 || + pathState == PathState.DRIVE_SHOOT3; + } + + private double adjustXPoseBasedOnAlliance(double pose){ + return (144-pose); + } + + private double adjustHeadingBasedOnAlliance(double heading){ + heading = 180 - heading; + while (heading > 180) {heading-=360;} + while (heading <= -180) {heading+=360;} + return heading; } @Override @@ -115,52 +319,86 @@ public class Auto12BallPedroPathing extends LinearOpMode { targeting = new Targeting(); targetingSettings = new Targeting.Settings(0,0); follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(new Pose(72,72,0)); turret = new Turret(robot, TELE, robot.limelight); spindexer = new Spindexer(hardwareMap); + servos = new Servos(hardwareMap); opModeTimer = new Timer(); - pathTimer = new Timer(); - hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU(); + shootingTimer = new Timer(); robot.light.setPosition(Color.LightRed); - int allianceTicks = 0; + boolean initializeRobot = false; while (opModeInInit()){ - boolean initalizeRobot = false; + follower.update(); - if (gamepad1.crossWasPressed() && !initalizeRobot){ - allianceTicks++; + if (gamepad1.crossWasPressed() && !initializeRobot){ Color.redAlliance = !Color.redAlliance; if (Color.redAlliance){ robot.light.setPosition(Color.LightRed); } else { robot.light.setPosition(Color.LightBlue); } - startPoseX = 144 - startPoseX; - startPoseH = 180 - startPoseH; - shoot0X = 144 - shoot0X; - shoot0H = 180 - shoot0H; - while (startPoseH > 180) {startPoseH-=360;} - while (startPoseH < -180) {startPoseH+=360;} + startPoseX = adjustXPoseBasedOnAlliance(startPoseX); + startPoseH = adjustHeadingBasedOnAlliance(startPoseH); - while (shoot0H > 180) {shoot0H-=360;} - while (shoot0H < -180) {shoot0H+=360;} + shoot0X = adjustXPoseBasedOnAlliance(shoot0X); + shoot0H = adjustHeadingBasedOnAlliance(shoot0H); + + drivePickup1X = adjustXPoseBasedOnAlliance(drivePickup1X); + drivePickup1H = adjustHeadingBasedOnAlliance(drivePickup1H); + + pickup1X = adjustXPoseBasedOnAlliance(pickup1X); + pickup1H = adjustHeadingBasedOnAlliance(pickup1H); + + shoot1X = adjustXPoseBasedOnAlliance(shoot1X); + shoot1H = adjustHeadingBasedOnAlliance(shoot1H); + + drivePickup2ControlX = adjustXPoseBasedOnAlliance(drivePickup2ControlX); + + drivePickup2X = adjustXPoseBasedOnAlliance(drivePickup2X); + drivePickup2H = adjustHeadingBasedOnAlliance(drivePickup2H); + + pickup2X = adjustXPoseBasedOnAlliance(pickup2X); + pickup2H = adjustHeadingBasedOnAlliance(pickup2H); + + shoot2ControlX = adjustXPoseBasedOnAlliance(shoot2ControlX); + + shoot2X = adjustXPoseBasedOnAlliance(shoot2X); + shoot2H = adjustHeadingBasedOnAlliance(shoot2H); + + drivePickup3ControlX = adjustXPoseBasedOnAlliance(drivePickup3ControlX); + + drivePickup3X = adjustXPoseBasedOnAlliance(drivePickup3X); + drivePickup3H = adjustHeadingBasedOnAlliance(drivePickup3H); + + pickup3X = adjustXPoseBasedOnAlliance(pickup3X); + pickup3H = adjustHeadingBasedOnAlliance(pickup3H); + + shoot3ControlX = adjustXPoseBasedOnAlliance(shoot3ControlX); + + shoot3X = adjustXPoseBasedOnAlliance(shoot3X); + shoot3H = adjustHeadingBasedOnAlliance(shoot3H); } if (gamepad1.triangleWasPressed()){ - initalizeRobot = true; + initializeRobot = true; } - if (initalizeRobot){ - follower.setStartingPose(startPose); + if (initializeRobot){ + initializePoses(); + follower.setPose(startPose); buildPaths(); sleep(2000); + turret.setTurret(turrDefault); servos.setSpinPos(spinStartPos); } TELE.addData("Red Alliance?", Color.redAlliance); - TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initalizeRobot); + TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); + TELE.addData("Start Pose", follower.getPose()); TELE.update(); } @@ -168,20 +406,36 @@ public class Auto12BallPedroPathing extends LinearOpMode { if (isStopRequested()) return; + robot.transfer.setPower(1); + limelightUsed = false; + opModeTimer.resetTimer(); + while (opModeIsActive()){ follower.update(); pathStateMachine(); Pose currentPose = follower.getPose(); - teleStartPoseX = currentPose.getX(); - teleStartPoseY = currentPose.getY(); - teleStartPoseH = Math.toDegrees(currentPose.getHeading()); - turret.trackGoal(currentPose); - targetingSettings = targeting.calculateSettings(currentPose.getX(), currentPose.getY(), currentPose.getHeading(), 0, turretInterpolate); +// teleStartPoseX = currentPose.getX(); +// teleStartPoseY = currentPose.getY(); +// teleStartPoseH = Math.toDegrees(currentPose.getHeading()); - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - servos.setHoodPos(targetingSettings.hoodAngle); +// turret.trackGoal(currentPose); +// targetingSettings = targeting.calculateSettings(currentPose.getX(), currentPose.getY(), currentPose.getHeading(), 0, turretInterpolate); +// +// double voltage = robot.voltage.getVoltage(); +// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); +// flywheel.manageFlywheel(targetingSettings.flywheelRPM); +// servos.setHoodPos(targetingSettings.hoodAngle); +// +// if (driveToShoot()){ +// servos.setSpinPos(spinStartPos); +// } else { +// spindexer.processIntake(); +// } + +// TELE.addData("X:", currentPose.getX()); +// TELE.addData("Y:", currentPose.getY()); +// TELE.addData("H:", currentPose.getHeading()); +// TELE.update(); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index dd67303..1c931de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -21,7 +21,7 @@ public class Constants { .forwardZeroPowerAcceleration(-29.512) .lateralZeroPowerAcceleration(-72.872) .translationalPIDFCoefficients(new PIDFCoefficients(0.35, 0, 0.03, 0.012)) - .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.02, 0.02)) + .headingPIDFCoefficients(new PIDFCoefficients(1, 0, 0.025, 0.02)) .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.03, 0, 0, 0.6, 0.03)) .centripetalScaling(0.0005); @@ -38,7 +38,7 @@ public class Constants { .xVelocity(64.675) .yVelocity(49.583); - public static double breakingStrength = 1.25; + public static double breakingStrength = 1; public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, breakingStrength, 1); public static PinpointConstants localizerConstants = new PinpointConstants()