Stage 2 CTRE Solution Code by roboteer5291 · Pull Request #175 · frcsoftware/frcsoftware.org

roboteer5291 · 2026-08-04T00:27:43Z

Creates stage 2 CTRE Solution Code. Still very much a WIP

Claude assisted with some debugging and outlining of the parts of the collision avoidance logic. All code written by hand, in the end.

Todo:

github-actions · 2026-08-04T00:29:08Z

🌐 Preview URL: https://pr-175.frcsoftware.pages.dev

Daniel1464

I know this is still a draft, but left some preliminary comments since i have time

github-actions · 2026-09-04T04:46:45Z

🌐 Preview URL: https://pr-175-frcsoftware.frcsoftware.workers.dev

github-actions · 2026-09-24T18:52:47Z

🌐 Preview URL: https://pr-175.frcsoftware.org

Changed pages
No changed pages detected.

ArchdukeTim · 2026-09-28T19:34:37Z

+        pivot = new TalonFX(Constants.PIVOT_ID, Constants.BUS);
+        rollers = new TalonFX(Constants.ROLLERS_ID, Constants.BUS);


why are these here? Instantiate where they're declared

… stage2-ctre-solution

No idea how that was screwed up

Daniel1464 · 2026-11-01T02:08:07Z

+  }
+
+  @Override
+  public void simulationPeriodic() {


I think it's better to put coralSim.periodic() under a RobotBase.isSimulation check inside robotPeriodic instead. (Just my opinion, but I think the dedicated simulationPeriodic method is kind of an anti-pattern)

Why so? It's an included method. I do agree that the lack of clarity in the docs on the run order isn't great.

ArchdukeTim · 2026-11-01T02:52:26Z

+    table.log("Applied Voltage", Volts.of(getAppliedVoltage()));
+    table.log("Position", Rotations.of(getPosition()));
+    table.log("Velocity", RotationsPerSecond.of(getVelocity()));


These getters should return Units

I disagree. We made a conscious decision to not use the units library because of the clunky math. Having getters that we use the value of for logic reasons (ie the position) return a non-unit value, or return a unit value just to unpackage it, doesn't really make sense. So all of the getters should return a non-unit value to be consistent there.

or return a unit value just to unpackage it

Yet here you're unpacking it just to pack it up again.

Yeah I don't love the unpack -> repack. Since we (presumably) want to log this data with units, I think it might be better to just get the value (with units) from the status signal directly, rather than having an intermediate helper function? You can keep the helpers that just return doubles

ArchdukeTim · 2026-11-01T15:09:31Z

+        .a()
+        .onTrue(
+            Command.noRequirements(
+                    (coro) -> {


Why Coro instead of just co?

I saw coro used somewhere, and decided to use that. That's it. I do feel like coro is a bit too short since you cut out the entire second part of coroutine (routine)

Yeah I guess for teaching, anything shorter than coroutine is vague, but I wonder if wpilib examples use a specific name for this.

I believe the wpilib examples all use coroutine

ArchdukeTim · 2026-11-01T15:28:39Z

+        .onTrue(
+            Command.parallel(
+                    intake.setState(Intake.State.INTAKING),
+                    indexer.setVoltage(12),


A command for indexing would be better than manually applying the voltage

ArchdukeTim · 2026-11-01T15:30:42Z

+  public ArmTest(Robot robot) {
+    CommandXboxController controller = new CommandXboxController(0);
+
+    controller.a().onTrue(robot.superstructure.setArmPosition(0));


Should this OpMode move the elevator to a safe position?

ArchdukeTim · 2026-11-01T15:31:20Z

+  public ElevatorTest(Robot robot) {
+    CommandXboxController controller = new CommandXboxController(0);
+
+    controller.a().onTrue(robot.superstructure.setElevatorPosition(0));


Likewise should this put the arm up so elevator can move freely

You run into a catch-22 on these with your other comment to do this on the arm test opmode. I'll change it for the arm test opmode, but on the elevator test, I'm assuming you'd physically constrain the arm up there somehow. If you were bringing up a real robot, you'd only be able to tune one at once, and the same applies here since the arm code won't be written yet at the time of tuning the elevator.

This isn't a tuning opmode, this is just testing that it goes to different positions. If it doesn't command the arm, then it'll become invalid once the arm is attached.

It should still command the arm, and the arm could have the API, but not actually be connected to anything requiring physical constraints until it does get written

I definitely think this is a tuning opmode, as the intent is to use it with phoenix tuner or rev hardware client. In the context of the course, I think it's definitely a tuning opmode, since it'll be created immediately after the "Mechanisms" themselves to facilitate testing and tuning them.

To be clear, I don't think putting it in a safe position is bad here. It's just odd from a sequence of events standpoint in terms of the course. The overall solution code here is the final state of the code, and I'm not sure how to make it so that an intermediate snapshot would work here, considering this code will likely be RLI'd into the course

DylanB5402

partial review - just the thoughts I've had so far, going to keep reading through this offline

DylanB5402 · 2026-11-01T21:58:21Z

+
+  private static class Constants {
+    static final int MOTOR_ID = 22;
+    static final CANBus BUS = new CANBus(CANPort.CAN_S1);


(nitpick) we define the CANBus as its own variable here, but we don't in other subsystems. I don't think we're using the CANBus variable here, so we should probably just construct it inline in the TalonFX constructor. Or maybe define the bus variables somewhere else and pass them around? idk how I feel about that

DylanB5402 · 2026-11-01T22:07:55Z

+
+public class Arm {
+
+  private static class Constants {


Can't remember if I commented on the Constants static class before. I don't love it, probably wouldn't run it with my own team, but if that's a convention we want to use and there's consensus on it, I'm ok with it

From what I recall there wasn't ever a clear consensus in any direction. I personally like it as a least-bad option. Dedicated constants file being bad, and not liking the constants just scattered among everything else.

DylanB5402 · 2026-11-01T22:18:28Z

+    table.log("Applied Voltage", Volts.of(getAppliedVoltage()));
+    table.log("Position", Rotations.of(getPosition()));
+    table.log("Velocity", RotationsPerSecond.of(getVelocity()));


Yeah I don't love the unpack -> repack. Since we (presumably) want to log this data with units, I think it might be better to just get the value (with units) from the status signal directly, rather than having an intermediate helper function? You can keep the helpers that just return doubles

DylanB5402

second round of reviews! Also, can we get an ascope layout for this / maybe double check keybinds too in simgui-ds.json? I was able to drive with WASD once I set the keyboard as a joystick in the simgui, not sure what other buttons were supposed to do.

DylanB5402 · 2026-11-05T04:28:12Z

+                new Slot0Configs().withKP(0.3).withKI(0).withKD(0).withKS(0.2).withKV(0.125))
+            //                .withDriveMotorGains(new Slot0Configs()
+            //                        .withKP(0.3).withKI(0).withKD(0)
+            //                        .withKS(0.1498).withKV(3.16356))


Should clean up the comment code in this file? unless you have a reason to keep it

DylanB5402 · 2026-11-05T04:38:37Z

+        .named("Stop Drive");
+  }
+
+  public static class AutoAlignCommand implements Command {


What's the reasoning for making this its own Class? Generally have an aversion to class commands, though I'm not sure how relevant that is for 2026. There also is a lot of logic going on here, +1 to the other comment asking about breaking it out into its own file. I wonder if there's any way we could simplify this or write it in a more functional/imperative way?

Might just be a preference / code style thing going on here: this command looks like it's trying to handle a number of different scenarios, while I think I'd rather build smaller commands that are more specific in purpose. This command feels a little too stateful in a way that I'm not sure we want to encourage students to write their own code

DylanB5402 · 2026-11-05T04:39:24Z

+                new Slot0Configs().withKP(0.3).withKI(0).withKD(0).withKS(0.2).withKV(0.125))
+            //                .withDriveMotorGains(new Slot0Configs()
+            //                        .withKP(0.3).withKI(0).withKD(0)
+            //                        .withKS(0.1498).withKV(3.16356))


Decent bit of commented out code throughout this file, can we clean it up (or if there's a reason for it to be commented out that's fine)

DylanB5402 · 2026-11-05T04:42:14Z

+  }
+
+  private final TalonFX pivot = new TalonFX(Constants.PIVOT_ID, Constants.BUS);
+  private final TalonFX rollers = new TalonFX(Constants.ROLLERS_ID, Constants.BUS);


Not entirely sure how I feel about the pivot and rollers being in the same Mechanism. I'm personally a fan of splitting them just to get a little bit of separation and looser coupling, but I can see the case for having them as the same mechanism too, not sure if there's been other thought/discussion about this

DylanB5402 · 2026-11-05T04:48:36Z

+    table.log("Active Commands", getRunningCommands().toString());
+  }
+
+  public Command setArmVoltage(double voltage) {


Doesn't having commands that only run half the subsystem (just arm or just elevator) open up some edge cases where you have a command for the elevator that runs but then gets interrupted by an arm command?

These commands also only look to be called by the opmode, so maybe it's not a big deal? Maybe it's worth having setArm also explicitly set elevator to 0 volts or something

DylanB5402 · 2026-11-05T04:56:23Z

+  //        .named("Set Position");
+  //  }
+
+  public Command setPosition(double elevatorPosition, double armPosition) {


Maybe more thought experiment than actual feedback: I wonder how this command changes if you split arm and elevator into separate mechanisms?

I'm thinking that you get to make use of some of the more interesting ways to chain/compose/fork commands if you run them as separate subsystems, but I don't think it matters too much in practice, pretty sure Superstructure being its own mechanism is pretty set in stone at this point.

Initial Stage 2 ctre code, unfinished

fc052ec

roboteer5291 added CTRE Anything starter code or curriculum material touching CTRE libraries Java Code For all content relating to Java code (includes robot code and Stage 0 code) Stage 2 labels Aug 4, 2026

github-actions Bot added the examples label Aug 4, 2026

Daniel1464 reviewed Aug 4, 2026

View reviewed changes

roboteer5291 added 2 commits August 3, 2026 20:45

Tweaks + Javadoc

45cbe13

Update to alpha 7

2072d69

ArchdukeTim reviewed Sep 5, 2026

View reviewed changes

ArchdukeTim reviewed Sep 6, 2026

View reviewed changes

Comment thread examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Elevator.java Resolved

Comment thread examples/stage2/templates/ctre/src/main/java/first/robot/mechanisms/Superstructure.java Resolved

Many updates

fb2d5a7

roboteer5291 changed the base branch from main to stage2 September 10, 2026 18:35

roboteer5291 and others added 3 commits September 23, 2026 22:19

Real CTRE alpha 7 vendordep

6fd6ff0

Merge branch 'stage2' into stage2-ctre-solution

61d2712

Merge branch 'stage2' into stage2-ctre-solution

2beac2e

ArchdukeTim suggested changes Sep 28, 2026

View reviewed changes

roboteer5291 and others added 10 commits October 13, 2026 13:25

Spotless + small fixes

bab11a4

Spotless for real this time

2089dfc

Merge branch 'stage2' into stage2-ctre-solution

6c6a899

Relocate periodic methods to Robot

b19310c

Merge remote-tracking branch 'roboteer5291/stage2-ctre-solution' into…

45c4545

… stage2-ctre-solution

Change drive ratios to being calculated, not just magic numbers

8ece340

Spotless

eab183f

Convert mechanism telemetry to tables, add units

ffe5876

Remove duplicate coralsim/robotvisualization periodic calls

3a9f182

Intake borked

a12b99a

roboteer5291 removed the examples label Oct 27, 2026

roboteer5291 added 14 commits October 29, 2026 15:33

push

ba650a2

Intake pivot works!

bb2f916

Remove intake sim notifier and retune PID

f44585d

Add claw, indexer, and intake voltage to CoralSim, add intake to teleop

874c025

Spotless

a7ee053

Create 4 coral auto

7b5923f

Make auto use a for loop

7f174a4

Retune arm, fix arm sign

33c9050

No idea how that was screwed up

Remove unneeded factories from FourCoralAuto

f53c602

Make commands not end, add small tweaks to some teleops

5108632

Change Arm, Elevator to not be Mechanisms and be complete hidden

5ffceb5

Oops

14fb847

Oops pt2

79b78d1

Cleanup

8e410d6

roboteer5291 requested review from Adrianamm, ArchdukeTim, Daniel1464 and DylanB5402 and removed request for ArchdukeTim October 31, 2026 22:15

roboteer5291 added 2 commits October 31, 2026 17:17

Change to solution

7bd9283

Alliance flipping, alpha 8

16567f9

roboteer5291 marked this pull request as ready for review November 1, 2026 00:34

Daniel1464 suggested changes Nov 1, 2026

View reviewed changes

roboteer5291 and others added 3 commits October 31, 2026 21:16

Changes from suggestions

376ccce

Spotless

ffa8475

Merge branch 'stage2' into stage2-ctre-solution

0ed9eaa

ArchdukeTim reviewed Nov 1, 2026

View reviewed changes

DylanB5402 reviewed Nov 1, 2026

View reviewed changes

DylanB5402 reviewed Nov 5, 2026

View reviewed changes

		pivot = new TalonFX(Constants.PIVOT_ID, Constants.BUS);
		rollers = new TalonFX(Constants.ROLLERS_ID, Constants.BUS);

Conversation

roboteer5291 commented Aug 4, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

github-actions Bot commented Aug 4, 2026

Uh oh!

Daniel1464 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

github-actions Bot commented Sep 4, 2026

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

github-actions Bot commented Sep 24, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

roboteer5291 commented Aug 4, 2026 •

edited

Loading

github-actions Bot commented Sep 24, 2026 •

edited

Loading