Add graphable swerve drive

Author: jhh

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

@@ -0,0 +1,84 @@
+package org.strykeforce.thirdcoast.swerve;
+
+import static org.strykeforce.thirdcoast.telemetry.grapher.Measure.ANGLE;
+import static org.strykeforce.thirdcoast.telemetry.grapher.Measure.GYRO_YAW;
+
+import com.kauailabs.navx.frc.AHRS;
+import com.squareup.moshi.JsonWriter;
+import java.io.IOException;
+import java.util.Collections;
+import java.util.EnumSet;
+import java.util.Set;
+import java.util.function.DoubleSupplier;
+import javax.inject.Inject;
+import org.jetbrains.annotations.NotNull;
+import org.strykeforce.thirdcoast.telemetry.TelemetryService;
+import org.strykeforce.thirdcoast.telemetry.grapher.Measure;
+import org.strykeforce.thirdcoast.telemetry.item.Item;
+
+public class GraphableSwerveDrive extends SwerveDrive implements Item {
+
+  private static final String TYPE = "swerve";
+  private static final String DESCRIPTION = "Swerve Drive";
+
+  private static final Set<Measure> MEASURES =
+      Collections.unmodifiableSet(EnumSet.of(ANGLE, GYRO_YAW));
+
+  @Inject
+  public GraphableSwerveDrive(AHRS gyro, Wheel[] wheels) {
+    super(gyro, wheels);
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    super.registerWith(telemetryService);
+    telemetryService.register(this);
+  }
+
+  @Override
+  public int deviceId() {
+    return 0;
+  }
+
+  @Override
+  public String type() {
+    return TYPE;
+  }
+
+  @Override
+  public String description() {
+    return DESCRIPTION;
+  }
+
+  @Override
+  public Set<Measure> measures() {
+    return MEASURES;
+  }
+
+  @Override
+  public DoubleSupplier measurementFor(Measure measure) {
+    if (!MEASURES.contains(measure)) {
+      throw new IllegalArgumentException("invalid measure: " + measure.name());
+    }
+    switch (measure) {
+      case ANGLE:
+        return gyro::getAngle;
+      case GYRO_YAW:
+        return gyro::getYaw;
+      default:
+        throw new AssertionError(measure);
+    }
+  }
+
+  @Override
+  public void toJson(JsonWriter writer) throws IOException {
+    writer.beginObject();
+    writer.name("not").value("implemented");
+    writer.endObject();
+  }
+
+  @Override
+  public int compareTo(@NotNull Item o) {
+    return 0;
+  }
+}

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

@@ -0,0 +1,15 @@
+package org.strykeforce.thirdcoast.swerve;
+
+import dagger.Module;
+import dagger.Provides;
+import javax.inject.Singleton;
+
+@Module
+public class GraphableSwerveDriveModule {
+
+  @Provides
+  @Singleton
+  public static SwerveDrive swerveDrive(GraphableSwerveDrive swerveDrive) {
+    return swerveDrive;
+  }
+}

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

@@ -31,7 +31,7 @@ public class SwerveDrive {
   static final Logger logger = LoggerFactory.getLogger(SwerveDrive.class);
   private static final int WHEEL_COUNT = 4;
   private final Wheel[] wheels;
-  private final AHRS gyro;
+  final AHRS gyro;
 
   @Inject
   SwerveDrive(AHRS gyro, Wheel[] wheels) {

core/src/main/java/org/strykeforce/thirdcoast/telemetry/grapher/Measure.java

@@ -24,7 +24,8 @@ public enum Measure {
   MOMAGIC_ACCL("Motion Magic Acceleration"),
   MOMAGIC_A_TRAJ_POS("Motion Magic Trajectory Point Target Position"),
   MOMAGIC_A_TRAJ_VEL("Motion Magic Trajectory Point Target Velocity"),
-  MOMAGIC_CRUISE_VEL("Motion Magic Cruise Velocity");
+  MOMAGIC_CRUISE_VEL("Motion Magic Cruise Velocity"),
+  GYRO_YAW("Gyro Yaw");
 
   //  private final static Map<Item.Type, Set<Measure>> byType = new HashMap<>();
   //

robot/src/main/java/org/team2767/thirdcoast/RobotComponent.java

@@ -4,6 +4,7 @@
 import dagger.Component;
 import java.io.File;
 import javax.inject.Singleton;
+import org.strykeforce.thirdcoast.swerve.GraphableSwerveDriveModule;
 import org.strykeforce.thirdcoast.swerve.GyroModule;
 import org.strykeforce.thirdcoast.swerve.SwerveDrive;
 import org.strykeforce.thirdcoast.swerve.WheelModule;
@@ -17,6 +18,7 @@
   modules = {
     GyroModule.class,
     WheelModule.class,
+    GraphableSwerveDriveModule.class,
   }
 )
 interface RobotComponent {