From 7de8447d82cd862c607fd2703e2280335ba2a4b7 Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Wed, 28 Jan 2026 15:04:36 +0000 Subject: [PATCH 1/2] Added Pedro Example --- src/extensions/pedro/path-example.md | 130 +++++++++++++++++++++++++++ 1 file changed, 130 insertions(+) create mode 100644 src/extensions/pedro/path-example.md diff --git a/src/extensions/pedro/path-example.md b/src/extensions/pedro/path-example.md new file mode 100644 index 0000000..c8575ae --- /dev/null +++ b/src/extensions/pedro/path-example.md @@ -0,0 +1,130 @@ +# Pedro Example Auto + +To get your PedroPathing journey started with NextFTC, here is an `example` autonomous composed with next ftc, involving `subsystems` as well. + +:::tabs key:code + +== Kotlin + +```kotlin +@Autonomous(name = "PedroExample") +class PedroExample: NextFTCOpMode() { + init { + addComponents( + SubsystemComponent(Flywheel, ShooterAngle), + BulkReadComponent, + PedroComponent(Constants::createFollower) + ) + } + + private val startPose = Pose(56.0, 7.5, Math.toRadians(90.0)) + private val shootPose = Pose(56.0, 10.5, Math.toRadians(90.0)) + private val leavePoint = Pose(36.49, 8.20, Math.toRadians(90.0)) + + private lateinit var Leave: PathChain + private lateinit var shoot: PathChain + + private fun buildPaths() { + shoot = follower.pathBuilder() + .addPath(BezierLine(startPose,shootPose)) + .setLinearHeadingInterpolation(startPose.heading, shootPose.heading) + .build() + Leave = follower.pathBuilder() + .addPath(BezierLine(startPose,leavePoint)) + .setLinearHeadingInterpolation(startPose.heading, leavePoint.heading) + .setLinearHeadingInterpolation(shootPose.heading, leavePoint.heading) + .build() + } + + val autoRoutine: Command + get() = + SequentialGroup( + ParallelGroup( + Flywheel.goTo(3000.0), + ShooterAngle.angle_up, + FollowPath(shoot) + + ), + Delay(2.0.seconds), + ParallelGroup( + Flywheel.goTo(0.0), + ShooterAngle.angle_down, + ), + FollowPath(Leave) + ) + + override fun onInit() { + follower.setMaxPower(1.0) + } + + override fun onStartButtonPressed() { + follower.setStartingPose(startPose) + buildPaths() + autoRoutine() + } +``` + +== Java + +```java +@Autonomous(name = "PedroExample") +public class PedroExample extends NextFTCOpMode { + private final Pose2d startPose = new Pose2d(56.0, 7.5, Math.toRadians(90.0)); + private final Pose2d shootPose = new Pose2d(56.0, 10.5, Math.toRadians(90.0)); + private final Pose2d leavePoint = new Pose2d(36.49, 8.20, Math.toRadians(90.0)); + + private PathChain Leave; + private PathChain shoot; + + public PedroExample() { + addComponents( + new SubsystemComponent(Flywheel, ShooterAngle), + BulkReadComponent, + new PedroComponent(Constants::createFollower) + ); + } + + private void buildPaths() { + shoot = follower.pathBuilder() + .addPath(new BezierLine(startPose, shootPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), shootPose.getHeading()) + .build(); + + Leave = follower.pathBuilder() + .addPath(new BezierLine(startPose, leavePoint)) + .setLinearHeadingInterpolation(startPose.getHeading(), leavePoint.getHeading()) + .setLinearHeadingInterpolation(shootPose.getHeading(), leavePoint.getHeading()) + .build(); + } + + private Command getAutoRoutine() { + return new SequentialGroup( + new ParallelGroup( + Flywheel.goTo(3000.0), + ShooterAngle.angle_up, + new FollowPath(shoot) + ), + new Delay(TimeUnit.SECONDS.toMillis(2)), + new ParallelGroup( + Flywheel.goTo(0.0), + ShooterAngle.angle_down + ), + new FollowPath(Leave) + ); + } + + @Override + public void onInit() { + follower.setMaxPower(1.0); + } + + @Override + public void onStartButtonPressed() { + follower.setStartingPose(startPose); + buildPaths(); + getAutoRoutine().run(); + } +} +``` + +::: From 90da553eaab50c480774f4d60905ef591d30ff5a Mon Sep 17 00:00:00 2001 From: Sid <114190535+siddharth-shah121@users.noreply.github.com> Date: Mon, 23 Feb 2026 15:48:27 +0000 Subject: [PATCH 2/2] camelCase and better description --- src/extensions/pedro/path-example.md | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/extensions/pedro/path-example.md b/src/extensions/pedro/path-example.md index c8575ae..82e2dd4 100644 --- a/src/extensions/pedro/path-example.md +++ b/src/extensions/pedro/path-example.md @@ -2,6 +2,8 @@ To get your PedroPathing journey started with NextFTC, here is an `example` autonomous composed with next ftc, involving `subsystems` as well. +This code starting in the `far` launch zone for autonomous, steps a few inches forward to shoot, then advancing to the leave point away from the wall. + :::tabs key:code == Kotlin @@ -21,7 +23,7 @@ class PedroExample: NextFTCOpMode() { private val shootPose = Pose(56.0, 10.5, Math.toRadians(90.0)) private val leavePoint = Pose(36.49, 8.20, Math.toRadians(90.0)) - private lateinit var Leave: PathChain + private lateinit var leave: PathChain private lateinit var shoot: PathChain private fun buildPaths() { @@ -50,7 +52,7 @@ class PedroExample: NextFTCOpMode() { Flywheel.goTo(0.0), ShooterAngle.angle_down, ), - FollowPath(Leave) + FollowPath(leave) ) override fun onInit() { @@ -73,7 +75,7 @@ public class PedroExample extends NextFTCOpMode { private final Pose2d shootPose = new Pose2d(56.0, 10.5, Math.toRadians(90.0)); private final Pose2d leavePoint = new Pose2d(36.49, 8.20, Math.toRadians(90.0)); - private PathChain Leave; + private PathChain leave; private PathChain shoot; public PedroExample() { @@ -97,19 +99,19 @@ public class PedroExample extends NextFTCOpMode { .build(); } - private Command getAutoRoutine() { + private Command autoRoutine() { return new SequentialGroup( new ParallelGroup( Flywheel.goTo(3000.0), ShooterAngle.angle_up, new FollowPath(shoot) ), - new Delay(TimeUnit.SECONDS.toMillis(2)), + new Delay(Duration.ofSeconds(2)), new ParallelGroup( Flywheel.goTo(0.0), ShooterAngle.angle_down ), - new FollowPath(Leave) + new FollowPath(leave) ); } @@ -122,7 +124,7 @@ public class PedroExample extends NextFTCOpMode { public void onStartButtonPressed() { follower.setStartingPose(startPose); buildPaths(); - getAutoRoutine().run(); + autoRoutine().schedule(); } } ```