Move swerve telemetry registration to telemetry pkg

Author: jhh

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

@@ -3,14 +3,15 @@
 import com.kauailabs.navx.frc.AHRS;
 import com.moandjiezana.toml.Toml;
 import edu.wpi.first.wpilibj.Preferences;
-import javax.inject.Inject;
-import javax.inject.Singleton;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 import org.strykeforce.thirdcoast.talon.Errors;
 import org.strykeforce.thirdcoast.talon.TalonConfiguration;
 import org.strykeforce.thirdcoast.util.Settings;
 
+import javax.inject.Inject;
+import javax.inject.Singleton;
+
 /**
  * Control a Third Coast swerve drive.
  *
@@ -220,26 +221,6 @@ public void zeroAzimuthEncoders() {
     if (errorCount > 0) logger.error("TalonSRX set azimuth zero error count = {}", errorCount);
   }
 
-  /**
-   * Register the swerve wheel azimuth and drive {@link com.ctre.phoenix.motorcontrol.can.TalonSRX}
-   * with the Telemetry service for data collection. The Telemetry service will set the Talon status
-   * frame update rates to default values during registration.
-   *
-   * @param telemetryService the active Telemetry service instance created by the robot
-   */
-  //  public void registerWith(TelemetryService telemetryService) {
-  //    for (int i = 0; i < WHEEL_COUNT; i++) {
-  //      TalonSRX t = wheels[i].getAzimuthTalon();
-  //      if (t != null)
-  //        telemetryService.register(
-  //            new TalonItem(t, "Azimuth Talon " + i + " (" + t.getDeviceID() + ")"));
-  //      t = wheels[i].getDriveTalon();
-  //      if (t != null)
-  //        telemetryService.register(
-  //            new TalonItem(t, "Drive Talon " + i + " (" + t.getDeviceID() + ")"));
-  //    }
-  //  }
-
   /**
    * Returns the four wheels of the swerve drive.
    *

telemetry/src/main/java/org/strykeforce/thirdcoast/telemetry/TelemetryService.java

@@ -1,19 +1,18 @@
 package org.strykeforce.thirdcoast.telemetry;
 
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
-import java.util.Collection;
-import java.util.Collections;
-import java.util.LinkedHashSet;
-import java.util.Optional;
-import java.util.Set;
-import javax.inject.Inject;
-import javax.inject.Singleton;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.thirdcoast.swerve.SwerveDrive;
+import org.strykeforce.thirdcoast.swerve.Wheel;
 import org.strykeforce.thirdcoast.talon.config.StatusFrameRate;
 import org.strykeforce.thirdcoast.telemetry.item.Item;
 import org.strykeforce.thirdcoast.telemetry.item.TalonItem;
 
+import javax.inject.Inject;
+import javax.inject.Singleton;
+import java.util.*;
+
 /**
  * The Telemetry service registers {@link Item} instances for data collection and controls the
  * starting and stopping of the service. When active, the services listens for incoming config
@@ -79,17 +78,6 @@ public void clear() {
     logger.info("item set was cleared");
   }
 
-  /**
-   * Convenience method to register a TalonSRX for telemetry sending.
-   *
-   * @param talon the TalonSRX to register for data collection
-   * @throws IllegalStateException if TelemetryService is running.
-   * @see StatusFrameRate
-   */
-  public void register(TalonSRX talon) {
-    register(new TalonItem(talon));
-  }
-
   /**
    * Registers an Item for telemetry sending.
    *
@@ -117,6 +105,31 @@ public void registerAll(Collection<Item> collection) {
     logger.info("registered all: {}", collection);
   }
 
+  /**
+   * Convenience method to register a {@code TalonSRX} for telemetry sending.
+   *
+   * @param talon the TalonSRX to register for data collection
+   * @throws IllegalStateException if TelemetryService is running.
+   * @see StatusFrameRate
+   */
+  public void register(TalonSRX talon) {
+    register(new TalonItem(talon));
+  }
+
+  /**
+   * Convenience method to register a {@code SwerveDrive} for telemetry sending.
+   *
+   * @param swerveDrive the SwerveDrive to register for data collection
+   * @throws IllegalStateException if TelemetryService is running.
+   * @see StatusFrameRate
+   */
+  public void register(SwerveDrive swerveDrive) {
+    for (Wheel wheel : swerveDrive.getWheels()) {
+      register(new TalonItem(wheel.getAzimuthTalon()));
+      register(new TalonItem(wheel.getDriveTalon()));
+    }
+  }
+
   /**
    * Configure the Talon with the given ID with the given status frame rates.
    *