Telemetry

Having good observability/telemetry from the robot is crucial when tuning control loops, monitoring performance, or debugging issues. It's something I've spent many many hours on and have gone through multiple iterations of over the years. While looking at the robot code for "how we did telemetry" this year, I found we actually had 3 of them enabled.

Setup: All-in-one Buffered Telemetry

This will set up a buffered telemetry server on the robot along with web-based graphical interface.

The API

We store the data we want to monitor/log in "DataStores". They are just container classes that allow you to get() and put() values into.

You can find a list of them in the 2023 code and reproduced below. If you type DataStore_ in your IDE they should pop up.

DataStore_Boolean
DataStore_Double
DataStore_Duration
DataStore_Enum
DataStore_Float
DataStore_Integer
DataStore_Iteration
DataStore_MotionProfile
DataStore_NamedInteger
DataStore_Pose2d
DataStore_Pose2dWithCurvature
DataStore_Primitive
DataStore_Rotation2d
DataStore_String
DataStore_Translation2d
DataStore_Twist2d

As part of each subsystem, we group multiple "DataStores" into a de-facto¹ Telemetry class (which extends GroupDataStore). This Telemetry class is meant to contain all of the values we want from a particular subsystem. We add the contents of this class to the subsystem's DataStoreCircularBuffers on each iteration.

1. Set up the robot code

In Robot.java: robotInit()

        HTTPWebDataViewerServer mWebDataViewerServer = new HTTPWebDataViewerServer();
        HashMap<String, DataStoreCircularBuffer<GroupDataStore>> webDataViewerServerBuffers = new HashMap<>();
        // For every subsystem:
        mArm.registerTelemetryBuffersTo(webDataViewerServerBuffers::put);

        // Notify the server of each subsystem's telemetry buffers
        mWebDataViewerServer.onNewBuffers(webDataViewerServerBuffers);

        // Start the server
        mWebDataViewerServer.onStart();

In each subsystem: (or wherever you want to collect data)

public class Arm implements AcesSubsystem<Arm.Telemetry>, BufferedTelemetryProvider<Arm.Telemetry> {

    private final Telemetry mTelemetry = new Telemetry();
    private final DataStoreCircularBuffer<GroupDataStore> mTelemetryBuffer = new DataStoreCircularBuffer<>(mTelemetry, Telemetry::new, Constants.loopRates.kArmHz, Constants.kTelemetryDefaultSecondsToBuffer);

    @Override
    public void loop(double timestamp) {
        mLock.lock();
        try {

            // Read periodic inputs (removed for brevity)

            mTelemetry.currentState.put(mArmOperationalState);
            mTelemetry.baseArmOpenLoopSetpoint.put(mBaseArmArmOpenLoopSetpoint);
            mTelemetry.forearmOpenLoopSetpoint.put(mForearmArmOpenLoopSetpoint);
            mTelemetry.baseArmVelocitySetpoint.put(mBaseArmVelocitySetpoint);
            mTelemetry.forearmVelocitySetpoint.put(mForearmArmOpenLoopSetpoint);
            mTelemetry.baseArmPositionSetpoint.put(mBaseArmPositionSetPoint);
            mTelemetry.forearmPositionSetpoint.put(mForearmPositionSetPoint);

            // Perform subsystem logic (removed for brevity)

            // Output telemetry from subclasses that support it (these are AcesMotors)
            mArmBaseArmLeader.outputTelemetry();
            mArmForearmLeader.outputTelemetry();

            // Output other telemetry
            var rrConfiguration = ArmAngleDisplacementConverter.fromArmState(getArmState());
            mTelemetry.armLocationXY.put(UnitUtil.metersToInches(new Translation2d(rrConfiguration.endEffectorX, rrConfiguration.endEffectorY)));
            rrConfiguration = ArmAngleDisplacementConverter.fromArmState(getArmSetpointState());
            mTelemetry.armSetpointXY.put(UnitUtil.metersToInches(new Translation2d(rrConfiguration.endEffectorX, rrConfiguration.endEffectorY)));

            // Update the timestamp in the telemetry
            mTelemetry.timestamp.put(timestamp);

            // Push this iteration's data into the buffer
            mTelemetryBuffer.put();
        } finally {
            mLock.unlock();
        }
    }

    @Override
    public void registerTelemetryBuffersTo(BufferedTelemetryConsumer<GroupDataStore> consumer) {
        consumer.registerTelemetryBuffer("arm", mTelemetryBuffer);
    }

    public static class Telemetry extends GroupDataStore {
        public DataStore_Float timestamp = new DataStore_Float();

        public DataStore_AcesMotor baseArmLeaderStatus = new DataStore_AcesMotor();
        public DataStore_MotionProfile baseArmLeaderMotionProfile = new DataStore_MotionProfile();
        public DataStore_AcesMotor forearmLeaderStatus = new DataStore_AcesMotor();
        public DataStore_MotionProfile forearmLeaderMotionProfile = new DataStore_MotionProfile();

        public DataStore_Enum<ArmOperationalState> currentState = new DataStore_Enum<>(ArmOperationalState.UN_INIT);
        public DataStore_Float baseArmOpenLoopSetpoint = new DataStore_Float();
        public DataStore_Float forearmOpenLoopSetpoint = new DataStore_Float();
        public DataStore_Float baseArmVelocitySetpoint = new DataStore_Float();
        public DataStore_Float forearmVelocitySetpoint = new DataStore_Float();
        public DataStore_Float baseArmPositionSetpoint = new DataStore_Float();
        public DataStore_Float forearmPositionSetpoint = new DataStore_Float();
        public DataStore_Translation2d armLocationXY = new DataStore_Translation2d();
        public DataStore_Translation2d armSetpointXY = new DataStore_Translation2d();
    }

}

---

1. De-facto in this context is referring to a pattern that's followed but is not required or enforced - it doesn't need to be named Telemetry. It could just as easily be called ArmTelemetry or SwerveDriveTelemetry.↩