LESSON 15

Motion Magic in Code

The gains you measured in Tuner X came across with the config you already pasted. Nothing left to configure: swap the control request, and the commands stop asking for volts and start naming a target.

Branchmech-3-MotionMagic10 minutes

You’ll need

Buttons moving your mechanism, from Hardware Simulation.
Gains tuned on the bench in PID Tuning in Tuner X and Motion Magic in Tuner X.

What mechanism are you working on?

The lesson below is written for the one you pick. Switch back any time to read it for the other.

Swap the control request

Every command so far pushed a voltage and hoped. The gains to do better are already in your Arm.javaFlywheel.java. Generate Code read them off the device you tuned in Workshop 1, and they came across with the rest of the config on Mechanisms. Nothing there needs touching.

What changes is the request. VoltageOut goes, and one that names a target takes its place. Nothing to import. The class you need has been at the top of the file since Mechanisms. The only import that moves is VoltageOut, leaving.

Arm.java: the field

1  // Moves the arm to a target angle along a smooth Motion Magic ramp.
2  private final MotionMagicVoltage positionOut = new MotionMagicVoltage(0);

Flywheel.java: the field

1  // Asks the motor to ramp to a target speed instead of jumping to it.
2  private final MotionMagicVelocityVoltage velocityOut = new MotionMagicVelocityVoltage(0);

Name targets, not volts

Rename the private setVoltage to setPositionsetVelocity and give it the new request to send. Leave the old name and the commands below have nothing to call. Nothing else about them moves: still runRepeatedly, still .named(...), still holds.

Arm.java: the commands

1  /**
2   * Move to vertical, 0.25 rotations or 90 degrees, and hold it there. This is the stowed
3   * position for transport. Never finishes.
4   */
5  public Command vertical() {
6    return runRepeatedly(() -> setPosition(0.25)).named("vertical (hold)");
7  }
8 
9  /**
10   * Move to horizontal, 0.5 rotations or 180 degrees, and hold it there. This is the ground
11   * intake position. Never finishes.
12   */
13  public Command horizontal() {
14    return runRepeatedly(() -> setPosition(0.5)).named("horizontal (hold)");
15  }
16 
17  private void setPosition(double rotations) {
18    motor.setControl(positionOut.withPosition(rotations));
19  }

Flywheel.java: the commands

1  /** Spin the flywheel at 25 rotations per second and hold it. Never finishes. */
2  public Command runSlow() {
3    return runRepeatedly(() -> setVelocity(25.0)).named("runSlow (hold)");
4  }
5 
6  /** Spin the flywheel at 75 rotations per second and hold it. Never finishes. */
7  public Command runFast() {
8    return runRepeatedly(() -> setVelocity(75.0)).named("runFast (hold)");
9  }
10 
11  /** Stop the flywheel and keep it stopped. Never finishes. */
12  public Command stop() {
13    return runRepeatedly(this::stopMotor).named("stop (hold)");
14  }
15 
16  private void setVelocity(double rps) {
17    motor.setControl(velocityOut.withVelocity(RotationsPerSecond.of(rps)));
18  }

Update the bindings

MyTeleop.java: the constructor

1  public MyTeleop(Robot robot) {
2    // Hold the left trigger to drive the arm to its vertical position. Releasing cancels the
3    // command; the position request stays applied, so the arm holds where it is.
4    driver.leftTrigger().whileTrue(robot.arm.vertical());
5 
6    // Right trigger: spin fast while held, drop back to the slow hold speed when released.
7    driver.rightTrigger().whileTrue(robot.flywheel.runFast()).whileFalse(robot.flywheel.runSlow());
8 
9    // A: spin fast while held, stop when released.
10    driver.a().whileTrue(robot.flywheel.runFast()).whileFalse(robot.flywheel.stop());
11  }

Note

Position needs no whileFalse

The arm binding is a bare whileTrue. Releasing the trigger cancels the command, and the controller carries on applying the last position request, so the arm holds where it got to. That is the trap from OpModes working for you rather than against you. Speed is different: a flywheel left on its last target keeps spinning, so those bindings keep their whileFalse.

Check your work

Start WPILib: Hardware Sim Robot Code and hold your binding. It builds on the way, so a compile error turns up here without a separate build step.

Check

You should see

The arm drive to the target and stop there, rather than push for as long as you hold.
The arm stay put when you release, holding against gravity.
The wheel come up to a speed and hold it, rather than climb for as long as the button is down.
The same speed every time, whatever the battery is doing.

A mechanism that does not move at all is the giveaway that the 0.0 gains are still in the file. One that overshoots and hunts is a real tuning problem, and it belongs back in Tuner X rather than in the Java.

Check yourself

Why does the arm binding use a bare whileTrue, with no whileFalse behind it?

a.Releasing cancels the command, and the controller keeps applying the last position request, so the arm holds where it isb.The scheduler zeroes a mechanism as soon as nothing commands itc.A position command finishes on its own, so there is nothing to canceld.whileFalse only works with VoltageOut requests

Where do the numbers in this config come from?

a.Tuner X computes them the first time the robot program connectsb.Workshop 1, measured on the bench and read back out with Generate Codec.The Phoenix 6 documentation's recommended starting valuesd.They are calculated from the mechanism's mass and gearing at startup

Pick an answer for each.