Commands

Here you will learn about commands.

Shows options on how to set up new commands. More information on commands can be found here

There are two main ways we recommend setting up commands.

This is a standard command. Refer to WPILib for more examples.

// Run flywheel full speed then stop
Commands.startEnd(
    () -> flywheel.setSpeed(1),
    () -> flywheel.setSpeed(0),
    flywheel
)

With more complex systems, you need a way to better define states. For this, we implement a state system designed by CCShambots.

package frc.robot.subsystems.flywheel; import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.utils.loggable.LoggableTalonFX; import frc.robot.utils.statemachine.StateMachine; import java.util.function.Supplier; public class Flywheel extends StateMachine<Flywheel.State> { private final LoggableTalonFX flywheel = new LoggableTalonFX(53, "flywheel"); final MotionMagicVelocityVoltage request = new MotionMagicVelocityVoltage(0); TalonFXConfiguration config = new TalonFXConfiguration(); private static final AngularVelocity AMP_SHOT = RotationsPerSecond.of(20.0); private static final AngularVelocity TRAP_SHOT = RotationsPerSecond.of(3.6); private static final AngularVelocity SUBWOOFER_SHOT = RotationsPerSecond.of(10.0); private static final AngularVelocity AMP_TOLERANCE = RotationsPerSecond.of(5.0); private static final AngularVelocity TRAP_TOLERANCE = RotationsPerSecond.of(1.0); private static final AngularVelocity SUBWOOFER_TOLERANCE = RotationsPerSecond.of(4.0); private static final AngularVelocity SPEAKER_TOLERANCE = RotationsPerSecond.of(0.75); private Supplier<AngularVelocity> speaker; public Flywheel(Supplier<AngularVelocity> speaker) { super("Flywheel", State.UNDETERMINED, State.class); this.speaker = speaker; registerStateCommands(); registerTransitions(); configureMotors(); } private void configureMotors() { config.CurrentLimits.SupplyCurrentLimit = 40.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.Slot0.kS = 0.366; // Add 0.366 V output to overcome static friction config.Slot0.kV = 8.0 / 54.154; // When applying 8V spins at 54.154 config.Slot0.kA = 0.0; config.Slot0.kP = 0.9; config.Slot0.kI = 0.0; config.Slot0.kD = 0.001; // set Motion Magic Velocity settings var motionMagicConfigs = config.MotionMagic; motionMagicConfigs.MotionMagicAcceleration = 50.0; motionMagicConfigs.MotionMagicJerk = 500; boolean statusOK = flywheel.getConfigurator().apply(config, 0.1) == StatusCode.OK; BaseStatusSignal.setUpdateFrequencyForAll( 50.0, flywheel.getPosition(), flywheel.getVelocity(), flywheel.getMotorVoltage(), flywheel.getTorqueCurrent()); } private void registerStateCommands() { registerStateCommand( State.AMP, new ParallelCommandGroup( new InstantCommand(() -> setFlywheelTarget(AMP_SHOT)), atSpeedCommand(() -> AMP_SHOT, AMP_TOLERANCE))); registerStateCommand( State.SPEAKER, new ParallelCommandGroup( new RunCommand(() -> setFlywheelTarget(speaker.get())), atSpeedCommand(speaker, SPEAKER_TOLERANCE))); registerStateCommand( State.SUBWOOFER, new ParallelCommandGroup( new RunCommand(() -> setFlywheelTarget(SUBWOOFER_SHOT)), atSpeedCommand(() -> SUBWOOFER_SHOT, SUBWOOFER_TOLERANCE))); registerStateCommand( State.TRAP, new ParallelCommandGroup( new RunCommand(() -> setFlywheelTarget(TRAP_SHOT)), atSpeedCommand(() -> TRAP_SHOT, TRAP_TOLERANCE))); registerStateCommand(State.IDLE, this::stop); registerStateCommand(State.TRAP, () -> setFlywheelTarget(TRAP_SHOT)); } private void registerTransitions() { addOmniTransition(State.IDLE); addOmniTransition(State.AMP); addOmniTransition(State.SPEAKER); addOmniTransition(State.TRAP); addOmniTransition(State.SUBWOOFER); } public void setFlywheelTarget(AngularVelocity velocity) { flywheel.setControl(request.withVelocity(velocity)); } private Command atSpeedCommand( Supplier<AngularVelocity> speedProvider, AngularVelocity accuracy) { return new RunCommand( () -> { AngularVelocity diff = flywheel.getVelocity() .getValue() .minus(speedProvider.get()); if (diff.magnitude() < accuracy.magnitude()) { setFlag(State.AT_SPEED); } else { clearFlag(State.AT_SPEED); } }); } public void stop() { flywheel.stopMotor(); } public enum State { UNDETERMINED, IDLE, SPEAKER, AMP, TRAP, SUBWOOFER, // flags AT_SPEED } @Override protected void determineSelf() { setState(State.IDLE); } }

Complex Example Breakdown

Let's go ahead and break down the complex command.

public class Flywheel extends StateMachine<Flywheel.State> {

This code allows us to extend the CCShambots StateMachine class and add different states. States are gotten from our enum Flywheel.State

  public Flywheel(Supplier<AngularVelocity> speaker) {
    super("Flywheel", State.UNDETERMINED, State.class);
    this.speaker = speaker;
    registerStateCommands();
    registerTransitions();
    configureMotors();
  }

In this block of code, we are setting our flywheel to an undetermined state, registering all states the flywheel can be in, and also the state-to-state transitions that are allowed

Now let's dive into registerStateCommands().

    registerStateCommand(
        State.AMP,
        new ParallelCommandGroup(
            new InstantCommand(() -> setFlywheelTarget(AMP_SHOT)),
            atSpeedCommand(() -> AMP_SHOT, AMP_TOLERANCE)));

This code defines what we want to happen when we tell the Flywheel to go into AMP mode.

In this case, we want to set the flywheel speed and trigger a flag when our speed is within tolerance to alert another part of our code to shoot.

By defining all possible states of the robot, we give it a clear purpose and direction, enhancing its functionality. This will allow you to add safety and more automation to your code.

  private void registerTransitions() {
    addOmniTransition(State.IDLE);
    addOmniTransition(State.AMP);
    addOmniTransition(State.SPEAKER);
    addOmniTransition(State.TRAP);
    addOmniTransition(State.SUBWOOFER);
  }

This allows our flywheel to transition between any state without restrictions. This will change on your applications.

After that, all the code is pretty standard in other commands.

Finally, you need to call from RobotContainer.java to set the state. You may need states in your RobotContainer.java for very complicated robots.

        joystick.a().whileTrue(
            flywheel.transitionCommand(Flywheel.State.AMP)
        );
    

        registerStateCommand(
            State.AMP,
            Commands.either(
                Commands.parallel(
                    flywheel.transitionCommand(Flywheel.State.AMP),
                    arm.transitionCommand(Arm.State.AMP),
                    magazine.transitionCommand(Magazine.State.IDLE)),
                Commands.parallel(
                    flywheel.transitionCommand(Flywheel.State.IDLE),
                    arm.transitionCommand(Arm.State.INTAKE),
                    feedIfArmInIntakePosition),
                () -> lineBreak.getState() == LineBreak.State.LOADED));
        joystick.a().whileTrue(RobotContainer.transitionCommand(State.AMP));
    

Last modified: 31 December 2024