Merge pull request #87 from strykeforce/telemetry-registrable

Add Registrable interface to Telemetry

Author: jhh

src/main/java/org/strykeforce/swerve/SwerveDrive.java

@@ -16,9 +16,11 @@
 import org.jetbrains.annotations.NotNull;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.Registrable;
+import org.strykeforce.telemetry.TelemetryService;
 
 /** Control a Third Coast swerve drive. */
-public class SwerveDrive {
+public class SwerveDrive implements Registrable {
 
   private static final Logger logger = LoggerFactory.getLogger(SwerveDrive.class);
 
@@ -271,4 +273,11 @@ public void setModuleStates(SwerveModuleState[] desiredStates) {
       swerveModules[i].setDesiredState(desiredStates[i], true);
     }
   }
+
+  @Override
+  public void registerWith(@NotNull TelemetryService telemetryService) {
+    for (SwerveModule swerveModule : swerveModules) {
+      swerveModule.registerWith(telemetryService);
+    }
+  }
 }

src/main/java/org/strykeforce/swerve/SwerveModule.java

@@ -2,9 +2,10 @@
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import org.strykeforce.telemetry.Registrable;
 
 /** Represents a Third Coast swerve module. */
-public interface SwerveModule {
+public interface SwerveModule extends Registrable {
 
   /**
    * Gets the maximum attainable speed of the drive.

src/main/java/org/strykeforce/swerve/TalonSwerveModule.java

@@ -12,8 +12,10 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Preferences;
+import org.jetbrains.annotations.NotNull;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
 
 /**
  * A swerve module that uses Talons for azimuth and drive motors. Uses a {@link Builder} to
@@ -201,6 +203,12 @@ private int getWheelIndex() {
     return 3;
   }
 
+  @Override
+  public void registerWith(@NotNull TelemetryService telemetryService) {
+    telemetryService.register(azimuthTalon);
+    telemetryService.register(driveTalon);
+  }
+
   @Override
   public String toString() {
     return "TalonSwerveModule{" + getWheelIndex() + '}';

src/main/kotlin/org/strykeforce/telemetry/Registrable.kt

@@ -0,0 +1,18 @@
+package org.strykeforce.telemetry
+
+/**
+ * A class that has one or more [Measurable] objects that should be registered with the [TelemetryService].
+ *
+ */
+interface Registrable {
+
+    /**
+     * Register this class with a [TelemetryService].
+     *
+     * This method should be called once during robot initialization before the [TelemetryService] is started.
+     *
+     * @param telemetryService the initialized TelemetryService to register with
+     * @throws IllegalStateException if TelemetryService is currently running
+     */
+    fun registerWith(telemetryService: TelemetryService)
+}

src/main/kotlin/org/strykeforce/telemetry/TelemetryService.kt

@@ -2,9 +2,11 @@
 
 package org.strykeforce.telemetry
 
+import com.ctre.phoenix.motorcontrol.can.BaseTalon
 import com.ctre.phoenix.motorcontrol.can.TalonFX
 import com.ctre.phoenix.motorcontrol.can.TalonSRX
 import mu.KotlinLogging
+import org.strykeforce.telemetry.measurable.BaseTalonMeasurable
 import org.strykeforce.telemetry.measurable.Measurable
 import org.strykeforce.telemetry.measurable.TalonSRXMeasurable
 import org.strykeforce.thirdcoast.talon.TalonFXMeasurable
@@ -95,7 +97,17 @@ class TelemetryService(private val telemetryControllerFactory: Function<Inventor
     fun registerAll(collection: Collection<Measurable>) = collection.forEach(this::register)
 
     /**
-     * Convenience method to register a `TalonSRX` for telemetry sending.
+     * Convenience method to register a [BaseTalon] for telemetry sending.
+     *
+     * @param talon the BaseTalon to register for data collection
+     * @throws IllegalStateException if TelemetryService is running.
+     */
+    fun register(talon: BaseTalon) {
+        register(BaseTalonMeasurable(talon))
+    }
+
+    /**
+     * Convenience method to register a [TalonSRX] for telemetry sending.
      *
      * @param talon the TalonSRX to register for data collection
      * @throws IllegalStateException if TelemetryService is running.
@@ -105,7 +117,7 @@ class TelemetryService(private val telemetryControllerFactory: Function<Inventor
     }
 
     /**
-     * Convenience method to register a `TalonFX` for telemetry sending.
+     * Convenience method to register a [TalonFX] for telemetry sending.
      *
      * @param talon the TalonSRX to register for data collection
      * @throws IllegalStateException if TelemetryService is running.

src/main/kotlin/org/strykeforce/telemetry/measurable/BaseTalonMeasurable.kt

@@ -0,0 +1,57 @@
+package org.strykeforce.telemetry.measurable
+
+import com.ctre.phoenix.motorcontrol.can.BaseTalon
+
+/** Represents a [BaseTalon] telemetry-enable [Measurable] item.  */
+class BaseTalonMeasurable @JvmOverloads constructor(
+    private val talon: BaseTalon, override val description: String = "BaseTalon ${talon.deviceID}"
+) : Measurable {
+    override val deviceId = talon.deviceID
+
+    override val measures = setOf(Measure(
+        CLOSED_LOOP_TARGET, "Closed-loop Setpoint (PID 0)"
+    ) { talon.getClosedLoopTarget(0) },
+        Measure(
+            org.strykeforce.thirdcoast.talon.STATOR_CURRENT, "Stator Current"
+        ) { talon.statorCurrent },
+        Measure(
+            org.strykeforce.thirdcoast.talon.SUPPLY_CURRENT, "Supply Current"
+        ) { talon.supplyCurrent },
+        Measure(OUTPUT_VOLTAGE, "Output Voltage") { talon.motorOutputVoltage },
+        Measure(OUTPUT_PERCENT, "Output Percentage") { talon.motorOutputPercent },
+        Measure(SELECTED_SENSOR_POSITION, "Selected Sensor Position (PID 0)") {
+            talon.getSelectedSensorPosition(0)
+        },
+        Measure(SELECTED_SENSOR_VELOCITY, "Selected Sensor Velocity (PID 0)") {
+            talon.getSelectedSensorVelocity(0)
+        },
+        Measure(
+            ACTIVE_TRAJECTORY_POSITION, "Active Trajectory Position"
+        ) { talon.activeTrajectoryPosition },
+        Measure(
+            ACTIVE_TRAJECTORY_VELOCITY, "Active Trajectory Velocity"
+        ) { talon.activeTrajectoryVelocity },
+        Measure(CLOSED_LOOP_ERROR, "Closed Loop Error (PID 0)") {
+            talon.getClosedLoopError(0)
+        },
+        Measure(BUS_VOLTAGE, "Bus Voltage") { talon.busVoltage },
+        Measure(ERROR_DERIVATIVE, "Error Derivative (PID 0)") { talon.getErrorDerivative(0) },
+        Measure(
+            INTEGRAL_ACCUMULATOR, "Integral Accumulator (PID 0)"
+        ) { talon.getIntegralAccumulator(0) },
+        Measure(TEMPERATURE, "Controller Temperature") { talon.temperature })
+
+
+    override fun equals(other: Any?): Boolean {
+        if (this === other) return true
+        if (javaClass != other?.javaClass) return false
+
+        other as BaseTalonMeasurable
+
+        if (deviceId != other.deviceId) return false
+
+        return true
+    }
+
+    override fun hashCode() = deviceId
+}

src/main/kotlin/org/strykeforce/telemetry/measurable/MeasurableSubsystem.kt

@@ -1,8 +1,37 @@
 package org.strykeforce.telemetry.measurable
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase
+import org.strykeforce.telemetry.Registrable
+import org.strykeforce.telemetry.TelemetryService
 
-abstract class MeasurableSubsystem() : SubsystemBase(), Measurable {
+/**
+ * A [Subsystem] that can send telemetry to the Third Coast grapher.
+ *
+ * To implement, override the [Measurable.measures] method, for example:
+ *
+ * <code><pre>
+ * @Override
+ * public Set<Measure> getMeasures() {
+ *     return Set.of(
+ *         new Measure("Gyro Rotation2d (deg)", () -> swerveDrive.getHeading().getDegrees()),
+ *         new Measure("Gyro Angle (deg)", swerveDrive::getGyroAngle),
+ *         new Measure("Odometry X", () -> swerveDrive.getPoseMeters().getX()),
+ *         new Measure("Odometry Y", () -> swerveDrive.getPoseMeters().getY()),
+ *     );
+ * }</pre></code>
+ *
+ * The robot will call [registerWith] during initialization, default behavior is to register this
+ * subsystem with the [TelemetryService]. If you wish to additionally register internal classes with
+ * the telemetry service, override the [registerWith] method, for example:
+ *
+ * <code><pre>
+ * @Override
+ * public void registerWith(TelemetryService: telemetryService) {
+ *     super.registerWith(telemetryService);
+ *     telemetryService.register(talon);
+ * }</pre></code>
+ */
+abstract class MeasurableSubsystem() : SubsystemBase(), Measurable, Registrable {
 
     override val description = name
 
@@ -16,4 +45,6 @@ abstract class MeasurableSubsystem() : SubsystemBase(), Measurable {
         val result = type.compareTo(other.type)
         return if (result != 0) result else deviceId.compareTo(other.deviceId)
     }
+
+    override fun registerWith(telemetryService: TelemetryService) = telemetryService.register(this)
 }