Conditions and Logging

First, decide what tolerance counts as good enough. The arm is measured as an angle, so its tolerance is an angle too. WPILib's Angle type converts between rotations, degrees and radians for you. Degrees are the easiest of the three to picture, so we will make the tolerance one degree.

First, decide what tolerance counts as good enough. The flywheel is measured as a speed, so its tolerance is a speed too. WPILib's AngularVelocity type converts between rotations a second, RPM and radians a second for you. We will make the tolerance half a rotation a second.

This may take some tuning. Too tight and it never comes true, because the sensor jitters. Too loose and it comes true before the armflywheel is anywhere useful. For the same reason, never check that the reading equals the target exactly: it almost never will.

Next, add three getters. One says where the armflywheel is. One says where it is headed, which we read off the current request. The last says whether it has arrived.

1public class Arm implements Mechanism {
2  // ... motor, encoder and positionOut, unchanged
3 
4  // How close counts as "at target".
5  private final Angle tolerance = Degrees.of(1.0);
6 
7  // ... constructor and commands, unchanged
8 
9  /** True when the arm has reached its target angle. */
10  public boolean isAtTarget() {
11    return getPosition().isNear(getTargetPosition(), tolerance);
12  }
13 
14  /** Current measured arm angle. */
15  public Angle getPosition() {
16    return encoder.getPosition().getValue();
17  }
18 
19  /** Angle the arm is currently driving toward. */
20  public Angle getTargetPosition() {
21    return positionOut.getPositionMeasure();
22  }
23 
24  // ... setPosition and stopMotor, unchanged
25}

1public class Flywheel implements Mechanism {
2  // ... motor and velocityOut, unchanged
3 
4  // How close counts as "at target".
5  private final AngularVelocity tolerance = RotationsPerSecond.of(0.5);
6 
7  // ... constructor and commands, unchanged
8 
9  /** True when the flywheel has reached its target speed. */
10  public boolean isAtTarget() {
11    return getVelocity().isNear(getTargetVelocity(), tolerance);
12  }
13 
14  /** Current measured flywheel speed. */
15  public AngularVelocity getVelocity() {
16    return motor.getVelocity().getValue();
17  }
18 
19  /** Speed the flywheel is currently driving toward. */
20  public AngularVelocity getTargetVelocity() {
21    return velocityOut.getVelocityMeasure();
22  }
23 
24  // ... setVelocity and stopMotor, unchanged
25}

Recording starts once, at the top of the Robot constructor, and runs from the first loop to the last.

1import org.wpilib.driverstation.DriverStation;
2import org.wpilib.system.DataLogManager;
3 
4public Robot() {
5  // Record every logged value, plus the driver station, to a .wpilog file.
6  DataLogManager.start();
7  DriverStation.startDataLog(DataLogManager.getLog());
8}

DataLogManager.start() creates the log file and records every NetworkTables value into it. On the robot it records console output too. DataLogManager.getLog() hands back that same file, and DriverStation.startDataLog writes the driver station's values into it. Those are values that never appear in NetworkTables: whether the robot is enabled, the robot mode, which OpMode is running, and the joystick positions.

The armflywheel writes its own signals. Add TelemetryLoggable to the implements line, then write a logTo method that logs the three getters and the motor voltage.

1import org.wpilib.telemetry.TelemetryLoggable;
2import org.wpilib.telemetry.TelemetryTable;
3 
4public class Arm implements Mechanism, TelemetryLoggable {
5  // ... everything above, unchanged
6 
7  /** Write the arm's signals to the log. Robot calls this every loop. */
8  @Override
9  public void logTo(TelemetryTable table) {
10    table.log("Position", getPosition());
11    table.log("TargetPosition", getTargetPosition());
12    table.log("AtTarget", isAtTarget());
13    table.log("AppliedVoltage", motor.getMotorVoltage().getValue());
14  }
15 
16  // ... setPosition and stopMotor, unchanged
17}

1import org.wpilib.telemetry.TelemetryLoggable;
2import org.wpilib.telemetry.TelemetryTable;
3 
4public class Flywheel implements Mechanism, TelemetryLoggable {
5  // ... everything above, unchanged
6 
7  /** Write the flywheel's signals to the log. Robot calls this every loop. */
8  @Override
9  public void logTo(TelemetryTable table) {
10    table.log("Velocity", getVelocity());
11    table.log("TargetVelocity", getTargetVelocity());
12    table.log("AtTarget", isAtTarget());
13    table.log("AppliedVoltage", motor.getMotorVoltage().getValue());
14  }
15 
16  // ... setVelocity and stopMotor, unchanged
17}

A unit value logs in its SI unit, with the unit attached: radians for an angleradians per second for a speed, and volts for the voltage. You do not convert it here. AdvantageScope can convert it to rotations or degreesrotations per second or RPM when you graph it.

Nothing calls logTo yet. Robot does, once per mechanism, every loop, right after the scheduler runs.

1import org.wpilib.telemetry.Telemetry;
2 
3@Override
4public void robotPeriodic() {
5  Scheduler.getDefault().run();
6  Telemetry.log("Arm", arm);
7  Telemetry.log("Flywheel", flywheel);
8}

Telemetry.log sees a TelemetryLoggable, makes a table with the name you gave it, and hands that table to logTo. Everything the armflywheel logs lands under Telemetry/ArmFlywheel. robotPeriodic runs in every mode, so the log keeps going whether or not a command is running.

1. Start WPILib: Hardware Sim Robot Code, enable Teleoperated, and hold the left triggerright trigger until the armflywheel settles. Let go, then do it once more.
2. Disable, then stop the program.
3. Open the newest .wpilog from the project's logs folder in AdvantageScope. Do it before you start the program again: a file still named WPILIB_TBD_ is deleted on the next start.
4. Expand NT:/Telemetry/Arm. Put Position and TargetPosition on one graph, and AtTarget and AppliedVoltage on a second.
5. Expand NT:/Telemetry/Flywheel. Put Velocity and TargetVelocity on one graph, and AtTarget and AppliedVoltage on a second.

A signal that holds one value is not always a bug. Telemetry writes an entry only when the value changes, so a armflywheel that is still records one sample and then nothing until it moves.