LESSON 17

Motion Magic in Code

You have already run Motion Magic from the Control drop-down in Tuner X. This lesson sends the same request from code. It takes one new field, a fresh paste of your tuned config, and commands that name 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.

The request from Tuner X

On Motion Magic in Tuner X you set the Control drop-down to Motion Magic VoltageMotion Magic Velocity Voltage, gave it a target, and watched the armflywheel follow a profile. That drop-down picks a control request, and the Java class has the same name. Swap the VoltageOut field for it.

Arm.java: the request 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 request field

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

Its import has been at the top of the file since Mechanisms. Delete the import com.ctre.phoenix6.controls.VoltageOut; line, since nothing uses it any more.

Paste your tuned config

The request names a target. The gains decide how hard the motor works to reach it, and they live in the config. The config in your file may hold 0.0 gains. The branch ships them so a fresh clone holds still, and a paste made before you tuned carries zeros too. So paste again.

In Tuner X, open the armflywheel's config panel, press the three dots, and choose Generate Code.
Select the whole final TalonFXConfiguration talonFXCfg = ...; statement in the constructor and paste over it. Leave motor.getConfigurator().apply(talonFXCfg); under it.
Check that withSlot0 and the Motion Magic values hold the numbers from your bench, not zeros.

Arm.java: the shape of the paste, with your numbers in place of 0.0

1    final TalonFXConfiguration talonFXCfg =
2        new TalonFXConfiguration()
3            .withMotorOutput(
4                new MotorOutputConfigs()
5                    .withNeutralMode(NeutralModeValue.Coast)
6                    .withInverted(InvertedValue.CounterClockwise_Positive))
7            .withSlot0(
8                new Slot0Configs()
9                    .withKG(0.0)
10                    .withKS(0.0)
11                    .withKP(0.0)
12                    .withKD(0.0)
13                    .withGravityType(GravityTypeValue.Arm_Cosine))
14            .withMotionMagic(
15                new MotionMagicConfigs()
16                    .withMotionMagicCruiseVelocity(RotationsPerSecond.of(0.0))
17                    .withMotionMagicAcceleration(RotationsPerSecondPerSecond.of(0.0))
18                    .withMotionMagicExpo_kV(
19                        Volts.per(RotationsPerSecond).ofNative(0.119999997317791))
20                    .withMotionMagicExpo_kA(
21                        Volts.per(RotationsPerSecondPerSecond).ofNative(0.10000000149011612)))
22            .withFeedback(
23                new FeedbackConfigs()
24                    .withFeedbackRemoteSensorID(32)
25                    .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder));

Flywheel.java: the shape of the paste, with your numbers in place of 0.0

1    final TalonFXConfiguration talonFXCfg =
2        new TalonFXConfiguration()
3            .withMotorOutput(
4                new MotorOutputConfigs()
5                    .withNeutralMode(NeutralModeValue.Coast)
6                    .withInverted(InvertedValue.Clockwise_Positive))
7            .withSlot0(new Slot0Configs().withKS(0.0).withKV(0.0).withKP(0.0))
8            .withMotionMagic(
9                new MotionMagicConfigs()
10                    .withMotionMagicCruiseVelocity(RotationsPerSecond.of(0.0))
11                    .withMotionMagicAcceleration(RotationsPerSecondPerSecond.of(0.0))
12                    .withMotionMagicExpo_kV(
13                        Volts.per(RotationsPerSecond).ofNative(0.119999997317791))
14                    .withMotionMagicExpo_kA(
15                        Volts.per(RotationsPerSecondPerSecond).ofNative(0.10000000149011612)));

Name targets, not volts

An arm that holds an angle does not need a slow push, a fast push and a stop. Delete runSlow, runFast, stop, setVoltage and stopMotor, and put these in their place.

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  }

Nothing binds horizontal yet. Workshop 4 uses it.

The flywheel keeps all three commands and stopMotor. Replace setVoltage with setVelocity, and change the two numbers from volts to rotations per second.

Flywheel.java: what changes

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  private void setVelocity(double rps) {
12    motor.setControl(velocityOut.withVelocity(RotationsPerSecond.of(rps)));
13  }

Update the bindings

robot.arm.stop() is gone, so the left-trigger line in MyTeleop.java loses its whileFalse.

MyTeleop.java: the arm binding

1    // Hold the left trigger to drive the arm to its vertical position. Releasing cancels the
2    // command; the position request stays applied, so the arm holds where it is.
3    driver.leftTrigger().whileTrue(robot.arm.vertical());

Releasing the trigger cancels the command, and the motor keeps applying the last request it received. On a voltage request that was the hazard in Hardware Simulation. On a position request it is what you want: the arm holds the target against gravity.

The flywheel bindings in MyTeleop.java do not change. A wheel left on its last velocity request keeps spinning, so they keep their whileFalse.

Check your work

Start Hardware Sim Robot Code, pick Teleoperated and your OpMode, enable, and hold the binding. The build runs on the way, so a compile error shows up here.

Check

You should see

The arm drive to vertical and stop there, however long you hold.
The arm stay at vertical when you release.
The wheel come up to 75 rotations per second and hold it on the right trigger, then settle at 25 when you release.
The same speeds every run, whatever the battery is doing.

A armflywheel that does not move at all still has 0.0 gains, so repeat the paste. One that overshoots and hunts is a tuning problem. Fix it in Tuner X with PID Tuning in Tuner X, then generate and paste again.