From 7d700ef02b0e1396d0fd762c4c255355a1fc419e Mon Sep 17 00:00:00 2001 From: p-rint <159627419+p-rint@users.noreply.github.com> Date: Mon, 12 Jan 2026 15:38:49 -0500 Subject: [PATCH 1/6] Readded my Main Auto Please stop deleting my files :( --- .../firstinspires/ftc/teamcode/MainAuto.java | 217 ++++++++++++++++++ 1 file changed, 217 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java new file mode 100644 index 000000000000..5a2293aa1d21 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java @@ -0,0 +1,217 @@ +package org.firstinspires.ftc.teamcode; // make sure this aligns with class location + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.subsystems.Intake; + +@Autonomous(name = "Main Auto") +public class MainAuto extends OpMode { + + private Follower follower; + private Timer pathTimer, actionTimer, opmodeTimer; + + private int pathState; + + + + /* + private final Pose poseA = new Pose(48, 12, Math.toRadians(180)); // Start Pose of our robot. + private final Pose poseB = new Pose(18, 84, Math.toRadians(135)); + private final Pose poseC = new Pose(74, 77, Math.toRadians(50)); + private final Pose poseD = new Pose(21, 60, Math.toRadians(50)); + private final Pose poseE = new Pose(73, 80, Math.toRadians(50)); + private final Pose poseF = new Pose(18, 36, Math.toRadians(50)); + private final Pose poseG = new Pose(75, 81, Math.toRadians(50)); + + private Path startPosition; + private PathChain moveB, moveC, moveD, moveE, moveF; + */ + + private final Pose startPose = new Pose(48, 12, Math.toRadians(90)); // Start Pose of our robot. + private final Pose pickUp1 = new Pose(18, 84, Math.toRadians(180)); + + private final Pose pickUp1C1 = new Pose(57, 90); + private final Pose pickUp1C2 = new Pose(57, 84); + + private final Pose pickUp2 = new Pose(18, 60, Math.toRadians(180)); + + private final Pose pickUp2C1 = new Pose(75, 57); + private final Pose pickUp2C2 = new Pose(66, 60); + + private final Pose pickUp3 = new Pose(18, 36, Math.toRadians(180)); + + private final Pose pickUp3C1 = new Pose(72, 30); + private final Pose pickUp3C2 = new Pose(72, 36); + + private final Pose shootPose = new Pose(75, 87, Math.toRadians(330)); + + + private Path startPosition; + private PathChain shoot1, intake2, shoot2, intake3, shoot3, goToStart; + public void buildPaths() { + //startPosition = new Path(new BezierLine(startPose, pickUp1)); + startPosition = new Path(new BezierCurve(startPose, pickUp1C1, pickUp1C2, pickUp1)); + startPosition.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)); + + + shoot1 = follower.pathBuilder() + .addPath(new BezierLine(pickUp1, shootPose)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330)) + .build(); + + intake2 = follower.pathBuilder() + //.addPath(new BezierLine(shootPose, pickUp2)) + .addPath(new BezierCurve(shootPose, pickUp2C1, pickUp2C2 ,pickUp2)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + .build(); + + shoot2 = follower.pathBuilder() + .addPath(new BezierLine(pickUp2, shootPose)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330)) + .build(); + + intake3 = follower.pathBuilder() + //.addPath(new BezierLine(shootPose, pickUp3)) + .addPath(new BezierCurve(shootPose, pickUp3C1, pickUp3C2 ,pickUp3)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + .build(); + + shoot3 = follower.pathBuilder() + .addPath(new BezierLine(pickUp3, shootPose)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330)) + .build(); + + goToStart = follower.pathBuilder() + .addPath(new BezierLine(shootPose, startPose)) + .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(90)) + .build(); + + + + } + + + public void autonomousPathUpdate() { + switch (pathState) { + case 0: + follower.followPath(startPosition); + setPathState(1); + break; + case 1: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(shoot1, 0.7, true); + setPathState(2); + } + break; + case 2: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(intake2, .7,true); + setPathState(3); + } + break; + case 3: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(shoot2, .7,true); + setPathState(4); + } + break; + case 4: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(intake3, .7,true); + setPathState(5); + } + break; + case 5: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(shoot3, .7,true); + setPathState(6); + + } + break; + case 6: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(goToStart, .7,true); + setPathState(-1); + + } + } + } + /** These change the states of the paths and actions. It will also reset the timers of the individual switches **/ + public void setPathState(int pState) { + pathState = pState; + pathTimer.resetTimer(); + } + + + @Override + public void loop() { + // These loop the movements of the robot, these must be called continuously in order to work + follower.update(); + autonomousPathUpdate(); + // Feedback to Driver Hub for debugging + telemetry.addData("path state", pathState); + telemetry.addData("x", follower.getPose().getX()); + telemetry.addData("y", follower.getPose().getY()); + telemetry.addData("heading", follower.getPose().getHeading()); + telemetry.update(); + } + + + + @Override + public void init() { + pathTimer = new Timer(); + opmodeTimer = new Timer(); + opmodeTimer.resetTimer(); + follower = Constants.createFollower(hardwareMap); + buildPaths(); + follower.setStartingPose(startPose); + } + + /** This method is called continuously after Init while waiting for "play". **/ + @Override + public void init_loop() {} + + + /** This method is called once at the start of the OpMode. + * It runs all the setup actions, including building paths and starting the path system **/ + @Override + public void start() { + opmodeTimer.resetTimer(); + setPathState(0); + } + /** We do not use this because everything should automatically disable **/ + @Override + public void stop() {} + + + + +} From c17b9e5ffcf02696e6480e32af63dd9ccaafb040 Mon Sep 17 00:00:00 2001 From: p-rint <159627419+p-rint@users.noreply.github.com> Date: Mon, 12 Jan 2026 15:45:32 -0500 Subject: [PATCH 2/6] test Just added a comment to my file. --- .../src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java index 5a2293aa1d21..704f4520036d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java @@ -1,5 +1,5 @@ package org.firstinspires.ftc.teamcode; // make sure this aligns with class location - +//HIIII pls don't delete import com.pedropathing.follower.Follower; import com.pedropathing.geometry.BezierCurve; import com.pedropathing.geometry.BezierLine; From 803980768fa0e76d9cba06ec60215f78a5f1d4f3 Mon Sep 17 00:00:00 2001 From: p-rint <159627419+p-rint@users.noreply.github.com> Date: Wed, 14 Jan 2026 14:59:41 -0500 Subject: [PATCH 3/6] Cloned mainTeleOp just c & paste --- .../ftc/teamcode/SecondaryTeleOp.java | 198 ++++++++++++++++++ 1 file changed, 198 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java new file mode 100644 index 000000000000..f762c755da2f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java @@ -0,0 +1,198 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.subsystems.Drive; +import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.Outtake; +import org.firstinspires.ftc.teamcode.subsystems.Robot; +import org.firstinspires.ftc.teamcode.subsystems.Storage; +import org.firstinspires.ftc.teamcode.subsystems.Transitions; +import org.firstinspires.ftc.teamcode.utils.Logger; + +import dev.nextftc.core.commands.CommandManager; +import dev.nextftc.core.components.BindingsComponent; +import dev.nextftc.core.components.SubsystemComponent; +import dev.nextftc.ftc.ActiveOpMode; +import dev.nextftc.ftc.GamepadEx; +import dev.nextftc.ftc.Gamepads; +import dev.nextftc.ftc.NextFTCOpMode; +import dev.nextftc.ftc.components.BulkReadComponent; + +@TeleOp(name="SecondTeleOp", group="TeleOp") +public class SecondaryTeleOp extends NextFTCOpMode { + { + addComponents( + BulkReadComponent.INSTANCE, // TODO: make actual MANUAL mode bulkreading (we don't need to also read the expansion hub every loop) + BindingsComponent.INSTANCE, + CommandManager.INSTANCE, + new SubsystemComponent( + Storage.INSTANCE, + Robot.INSTANCE, + Drive.INSTANCE, + Intake.INSTANCE, + Outtake.INSTANCE, + Transitions.INSTANCE + ) + //new PedroComponent(Constants::createFollower) + ); + } + + @Override public void onInit() { + } + @Override public void onWaitForStart() { + ActiveOpMode.telemetry().update(); + } + @Override public void onStartButtonPressed() { + + GamepadEx caimo = Gamepads.gamepad1(); + GamepadEx jeff = Gamepads.gamepad2(); + + // Debug stuff + jeff.back() + .whenBecomesTrue(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.resetEncoderAtOuttakeCommand().schedule(); + }); + + jeff.start() + .whenBecomesTrue(() -> { + Storage.setPIDMode(false).schedule(); + }); + +// // Storage nonsense +// jeff.leftStickX().greaterThan(0.1).and(jeff.leftStickButton() +// .whenTrue(() -> { +// Storage.setManualModeCommand(true).schedule(); +// Storage.setManualPowerCommand(jeff.leftStickX().get() / 1.5).schedule(); +// }) +// .whenFalse(() -> { +// Storage.setManualModeCommand(true).schedule(); +// Storage.setManualPowerCommand(jeff.leftStickX().get() / 4).schedule(); +// })); +// +// jeff.leftStickX().lessThan(-0.1).and(jeff.leftStickButton() +// .whenTrue(() -> { +// Storage.setManualModeCommand(true).schedule(); +// Storage.setManualPowerCommand(jeff.leftStickX().get() / 1.5).schedule(); +// }) +// .whenFalse(() -> { +// Storage.setManualModeCommand(true).schedule(); +// Storage.setManualPowerCommand(jeff.leftStickX().get() / 4).schedule(); +// })); +// +// jeff.leftStickX() +// .inRange(-0.05, 0.05) +// .whenBecomesTrue(() -> { +// Storage.setManualModeCommand(true).schedule(); +// Storage.setManualPowerCommand(0).schedule(); +// }); + + // Run Outake + jeff.rightBumper() + .whenBecomesTrue(() -> { + Outtake.setRunDownCommand(false).schedule(); + Outtake.on.schedule(); + }) + .whenBecomesFalse(() -> { + Outtake.setRunDownCommand(true).schedule(); + }); + + jeff.leftBumper() + .whenBecomesTrue(() -> { + Outtake.setRunDownCommand(true).schedule(); + Outtake.setOuttakePowerCommand(1).schedule(); + }) + .whenBecomesFalse(() -> { + Outtake.setRunDownCommand(true).schedule(); + Outtake.setOuttakePowerCommand(0).schedule(); + }); + + jeff.x() + .whenBecomesTrue(() -> Transitions.setOuttakePositionCommand(Transitions.UP_POS).schedule()) + .whenBecomesFalse(() -> Transitions.setOuttakePositionCommand(Transitions.DOWN_POS).schedule()); + + + // Drive Stuff + caimo.rightBumper() + .whenBecomesTrue(() -> Drive.setSlowModeCommand(true).schedule()) + .whenBecomesFalse(() -> Drive.setSlowModeCommand(false).schedule()); + + caimo.back() + .toggleOnBecomesTrue() + .whenBecomesTrue(() -> Drive.setHeadingLock(true)) + .whenBecomesFalse(() -> Drive.setHeadingLock(false)); + + caimo.back().and(caimo.start()) + .whenBecomesTrue(() -> Drive.resetDriveCommand().schedule()); + + caimo.rightTrigger().greaterThan(0.5) + .whenBecomesTrue(() -> Intake.setIntakePowerCommand(1).schedule()) + .whenBecomesFalse(() -> Intake.setIntakePowerCommand(0).schedule()); + caimo.leftTrigger().greaterThan(0.5) + .whenBecomesTrue(() -> Intake.setIntakePowerCommand(-1).schedule()) + .whenBecomesFalse(() -> Intake.setIntakePowerCommand(0).schedule()); + + jeff.dpadDown() + .whenBecomesTrue(() -> Storage.spinToNextIntakeIndex().schedule()); + jeff.dpadUp() + .whenBecomesTrue(() -> Storage.spinToNextOuttakeIndex().schedule()); + caimo.dpadDown() + .whenBecomesTrue(() -> Storage.spinToNextIntakeIndex().schedule()); + caimo.dpadUp() + .whenBecomesTrue(() -> Storage.spinToNextOuttakeIndex().schedule()); + + caimo.dpadLeft() + .whenBecomesTrue(() -> Robot.intakeAll.schedule()); + caimo.dpadRight() + .whenBecomesTrue(() -> Robot.outtakeAll.schedule()); + jeff.dpadLeft() + .whenBecomesTrue(() -> Robot.intakeAll.schedule()); + jeff.dpadRight() + .whenBecomesTrue(() -> Robot.outtakeAll.schedule()); + + jeff.a() + .whenBecomesTrue(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0.075).schedule(); + }) + .whenBecomesFalse(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0).schedule(); + }); + jeff.b() + .whenBecomesTrue(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0.5).schedule(); + }) + .whenBecomesFalse(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0).schedule(); + }); + jeff.y() + .whenBecomesTrue(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0.2).schedule(); + }) + .whenBecomesFalse(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0).schedule(); + }); +// jeff.rightBumper() +// .whenBecomesTrue(() -> Outtake.setOuttakePowerCommand(1).schedule()) +// .whenBecomesFalse(() -> Outtake.setOuttakePowerCommand(0).schedule()); +// jeff.leftBumper() +// .whenBecomesTrue(() -> Outtake.setOuttakePowerCommand(0.8).schedule()) +// .whenBecomesFalse(() -> Outtake.setOuttakePowerCommand(0).schedule()); + + } + @Override public void onUpdate() { + for (String cname : CommandManager.INSTANCE.snapshot()) { + Logger.add("Commands", Logger.Level.DEBUG, cname); + } + Logger.update(Logger.Level.DEBUG); + } + + @Override public void onStop() { + } +} From 0dd3c693d854be94aa219431449d3001c57fe40f Mon Sep 17 00:00:00 2001 From: p-rint <159627419+p-rint@users.noreply.github.com> Date: Wed, 14 Jan 2026 15:02:56 -0500 Subject: [PATCH 4/6] d d --- README.md | 2 +- TeamCode/build.gradle | 11 +- .../ftc/teamcode/ANewJeffreyAuto.java | 94 -------- .../firstinspires/ftc/teamcode/KennyAuto.java | 216 ++++++++++++++++++ .../firstinspires/ftc/teamcode/MainAuto.java | 93 ++++++++ .../ftc/teamcode/MainTeleOp.java | 1 - .../ftc/teamcode/subsystems/Drive.java | 16 +- .../ftc/teamcode/subsystems/Robot.java | 7 + .../ftc/teamcode/subsystems/Storage.java | 127 +++++----- .../ftc/teamcode/subsystems/Transitions.java | 6 +- build.dependencies.gradle | 5 - 11 files changed, 397 insertions(+), 181 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ANewJeffreyAuto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/KennyAuto.java diff --git a/README.md b/README.md index 522fb8048e75..0746d8933a7b 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ## NOTICE -This repository contains the public FTC SDK for the DECODE (2025-2026) competition season. +This repository contains the public FTC SDK for the DECODE (2025-2026) competition season! ## Welcome! This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 934b40131b21..61a7c132f120 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -46,16 +46,15 @@ repositories { dependencies { implementation project(':FtcRobotController') - implementation 'dev.nextftc:ftc:1.1.0-beta.1' + implementation 'dev.nextftc:ftc:1.1.0-beta.2' implementation 'dev.nextftc:control:1.0.0' implementation 'dev.nextftc:bindings:1.0.1' - implementation 'dev.nextftc:hardware:1.1.0-beta.1' + implementation 'dev.nextftc:hardware:1.1.0-beta.2' implementation 'dev.nextftc.extensions:pedro:1.0.0' + implementation 'com.pedropathing:ftc:2.0.5' + implementation 'com.pedropathing:telemetry:1.0.0' - /* - the dependency below breaks Panels features (starting + stopping opmodes, configurables) - the telemetry, field, graph will all still work (allegedly, hopefully, not sure) - */ + implementation("com.bylazar.sloth:fullpanels:0.2.4+1.0.12") implementation("dev.frozenmilk.sinister:Sloth:0.2.4") } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ANewJeffreyAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ANewJeffreyAuto.java deleted file mode 100644 index d7b6675070d7..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ANewJeffreyAuto.java +++ /dev/null @@ -1,94 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import static dev.nextftc.extensions.pedro.PedroComponent.follower; - -import com.pedropathing.geometry.BezierLine; -import com.pedropathing.geometry.Pose; -import com.pedropathing.paths.Path; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import org.firstinspires.ftc.teamcode.pedroPathing.Constants; -import org.firstinspires.ftc.teamcode.subsystems.Drive; -import org.firstinspires.ftc.teamcode.subsystems.Intake; -import org.firstinspires.ftc.teamcode.subsystems.Outtake; -import org.firstinspires.ftc.teamcode.subsystems.Robot; -import org.firstinspires.ftc.teamcode.subsystems.Storage; -import org.firstinspires.ftc.teamcode.subsystems.Transitions; - -import dev.nextftc.core.commands.Command; -import dev.nextftc.core.commands.CommandManager; -import dev.nextftc.core.commands.delays.Delay; -import dev.nextftc.core.commands.groups.SequentialGroup; -import dev.nextftc.core.components.BindingsComponent; -import dev.nextftc.core.components.SubsystemComponent; -import dev.nextftc.extensions.pedro.FollowPath; -import dev.nextftc.extensions.pedro.PedroComponent; -import dev.nextftc.ftc.NextFTCOpMode; -import dev.nextftc.ftc.components.BulkReadComponent; - - -@Autonomous -public class ANewJeffreyAuto extends NextFTCOpMode { - - public static Pose endPose; - - - public ANewJeffreyAuto() { - addComponents( - BulkReadComponent.INSTANCE, // TODO: make actual MANUAL mode bulkreading (we don't need to also read the expansion hub every loop) - BindingsComponent.INSTANCE, - CommandManager.INSTANCE, - new SubsystemComponent( - Storage.INSTANCE, - Robot.INSTANCE, - Drive.INSTANCE, - Intake.INSTANCE, - Outtake.INSTANCE, - Transitions.INSTANCE - ), - new PedroComponent(Constants::createFollower) - - ); - } - - - public static final Pose startPoseBlue = new Pose(56, 8, Math.toRadians(270)); // Start Pose of our robot. - public static final Pose scorePoseBlue = new Pose(68, 76, Math.toRadians(315)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. - - public static final Pose startPoseRed = new Pose(87, 8, Math.toRadians(270)); // Start Pose of our robot. - public static final Pose scorePoseRed = new Pose(76, 76, Math.toRadians(225)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. - - - private final Pose startPose = startPoseBlue; - public static final Pose scorePose = scorePoseBlue; - public static final Pose scorePosebutActually = new Pose(73, 70, Math.toRadians(135)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. - - - Path scorePreload = new Path(new BezierLine(startPose, scorePose)); - - - private Command autonomousRoutine() { - scorePreload.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading()); - return new SequentialGroup( - new FollowPath(scorePreload), - new Delay(2), - Robot.INSTANCE.outtakeAll - ); - } - - @Override - public void onStartButtonPressed() { - follower().setStartingPose(startPose); - autonomousRoutine().schedule(); - follower().breakFollowing(); - } - - public void onUpdate(){ - Drive.telemetryM.update(); - follower().update(); - } - - public void onStop(){ - endPose = follower().getPose(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/KennyAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/KennyAuto.java new file mode 100644 index 000000000000..995982cd4f51 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/KennyAuto.java @@ -0,0 +1,216 @@ +package org.firstinspires.ftc.teamcode; // make sure this aligns with class location + +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.util.Timer; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + +@Autonomous(name = "KennyAuto") +public class KennyAuto extends OpMode { + + private Follower follower; + private Timer pathTimer, actionTimer, opmodeTimer; + + private int pathState; + + + + /* + private final Pose poseA = new Pose(48, 12, Math.toRadians(180)); // Start Pose of our robot. + private final Pose poseB = new Pose(18, 84, Math.toRadians(135)); + private final Pose poseC = new Pose(74, 77, Math.toRadians(50)); + private final Pose poseD = new Pose(21, 60, Math.toRadians(50)); + private final Pose poseE = new Pose(73, 80, Math.toRadians(50)); + private final Pose poseF = new Pose(18, 36, Math.toRadians(50)); + private final Pose poseG = new Pose(75, 81, Math.toRadians(50)); + + private Path startPosition; + private PathChain moveB, moveC, moveD, moveE, moveF; + */ + + private final Pose startPose = new Pose(48, 12, Math.toRadians(90)); // Start Pose of our robot. + private final Pose pickUp1 = new Pose(18, 84, Math.toRadians(180)); + + private final Pose pickUp1C1 = new Pose(57, 90); + private final Pose pickUp1C2 = new Pose(57, 84); + + private final Pose pickUp2 = new Pose(18, 60, Math.toRadians(180)); + + private final Pose pickUp2C1 = new Pose(75, 57); + private final Pose pickUp2C2 = new Pose(66, 60); + + private final Pose pickUp3 = new Pose(18, 36, Math.toRadians(180)); + + private final Pose pickUp3C1 = new Pose(72, 30); + private final Pose pickUp3C2 = new Pose(72, 36); + + private final Pose shootPose = new Pose(75, 87, Math.toRadians(330)); + + + private Path startPosition; + private PathChain shoot1, intake2, shoot2, intake3, shoot3, goToStart; + public void buildPaths() { + //startPosition = new Path(new BezierLine(startPose, pickUp1)); + startPosition = new Path(new BezierCurve(startPose, pickUp1C1, pickUp1C2, pickUp1)); + startPosition.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)); + + + shoot1 = follower.pathBuilder() + .addPath(new BezierLine(pickUp1, shootPose)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330)) + .build(); + + intake2 = follower.pathBuilder() + //.addPath(new BezierLine(shootPose, pickUp2)) + .addPath(new BezierCurve(shootPose, pickUp2C1, pickUp2C2 ,pickUp2)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + .build(); + + shoot2 = follower.pathBuilder() + .addPath(new BezierLine(pickUp2, shootPose)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330)) + .build(); + + intake3 = follower.pathBuilder() + //.addPath(new BezierLine(shootPose, pickUp3)) + .addPath(new BezierCurve(shootPose, pickUp3C1, pickUp3C2 ,pickUp3)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + .build(); + + shoot3 = follower.pathBuilder() + .addPath(new BezierLine(pickUp3, shootPose)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330)) + .build(); + + goToStart = follower.pathBuilder() + .addPath(new BezierLine(shootPose, startPose)) + .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(90)) + .build(); + + + + } + + + public void autonomousPathUpdate() { + switch (pathState) { + case 0: + follower.followPath(startPosition); + setPathState(1); + break; + case 1: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(shoot1, 0.7, true); + setPathState(2); + } + break; + case 2: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(intake2, .7,true); + setPathState(3); + } + break; + case 3: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(shoot2, .7,true); + setPathState(4); + } + break; + case 4: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(intake3, .7,true); + setPathState(5); + } + break; + case 5: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(shoot3, .7,true); + setPathState(6); + + } + break; + case 6: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(goToStart, .7,true); + setPathState(-1); + + } + } + } + /** These change the states of the paths and actions. It will also reset the timers of the individual switches **/ + public void setPathState(int pState) { + pathState = pState; + pathTimer.resetTimer(); + } + + + @Override + public void loop() { + // These loop the movements of the robot, these must be called continuously in order to work + follower.update(); + autonomousPathUpdate(); + // Feedback to Driver Hub for debugging + telemetry.addData("path state", pathState); + telemetry.addData("x", follower.getPose().getX()); + telemetry.addData("y", follower.getPose().getY()); + telemetry.addData("heading", follower.getPose().getHeading()); + telemetry.update(); + } + + + + @Override + public void init() { + pathTimer = new Timer(); + opmodeTimer = new Timer(); + opmodeTimer.resetTimer(); + follower = Constants.createFollower(hardwareMap); + buildPaths(); + follower.setStartingPose(startPose); + } + + /** This method is called continuously after Init while waiting for "play". **/ + @Override + public void init_loop() {} + + + /** This method is called once at the start of the OpMode. + * It runs all the setup actions, including building paths and starting the path system **/ + @Override + public void start() { + opmodeTimer.resetTimer(); + setPathState(0); + } + /** We do not use this because everything should automatically disable **/ + @Override + public void stop() {} + + + + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java index 704f4520036d..a522cdb19ca2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java @@ -1,3 +1,4 @@ +<<<<<<< HEAD package org.firstinspires.ftc.teamcode; // make sure this aligns with class location //HIIII pls don't delete import com.pedropathing.follower.Follower; @@ -215,3 +216,95 @@ public void stop() {} } +======= +package org.firstinspires.ftc.teamcode; + +import static dev.nextftc.extensions.pedro.PedroComponent.follower; + +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.Path; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.subsystems.Drive; +import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.Outtake; +import org.firstinspires.ftc.teamcode.subsystems.Robot; +import org.firstinspires.ftc.teamcode.subsystems.Storage; +import org.firstinspires.ftc.teamcode.subsystems.Transitions; + +import dev.nextftc.core.commands.Command; +import dev.nextftc.core.commands.CommandManager; +import dev.nextftc.core.commands.delays.Delay; +import dev.nextftc.core.commands.groups.SequentialGroup; +import dev.nextftc.core.components.BindingsComponent; +import dev.nextftc.core.components.SubsystemComponent; +import dev.nextftc.extensions.pedro.FollowPath; +import dev.nextftc.extensions.pedro.PedroComponent; +import dev.nextftc.ftc.NextFTCOpMode; +import dev.nextftc.ftc.components.BulkReadComponent; + +@Autonomous +public class MainAuto extends NextFTCOpMode { + { + addComponents( + BulkReadComponent.INSTANCE, // TODO: make actual MANUAL mode bulkreading (we don't need to also read the expansion hub every loop) + BindingsComponent.INSTANCE, + CommandManager.INSTANCE, + new SubsystemComponent( + Storage.INSTANCE, + Robot.INSTANCE, + Drive.INSTANCE, + Intake.INSTANCE, + Outtake.INSTANCE, + Transitions.INSTANCE + ) + ); + } + + public static Pose endPose; + + + + public static final Pose startPoseBlue = new Pose(56, 8, Math.toRadians(270)); // Start Pose of our robot. + public static final Pose scorePoseBlue = new Pose(68, 76, Math.toRadians(315)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. + + public static final Pose startPoseRed = new Pose(87, 8, Math.toRadians(270)); // Start Pose of our robot. + public static final Pose scorePoseRed = new Pose(76, 76, Math.toRadians(225)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. + + + private final Pose startPose = startPoseBlue; + public static final Pose scorePose = scorePoseBlue; + public static final Pose scorePosebutActually = new Pose(73, 70, Math.toRadians(135)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. + + + Path scorePreload = new Path(new BezierLine(startPose, scorePose)); + + + private Command autonomousRoutine() { + scorePreload.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading()); + return new SequentialGroup( + new FollowPath(scorePreload), + new Delay(2), + Robot.outtakeAll + ); + } + + @Override + public void onStartButtonPressed() { + follower().setStartingPose(startPose); + autonomousRoutine().schedule(); + follower().breakFollowing(); + } + + public void onUpdate(){ + Drive.telemetryM.update(); + follower().update(); + } + + public void onStop(){ + endPose = follower().getPose(); + } +} +>>>>>>> 5e83da2d1652b30a4b5bb2345d0985dcb419e579 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainTeleOp.java index 244ee599cda1..ee24ba24cec7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainTeleOp.java @@ -37,7 +37,6 @@ public class MainTeleOp extends NextFTCOpMode { Outtake.INSTANCE, Transitions.INSTANCE ) - //new PedroComponent(Constants::createFollower) ); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drive.java index 9d270d4665ca..fee4ddc9bf95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drive.java @@ -25,16 +25,12 @@ public class Drive implements Subsystem { private static final double slowModeMultiplier = 0.2; private static final boolean robotCentric = false; private static Pose startingPose = new Pose(24, 24, Math.toRadians(0)); - private static Pose shootTarget = new Pose(6, 144 - 6, 0); - public static Robot.Alliance currentAlliance = Robot.Alliance.RED; - private static double headingGoal; // Radians + private static Pose shootTarget = new Pose(6, 144 - 6, 0);private static double headingGoal; // Radians private static PIDFController controller; private static boolean headingLock = false; - public static Robot.Alliance getCurrentAlliance(){ - return currentAlliance; - } + @Override public void initialize() { follower = Constants.createFollower(ActiveOpMode.hardwareMap()); @@ -62,9 +58,9 @@ private static double getHeadingError() { } private static void setShootTarget() { - if (currentAlliance == Robot.Alliance.BLUE && shootTarget.getX() != 6) + if (Robot.getCurrentAlliance() == Robot.Alliance.BLUE && shootTarget.getX() != 6) shootTarget = new Pose(6, 144 - 6, 0); - else if (currentAlliance == Robot.Alliance.RED && shootTarget.getX() != (144 - 6)) + else if (Robot.getCurrentAlliance() == Robot.Alliance.RED && shootTarget.getX() != (144 - 6)) shootTarget = shootTarget.mirror(); } @@ -77,7 +73,7 @@ public static Command setSlowModeCommand(boolean newMode) { } public static void resetDrive() { - if (currentAlliance.equals(Robot.Alliance.BLUE)) { + if (Robot.getCurrentAlliance().equals(Robot.Alliance.BLUE)) { follower.setPose(new Pose(8, 6.25, Math.toRadians(0)).mirror()); } else { follower.setPose(new Pose(8, 6.25, Math.toRadians(0))); @@ -85,7 +81,7 @@ public static void resetDrive() { } public static Command resetDriveCommand() { - return new InstantCommand(() -> resetDrive()); + return new InstantCommand(Drive::resetDrive); } public static Command drive = new LambdaCommand() .setStart(() -> follower.startTeleopDrive()) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java index 9df3b8264810..76ea7777b88d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java @@ -7,6 +7,13 @@ public class Robot extends SubsystemGroup { public static final Robot INSTANCE = new Robot(); + public static Robot.Alliance getCurrentAlliance(){ + return currentAlliance; + } + + public static Robot.Alliance currentAlliance = Robot.Alliance.RED; + + public enum Alliance { RED, BLUE diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Storage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Storage.java index 0559671f3a40..35ebaa982863 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Storage.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Storage.java @@ -1,5 +1,9 @@ package org.firstinspires.ftc.teamcode.subsystems; +import com.qualcomm.robotcore.hardware.DigitalChannel; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; + import org.firstinspires.ftc.teamcode.utils.Logger; import dev.nextftc.control.ControlSystem; @@ -8,6 +12,7 @@ import dev.nextftc.core.commands.utility.InstantCommand; import dev.nextftc.core.commands.utility.LambdaCommand; import dev.nextftc.core.subsystems.Subsystem; +import dev.nextftc.ftc.ActiveOpMode; import dev.nextftc.hardware.impl.MotorEx; public class Storage implements Subsystem { @@ -17,18 +22,19 @@ public class Storage implements Subsystem { private static double manualPower = 0; private final static MotorEx spin = new MotorEx("motorExp0").brakeMode(); - //private static DigitalChannel limitSwitch; - //private static NormalizedColorSensor colorSensor; - //private static int index = 0; + private static DigitalChannel limitSwitch; + private static NormalizedColorSensor colorSensor; private static double startPos = 0; private static final double TICKS = 185; + private static boolean lastState = false; + public static boolean getManualMode(){ return manualMode; } public static ControlSystem controller = ControlSystem.builder() - .posPid(0.02, 0, 0) + .posPid(0.0075, 0, 0) .build(); public static final State[] STATES = { @@ -50,45 +56,40 @@ public void initialize() { controller.setGoal(new KineticState(0)); -// limitSwitch = ActiveOpMode.hardwareMap().get(DigitalChannel.class, -// "limitSwitch"); -// limitSwitch.setMode(DigitalChannel.Mode.INPUT); + limitSwitch = ActiveOpMode.hardwareMap().get(DigitalChannel.class, + "limitSwitch"); + limitSwitch.setMode(DigitalChannel.Mode.INPUT); + + colorSensor = ActiveOpMode.hardwareMap().get(NormalizedColorSensor.class, + "colorSensor"); - // colorSensor = ActiveOpMode.hardwareMap().get(NormalizedColorSensor.class, - // "sensor_color"); - // - // ((SwitchableLight) colorSensor).enableLight(false); } @Override public void periodic() { +// if (wasJustPressed()) { +// resetEncoderAtOuttake(); +// } + if (manualMode) { spin.setPower(manualPower); } else if (pidControlMode){ - double testPower = controller.calculate(spin.getState()); - if (Math.abs(testPower) > 0.05) { - spin.setPower(testPower); - } else { - spin.setPower(0); - } +// double testPower = controller.calculate(spin.getState()); +// if (Math.abs(testPower) > 0.05) { +// spin.setPower(testPower); +// } else { +// spin.setPower(0); +// } } // Write Telemetry Logger.add("Storage", Logger.Level.DEBUG, "ticks: " + spin.getCurrentPosition()); Logger.add("Storage", Logger.Level.DEBUG, "pid?" + pidControlMode + "power: " + controller.calculate(spin.getState())); Logger.add("Storage", Logger.Level.DEBUG, "manual?" + manualMode + "power: " + manualPower); - //Logger.add("Storage", Logger.Level.DEBUG, "limit switch" + limitSwitch.getState()); - // Track Indexes - // if (wasJustPressed()) { - // index++; - // if (index >= STATES.length) { - // index = 0; - // } - // } - // ActiveOpMode.telemetry().addLine("Limit: " + limitSwitch.getState()); - // ActiveOpMode.telemetry().addLine("Color: " + getColor().red + ", " + - // getColor().green + ", " + getColor().blue ); + Logger.add("Storage", Logger.Level.DEBUG, "limit switch" + limitSwitch.getState()); + Logger.add("Storage", Logger.Level.DEBUG, "test"); + Logger.add("Storage", Logger.Level.DEBUG, "Color: " + getColor().red + ", " + getColor().green + ", " + getColor().blue); } public static Command setManualPowerCommand(double newPower) { @@ -132,6 +133,8 @@ public static Command spinToNextIntakeIndex() { public static Command spinToNextOuttakeIndex() { return new LambdaCommand() .setStart(() -> { + manualMode = true; + manualPower = 0.1; manualMode = false; pidControlMode = true; startPos = spin.getCurrentPosition() - 10; @@ -145,7 +148,8 @@ public static Command spinToNextOuttakeIndex() { ticksToMove -= TICKS; } double nextPos = startPos + ticksToMove; - controller.setGoal(new KineticState(nextPos));}) + controller.setGoal(new KineticState(nextPos)); + }) .setIsDone(() -> true) .setStop(interrupted -> { }) @@ -213,38 +217,39 @@ public static Command resetEncoderAtOuttakeCommand() { -// public static NormalizedRGBA getColor() { -// return colorSensor.getNormalizedColors(); -// } -// -// public static State readColor() { -// NormalizedRGBA colors = getColor(); -// -// double red = colors.red; -// double green = colors.green; -// double blue = colors.blue; -// -// double sum = red + green + blue; -// -// double r = red / sum; -// double g = green / sum; -// double b = blue / sum; -// -// if (r > 0.35 && b > 0.35 && g < 0.25) { -// return State.PURPLE; -// } -// if (g > 0.45 && r < 0.3 && b < 0.3) { -// return State.GREEN; -// } -// return State.NONE; -// } -// -// public boolean wasJustPressed() { -// boolean current = limitSwitch.getState(); -// boolean triggered = current && !lastState; -// lastState = current; -// return triggered; -// } + public static NormalizedRGBA getColor() { + return colorSensor.getNormalizedColors(); + } + + public static State readColor() { + NormalizedRGBA colors = getColor(); + + double red = colors.red; + double green = colors.green; + double blue = colors.blue; + + double sum = red + green + blue; + + double r = red / sum; + double g = green / sum; + double b = blue / sum; + + if (r > 0.35 && b > 0.35 && g < 0.25) { + return State.PURPLE; + } + if (g > 0.45 && r < 0.3 && b < 0.3) { + return State.GREEN; + } + return State.NONE; + } + + public static boolean wasJustPressed() { + boolean currentState = limitSwitch.getState(); + boolean justPressed = currentState && !lastState; + lastState = currentState; + return justPressed; + } + // // public static Command prioritySpin(int targetIndex) { // return new LambdaCommand() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transitions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transitions.java index dd8a0a4e8872..95598b2a1cd3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transitions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transitions.java @@ -12,14 +12,14 @@ public class Transitions implements Subsystem { public static final Transitions INSTANCE = new Transitions(); - public static double DOWN_POS = 0.95; - public static double UP_POS = 0.55; + public static double DOWN_POS = 0.62; + public static double UP_POS = 0.25; private static double outtakePosition = DOWN_POS; private Servo outtakeServo; @Override public void initialize() { - outtakeServo = ActiveOpMode.hardwareMap().servo.get("servo0"); + outtakeServo = ActiveOpMode.hardwareMap().servo.get("servoExp0"); outtakeServo.setDirection(Servo.Direction.REVERSE); } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index d2b45635b2b1..1c07e8cba6c1 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,7 +1,6 @@ repositories { mavenCentral() google() // Needed for androidx - maven { url = "https://mymaven.bylazar.com/releases" } } dependencies { @@ -14,9 +13,5 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:11.0.0' implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' - - implementation 'com.pedropathing:ftc:2.0.5' - implementation 'com.pedropathing:telemetry:1.0.0' - implementation 'com.bylazar:fullpanels:1.0.12' } From 2217deb8d1c33411631e307c58fff9666cb17ee1 Mon Sep 17 00:00:00 2001 From: p-rint <159627419+p-rint@users.noreply.github.com> Date: Wed, 14 Jan 2026 15:13:45 -0500 Subject: [PATCH 5/6] PLEaSE WORK MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 😭 --- .../java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java index f762c755da2f..fa2467d78a28 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java @@ -19,6 +19,8 @@ import dev.nextftc.ftc.NextFTCOpMode; import dev.nextftc.ftc.components.BulkReadComponent; +//PLEASE PUSHHH!H!! + @TeleOp(name="SecondTeleOp", group="TeleOp") public class SecondaryTeleOp extends NextFTCOpMode { { From b81359fe866b7b40a5646a0cf0ff51ef8f25a4cd Mon Sep 17 00:00:00 2001 From: p-rint <159627419+p-rint@users.noreply.github.com> Date: Wed, 14 Jan 2026 16:06:46 -0500 Subject: [PATCH 6/6] Fixed MainAuto.java, removed some commented stuff frim My teleop --- .../firstinspires/ftc/teamcode/MainAuto.java | 97 +------------------ .../ftc/teamcode/SecondaryTeleOp.java | 29 +----- 2 files changed, 3 insertions(+), 123 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java index a522cdb19ca2..2ad41a7f1d44 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java @@ -1,6 +1,7 @@ -<<<<<<< HEAD + package org.firstinspires.ftc.teamcode; // make sure this aligns with class location //HIIII pls don't delete + import com.pedropathing.follower.Follower; import com.pedropathing.geometry.BezierCurve; import com.pedropathing.geometry.BezierLine; @@ -213,98 +214,4 @@ public void start() { public void stop() {} - - -} -======= -package org.firstinspires.ftc.teamcode; - -import static dev.nextftc.extensions.pedro.PedroComponent.follower; - -import com.pedropathing.geometry.BezierLine; -import com.pedropathing.geometry.Pose; -import com.pedropathing.paths.Path; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import org.firstinspires.ftc.teamcode.pedroPathing.Constants; -import org.firstinspires.ftc.teamcode.subsystems.Drive; -import org.firstinspires.ftc.teamcode.subsystems.Intake; -import org.firstinspires.ftc.teamcode.subsystems.Outtake; -import org.firstinspires.ftc.teamcode.subsystems.Robot; -import org.firstinspires.ftc.teamcode.subsystems.Storage; -import org.firstinspires.ftc.teamcode.subsystems.Transitions; - -import dev.nextftc.core.commands.Command; -import dev.nextftc.core.commands.CommandManager; -import dev.nextftc.core.commands.delays.Delay; -import dev.nextftc.core.commands.groups.SequentialGroup; -import dev.nextftc.core.components.BindingsComponent; -import dev.nextftc.core.components.SubsystemComponent; -import dev.nextftc.extensions.pedro.FollowPath; -import dev.nextftc.extensions.pedro.PedroComponent; -import dev.nextftc.ftc.NextFTCOpMode; -import dev.nextftc.ftc.components.BulkReadComponent; - -@Autonomous -public class MainAuto extends NextFTCOpMode { - { - addComponents( - BulkReadComponent.INSTANCE, // TODO: make actual MANUAL mode bulkreading (we don't need to also read the expansion hub every loop) - BindingsComponent.INSTANCE, - CommandManager.INSTANCE, - new SubsystemComponent( - Storage.INSTANCE, - Robot.INSTANCE, - Drive.INSTANCE, - Intake.INSTANCE, - Outtake.INSTANCE, - Transitions.INSTANCE - ) - ); - } - - public static Pose endPose; - - - - public static final Pose startPoseBlue = new Pose(56, 8, Math.toRadians(270)); // Start Pose of our robot. - public static final Pose scorePoseBlue = new Pose(68, 76, Math.toRadians(315)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. - - public static final Pose startPoseRed = new Pose(87, 8, Math.toRadians(270)); // Start Pose of our robot. - public static final Pose scorePoseRed = new Pose(76, 76, Math.toRadians(225)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. - - - private final Pose startPose = startPoseBlue; - public static final Pose scorePose = scorePoseBlue; - public static final Pose scorePosebutActually = new Pose(73, 70, Math.toRadians(135)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. - - - Path scorePreload = new Path(new BezierLine(startPose, scorePose)); - - - private Command autonomousRoutine() { - scorePreload.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading()); - return new SequentialGroup( - new FollowPath(scorePreload), - new Delay(2), - Robot.outtakeAll - ); - } - - @Override - public void onStartButtonPressed() { - follower().setStartingPose(startPose); - autonomousRoutine().schedule(); - follower().breakFollowing(); - } - - public void onUpdate(){ - Drive.telemetryM.update(); - follower().update(); - } - - public void onStop(){ - endPose = follower().getPose(); - } } ->>>>>>> 5e83da2d1652b30a4b5bb2345d0985dcb419e579 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java index fa2467d78a28..c0dca67cffaa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java @@ -49,7 +49,7 @@ public class SecondaryTeleOp extends NextFTCOpMode { GamepadEx caimo = Gamepads.gamepad1(); GamepadEx jeff = Gamepads.gamepad2(); - + //gamepad1. // Debug stuff jeff.back() .whenBecomesTrue(() -> { @@ -62,34 +62,7 @@ public class SecondaryTeleOp extends NextFTCOpMode { Storage.setPIDMode(false).schedule(); }); -// // Storage nonsense -// jeff.leftStickX().greaterThan(0.1).and(jeff.leftStickButton() -// .whenTrue(() -> { -// Storage.setManualModeCommand(true).schedule(); -// Storage.setManualPowerCommand(jeff.leftStickX().get() / 1.5).schedule(); -// }) -// .whenFalse(() -> { -// Storage.setManualModeCommand(true).schedule(); -// Storage.setManualPowerCommand(jeff.leftStickX().get() / 4).schedule(); -// })); -// -// jeff.leftStickX().lessThan(-0.1).and(jeff.leftStickButton() -// .whenTrue(() -> { -// Storage.setManualModeCommand(true).schedule(); -// Storage.setManualPowerCommand(jeff.leftStickX().get() / 1.5).schedule(); -// }) -// .whenFalse(() -> { -// Storage.setManualModeCommand(true).schedule(); -// Storage.setManualPowerCommand(jeff.leftStickX().get() / 4).schedule(); -// })); // -// jeff.leftStickX() -// .inRange(-0.05, 0.05) -// .whenBecomesTrue(() -> { -// Storage.setManualModeCommand(true).schedule(); -// Storage.setManualPowerCommand(0).schedule(); -// }); - // Run Outake jeff.rightBumper() .whenBecomesTrue(() -> {