Pull in SDS lib

build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2023.4.3"
-    id 'com.diffplug.spotless' version '6.12.0'
+    // id 'com.diffplug.spotless' version '6.12.0'
 }
 
 sourceCompatibility = JavaVersion.VERSION_11
@@ -99,45 +99,45 @@ tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
 }
 
-spotless {
-    java {
-        target fileTree('.') {
-            include '**/*.java'
-            exclude '**/build/**', '**/build-*/**'
-        }
-        toggleOffOn()
-        googleJavaFormat()
-        removeUnusedImports()
-        trimTrailingWhitespace()
-        endWithNewline()
-    }
-    groovyGradle {
-        target fileTree('.') {
-            include '**/*.gradle'
-            exclude '**/build/**', '**/build-*/**'
-        }
-        greclipse()
-        indentWithSpaces(4)
-        trimTrailingWhitespace()
-        endWithNewline()
-    }
-    format 'xml', {
-        target fileTree('.') {
-            include '**/*.xml'
-            exclude '**/build/**', '**/build-*/**'
-        }
-        eclipseWtp('xml')
-        trimTrailingWhitespace()
-        indentWithSpaces(2)
-        endWithNewline()
-    }
-    format 'misc', {
-        target fileTree('.') {
-            include '**/*.md', '**/.gitignore'
-            exclude '**/build/**', '**/build-*/**'
-        }
-        trimTrailingWhitespace()
-        indentWithSpaces(2)
-        endWithNewline()
-    }
-}
+// spotless {
+//     java {
+//         target fileTree('.') {
+//             include '**/*.java'
+//             exclude '**/build/**', '**/build-*/**'
+//         }
+//         toggleOffOn()
+//         googleJavaFormat()
+//         removeUnusedImports()
+//         trimTrailingWhitespace()
+//         endWithNewline()
+//     }
+//     groovyGradle {
+//         target fileTree('.') {
+//             include '**/*.gradle'
+//             exclude '**/build/**', '**/build-*/**'
+//         }
+//         greclipse()
+//         indentWithSpaces(4)
+//         trimTrailingWhitespace()
+//         endWithNewline()
+//     }
+//     format 'xml', {
+//         target fileTree('.') {
+//             include '**/*.xml'
+//             exclude '**/build/**', '**/build-*/**'
+//         }
+//         eclipseWtp('xml')
+//         trimTrailingWhitespace()
+//         indentWithSpaces(2)
+//         endWithNewline()
+//     }
+//     format 'misc', {
+//         target fileTree('.') {
+//             include '**/*.md', '**/.gitignore'
+//             exclude '**/build/**', '**/build-*/**'
+//         }
+//         trimTrailingWhitespace()
+//         indentWithSpaces(2)
+//         endWithNewline()
+//     }
+// }

src/main/java/com/swervedrivespecialties/swervelib/AbsoluteEncoder.java

@@ -0,0 +1,20 @@
+package com.swervedrivespecialties.swervelib;
+
+@FunctionalInterface
+public interface AbsoluteEncoder {
+    /**
+     * Gets the current angle reading of the encoder in radians.
+     *
+     * @return The current angle in radians. Range: [0, 2pi)
+     */
+    double getAbsoluteAngle();
+
+    /**
+     * Returns the internal encoder object, if applicable
+     * 
+     * @return The internal encoder object.
+     */
+    default Object getInternal() {
+        return null;
+    }
+}

src/main/java/com/swervedrivespecialties/swervelib/AbsoluteEncoderFactory.java

@@ -0,0 +1,6 @@
+package com.swervedrivespecialties.swervelib;
+
+@FunctionalInterface
+public interface AbsoluteEncoderFactory<Configuration> {
+    AbsoluteEncoder create(Configuration configuration);
+}

src/main/java/com/swervedrivespecialties/swervelib/DriveController.java

@@ -0,0 +1,11 @@
+package com.swervedrivespecialties.swervelib;
+
+public interface DriveController {
+    Object getDriveMotor();
+
+    void setReferenceVoltage(double voltage);
+
+    double getStateVelocity();
+
+    double getStateDistance();
+}

src/main/java/com/swervedrivespecialties/swervelib/DriveControllerFactory.java

@@ -0,0 +1,45 @@
+package com.swervedrivespecialties.swervelib;
+
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer;
+
+@FunctionalInterface
+public interface DriveControllerFactory<Controller extends DriveController, DriveConfiguration> {
+    default void addDashboardEntries(
+            ShuffleboardContainer container,
+            Controller controller
+    ) {
+        container.addNumber("Current Velocity", controller::getStateVelocity);
+    }
+
+    default Controller create(
+            ShuffleboardContainer container,
+            DriveConfiguration driveConfiguration,
+            String canbus,
+            MechanicalConfiguration mechConfiguration
+    ) {
+        var controller = create(driveConfiguration, canbus, mechConfiguration);
+        addDashboardEntries(container, controller);
+
+        return controller;
+    }
+
+    default Controller create(
+            ShuffleboardContainer container,
+            DriveConfiguration driveConfiguration,
+            MechanicalConfiguration mechConfiguration
+    ) {
+        var controller = create(driveConfiguration, mechConfiguration);
+        addDashboardEntries(container, controller);
+
+        return controller;
+    }
+
+    default Controller create(
+        DriveConfiguration driveConfiguration,
+        MechanicalConfiguration mechConfiguration
+    ) {
+        return create(driveConfiguration, "", mechConfiguration);
+    }
+
+    Controller create(DriveConfiguration driveConfiguration, String canbus, MechanicalConfiguration mechConfiguration);
+}

src/main/java/com/swervedrivespecialties/swervelib/MechanicalConfiguration.java

@@ -0,0 +1,114 @@
+package com.swervedrivespecialties.swervelib;
+
+import java.util.Objects;
+
+/**
+ * A swerve module configuration.
+ * <p>
+ * A configuration represents a unique mechanical configuration of a module. For example, the Swerve Drive Specialties
+ * Mk3 swerve module has two configurations, standard and fast, and therefore should have two configurations
+ * ({@link SdsModuleConfigurations#MK3_STANDARD} and {@link SdsModuleConfigurations#MK3_FAST} respectively).
+ */
+public class MechanicalConfiguration {
+    private final double wheelDiameter;
+    private final double driveReduction;
+    private final boolean driveInverted;
+
+    private final double steerReduction;
+    private final boolean steerInverted;
+
+    /**
+     * Creates a new module configuration.
+     *
+     * @param wheelDiameter  The diameter of the module's wheel in meters.
+     * @param driveReduction The overall drive reduction of the module. Multiplying motor rotations by this value
+     *                       should result in wheel rotations.
+     * @param driveInverted  Whether the drive motor should be inverted. If there is an odd number of gea reductions
+     *                       this is typically true.
+     * @param steerReduction The overall steer reduction of the module. Multiplying motor rotations by this value
+     *                       should result in rotations of the steering pulley.
+     * @param steerInverted  Whether the steer motor should be inverted. If there is an odd number of gear reductions
+     *                       this is typically true.
+     */
+    public MechanicalConfiguration(double wheelDiameter, double driveReduction, boolean driveInverted,
+                               double steerReduction, boolean steerInverted) {
+        this.wheelDiameter = wheelDiameter;
+        this.driveReduction = driveReduction;
+        this.driveInverted = driveInverted;
+        this.steerReduction = steerReduction;
+        this.steerInverted = steerInverted;
+    }
+
+    /**
+     * Gets the diameter of the wheel in meters.
+     */
+    public double getWheelDiameter() {
+        return wheelDiameter;
+    }
+
+    /**
+     * Gets the overall reduction of the drive system.
+     * <p>
+     * If this value is multiplied by drive motor rotations the result would be drive wheel rotations.
+     */
+    public double getDriveReduction() {
+        return driveReduction;
+    }
+
+    /**
+     * Gets if the drive motor should be inverted.
+     */
+    public boolean isDriveInverted() {
+        return driveInverted;
+    }
+
+    /**
+     * Gets the overall reduction of the steer system.
+     * <p>
+     * If this value is multiplied by steering motor rotations the result would be steering pulley rotations.
+     */
+    public double getSteerReduction() {
+        return steerReduction;
+    }
+
+    /**
+     * Gets if the steering motor should be inverted.
+     */
+    public boolean isSteerInverted() {
+        return steerInverted;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        MechanicalConfiguration that = (MechanicalConfiguration) o;
+        return Double.compare(that.getWheelDiameter(), getWheelDiameter()) == 0 &&
+                Double.compare(that.getDriveReduction(), getDriveReduction()) == 0 &&
+                isDriveInverted() == that.isDriveInverted() &&
+                Double.compare(that.getSteerReduction(), getSteerReduction()) == 0 &&
+                isSteerInverted() == that.isSteerInverted();
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(
+                getWheelDiameter(),
+                getDriveReduction(),
+                isDriveInverted(),
+                getSteerReduction(),
+                isSteerInverted()
+        );
+    }
+
+    @Override
+    public String toString() {
+        return "MechanicalConfiguration{" +
+                "wheelDiameter=" + wheelDiameter +
+                ", driveReduction=" + driveReduction +
+                ", driveInverted=" + driveInverted +
+                ", steerReduction=" + steerReduction +
+                ", steerInverted=" + steerInverted +
+                '}';
+    }
+}

src/main/java/com/swervedrivespecialties/swervelib/Mk3ModuleConfiguration.java

@@ -0,0 +1,64 @@
+package com.swervedrivespecialties.swervelib;
+
+import java.util.Objects;
+
+/**
+ * Additional Mk3 module configuration parameters.
+ * <p>
+ * The configuration parameters here are used to customize the behavior of the Mk3 swerve module.
+ * Each setting is initialized to a default that should be adequate for most use cases.
+ * 
+ * @deprecated use {@link MkModuleConfiguration} instead, which provides support for CANivores.
+ */
+@Deprecated(since = "2023.1.2.0", forRemoval = true)
+public class Mk3ModuleConfiguration {
+    private double nominalVoltage = 12.0;
+    private double driveCurrentLimit = 80.0;
+    private double steerCurrentLimit = 20.0;
+
+    public double getNominalVoltage() {
+        return nominalVoltage;
+    }
+
+    public void setNominalVoltage(double nominalVoltage) {
+        this.nominalVoltage = nominalVoltage;
+    }
+
+    public double getDriveCurrentLimit() {
+        return driveCurrentLimit;
+    }
+
+    public void setDriveCurrentLimit(double driveCurrentLimit) {
+        this.driveCurrentLimit = driveCurrentLimit;
+    }
+
+    public double getSteerCurrentLimit() {
+        return steerCurrentLimit;
+    }
+
+    public void setSteerCurrentLimit(double steerCurrentLimit) {
+        this.steerCurrentLimit = steerCurrentLimit;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        Mk3ModuleConfiguration that = (Mk3ModuleConfiguration) o;
+        return Double.compare(that.getNominalVoltage(), getNominalVoltage()) == 0 && Double.compare(that.getDriveCurrentLimit(), getDriveCurrentLimit()) == 0 && Double.compare(that.getSteerCurrentLimit(), getSteerCurrentLimit()) == 0;
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(getNominalVoltage(), getDriveCurrentLimit(), getSteerCurrentLimit());
+    }
+
+    @Override
+    public String toString() {
+        return "Mk3ModuleConfiguration{" +
+                "nominalVoltage=" + nominalVoltage +
+                ", driveCurrentLimit=" + driveCurrentLimit +
+                ", steerCurrentLimit=" + steerCurrentLimit +
+                '}';
+    }
+}