Improve structure of command-based framework #22

ArchdukeTim · 2023-04-17T04:36:24Z

This is not meant to be my way or the highway

I spent the last week looking at the docs for command-based and talking with some of the developers in the frc discord. After lots of learning, I've taken some of the concepts that were recommended and applied them to this code. There are two main sets of changes

Changing around how subsystems work to make them fit in better with command-based.
- Commands that only rely on a single subsystem belong to that subsystem
- Composition commands that are just chains of more primitive commands are defined in RobotContainer
  - This is mainly to avoid boilerplate code associated with putting each different high-level command in its own class.
    - Putting high-level commands in their own class is not wrong, I just prefer fewer lines myself.
  - Subsystems now use the default command to act upon the motors (unless the motors themself have closed-loop positioning
    - This seems to make it easier to link user inputs to commands
Rewriting the command subroutines (auto, buttons that perform complex commands like balancing) to use the command factories
- Greatly improves readability, since the sequence tends to flow from top to bottom now

Obviously I haven't tested any of this on a real robot, but I think the general principles still come across, even if it's a tad bit buggy hot off the press.

I'd love for you guys to look at these changes and try to understand what it is I've done differently. Think about how you might add additional functionality and if the way I've set up the commands would make that easier / harder. This is a great time to look back on the season and become more skilled for next year.

If you dare, try putting it on the robot and see what bugs there are. One big change I made was I removed the manual control of the arm, as a challenge to get the PID+FF controller working flawlessly. That level of robot control will elevate you to the next level.

ArchdukeTim · 2023-04-17T04:36:50Z

build.gradle

@@ -97,3 +98,11 @@ wpi.java.configureTestTasks(test)
 tasks.withType(JavaCompile) {
    options.compilerArgs.add '-XDstringConcat=inline'
 }
+
+spotless {


./gradlew spotlessApply formats the code consistently.

ArchdukeTim · 2023-04-17T04:38:18Z

src/main/java/frc/robot/Robot.java

This file just has changes recommend by IDE. You can ignore

ArchdukeTim · 2023-04-17T04:39:33Z

src/main/java/frc/robot/RobotContainer.java

+  public Command followTrajectory(String name, PathConstraints constraints) {
+    return new FollowTrajectoryCommand(name, odometry, swerveDrive, eventMap, constraints);
+  }


Command factory functions like these help reduce duplicate code

ArchdukeTim · 2023-04-17T04:40:37Z

src/main/java/frc/robot/RobotContainer.java

+    return new FollowTrajectoryCommand(name, odometry, swerveDrive, eventMap, constraints);
+  }
+
+  public Command deliverCone(Constants.ConePosition conePosition) {


Not putting commands in separate classes makes it quick to move a subroutine into a named variable, which improves readability, and makes it easy to use this subroutine in multiple larger routines

ArchdukeTim · 2023-04-17T04:42:23Z

src/main/java/frc/robot/commands/DeliverMiddleConeCommand.java

-        addCommands(
-            new InstantCommand(() -> grabberSubsystem.grab()),
-            new WaitCommand(0.2),
-            new WaitCommand(5)
-                .deadlineWith(
-                    new WaitUntilCommand(() -> Math.abs(telescopeSubsystem.getTelescopePosition() - ArmConstants.telescopeMiddleSetpoint) < 0.05)
-                        .andThen(new InstantCommand(() -> grabberSubsystem.toggle()))
-                            .deadlineWith(
-                                new RunCommand(() -> pivotSubsystem.setPivotPosition(0.015)),
-                                new WaitUntilCommand(() -> pivotSubsystem.getPivotInRange(0.015, 0.012))
-                                    .andThen(new RunCommand(() -> telescopeSubsystem.setTelescopePosition(ArmConstants.telescopeMiddleSetpoint))))
-                        .andThen(new WaitCommand(0.5)
-                            .andThen(new ParallelCommandGroup(
-                                new RunCommand(() -> telescopeSubsystem.setTelescopePosition(0.03)),
-                                new WaitUntilCommand(() -> telescopeSubsystem.getTelescopePosition() < 0.2)
-                                    .andThen(new RunCommand(() -> pivotSubsystem.setPivotPosition(ArmConstants.elevatorUpPivotDownPosition)))))))


Compare this to the new definition of this auto routine:

return grabberSubsystem .grab() .andThen( pivotSubsystem .moveToTargetContinuously(getArmPositionForCone(conePosition)) .raceWith( telescopeSubsystem .moveToTargetAndHold(getTelescopePositionForCone(conePosition)) .andThen( grabberSubsystem.release(), telescopeSubsystem.moveToTarget(0.03))), pivotSubsystem.moveToTargetUntilThere(ArmConstants.elevatorUpPivotDownPosition));

ArchdukeTim · 2023-04-17T04:49:33Z

src/main/java/frc/robot/subsystems/GrabberSubsystem.java

+  public Command release() {
+    return runOnce(this::openGrabber).andThen(Commands.waitSeconds(0.2));
+  }


Only commands are exposed publicly. That forces you to only control subsystems through the command scheduler, which can avoid conflicts caused by not setting requirements correctly. It also allows you to add command-based behavior to your publicly facing methods.

Consider this grabber case: You always had a delay after triggering the grabber solenoids to account for the time it takes for the pistons to move.
By making the control of these pistons commands, now the delay is built into this control, and is abstracted away from the user of this subsystem. If you replaced the current pistons with a slower/faster piston, you only need to change the time in these two methods, rather than every place you use them

ArchdukeTim · 2023-04-17T04:57:27Z

src/main/java/frc/robot/subsystems/GrabberSubsystem.java

+  public boolean isOpen() {
+    return leftSolenoid.get() == Value.kForward;
+  }


Methods that expose the state of the subsystem are useful/necessary for building complex routines that need to know about the state of the subsystem. Remember, if you need to access the state of a subsystem from another, move the commands up to a common scope (like RobotContainer, but pls not network tables)

ArchdukeTim · 2023-04-17T04:59:52Z

src/main/java/frc/robot/subsystems/LedSubsystem.java

-        color = LedColor.DOCKED;
-    }
+  public Command showBalancingColorCommand() {
+    return runOnce(() -> color = LedColor.BALANCING);


This class is a very simple example of how the commands set the wanted state, and the default command applies the wanted state.

You could 100% remove the default command, and have each of these command factories set a value on the motor, but it's nice to have the physical values all get set in the same place, just to make adding on things easier

ArchdukeTim · 2023-04-17T05:02:11Z

src/main/java/frc/robot/subsystems/PivotSubsystem.java

+
+  private double targetPos;
+
+  public PivotSubsystem(CANSparkMax pivotMotor, TalonFXSensorCollection telescopeSensor) {


maybe this is cheating. You decide. If you didn't want to pass in sensors from other subsystems, you could have a PivotFeedforwardCalculator class that aggregated the data and provided an ff value

ArchdukeTim · 2023-04-17T05:02:59Z

src/main/java/frc/robot/subsystems/PivotSubsystem.java

+
+  @Override
+  public void periodic() {
+    ntPivotPosition.setDouble(pivotEncoder.getPosition());


periodic is only used for updating internal state (like network tables), not external state (like motor values)

Rewrite to better follow command-based paradigms

dc3261a

ArchdukeTim commented Apr 17, 2023

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Improve structure of command-based framework #22

Improve structure of command-based framework #22

ArchdukeTim commented Apr 17, 2023 •

edited

Loading

ArchdukeTim Apr 17, 2023

ArchdukeTim Apr 17, 2023

ArchdukeTim Apr 17, 2023

ArchdukeTim Apr 17, 2023

ArchdukeTim Apr 17, 2023

ArchdukeTim Apr 17, 2023

ArchdukeTim Apr 17, 2023

ArchdukeTim Apr 17, 2023

ArchdukeTim Apr 17, 2023

ArchdukeTim Apr 17, 2023


		private double targetPos;

		public PivotSubsystem(CANSparkMax pivotMotor, TalonFXSensorCollection telescopeSensor) {

Improve structure of command-based framework #22

Are you sure you want to change the base?

Improve structure of command-based framework #22

Conversation

ArchdukeTim commented Apr 17, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ArchdukeTim commented Apr 17, 2023 •

edited

Loading