Swerve

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Main",
+      "request": "launch",
+      "mainClass": "frc.robot.Main",
+      "projectName": "beta"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

build.gradle

@@ -33,7 +33,7 @@ deploy {
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
-                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
+                    deleteOldFiles = true // Change to true to delete files on roboRIO that no
                                            // longer exist in deploy directory of this project
                 }
             }

src/main/deploy/pathplanner/navgrid.json

@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}

src/main/deploy/pathplanner/paths/Example Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 40.0,
+  "robotMOI": 2.0,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.0508,
+  "driveGearing": 6.122,
+  "maxDriveSpeed": 4.0,
+  "driveMotorType": "krakenX60FOC",
+  "driveCurrentLimit": 20.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}

src/main/java/frc/FSLib2025/chassis/OnboardModuleState.java

@@ -0,0 +1,60 @@
+package frc.FSLib2025.chassis;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public class OnboardModuleState {
+
+  /**
+   * Minimize the change in heading the desired swerve module state would require
+   * by potentially
+   * reversing the direction the wheel spins. Customized from WPILib's version to
+   * include placing in
+   * appropriate scope for CTRE and REV onboard control as both controllers as of
+   * writing don't have
+   * support for continuous input.
+   *
+   * @param desiredState The desired state.
+   * @param currentAngle The current module angle.
+   */
+  public static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
+    double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
+    double targetSpeed = desiredState.speedMetersPerSecond;
+    double delta = targetAngle - currentAngle.getDegrees();
+    if (Math.abs(delta) > 90) {
+      targetSpeed = -targetSpeed;
+      targetAngle = delta > 90 ? (targetAngle -= 180) : (targetAngle += 180);
+    }
+    return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
+  }
+
+  /**
+   * @param scopeReference Current Angle
+   * @param newAngle       Target Angle
+   * @return Closest angle within scope
+   */
+  private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
+    double lowerBound;
+    double upperBound;
+    double lowerOffset = scopeReference % 360;
+    if (lowerOffset >= 0) {
+      lowerBound = scopeReference - lowerOffset;
+      upperBound = scopeReference + (360 - lowerOffset);
+    } else {
+      upperBound = scopeReference - lowerOffset;
+      lowerBound = scopeReference - (360 + lowerOffset);
+    }
+    while (newAngle < lowerBound) {
+      newAngle += 360;
+    }
+    while (newAngle > upperBound) {
+      newAngle -= 360;
+    }
+    if (newAngle - scopeReference > 180) {
+      newAngle -= 360;
+    } else if (newAngle - scopeReference < -180) {
+      newAngle += 360;
+    }
+    return newAngle;
+  }
+}

src/main/java/frc/FSLib2025/chassis/SwerveModuleConfig.java

@@ -0,0 +1,18 @@
+package frc.FSLib2025.chassis;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class SwerveModuleConfig {
+
+    public int DriveMotorId;
+    public int AngleMotorId;
+    public int CancoderId;
+    public Rotation2d AngleOffset;
+
+    public SwerveModuleConfig (int driveMotorId, int angleMotorId, int cancoderId, Rotation2d angleOffset) {
+        this.DriveMotorId = driveMotorId;
+        this.AngleMotorId = angleMotorId;
+        this.CancoderId = cancoderId;
+        this.AngleOffset = angleOffset;
+    }
+}

src/main/java/frc/FSLib2025/chassis/SwerveModuleConstants.java

@@ -0,0 +1,8 @@
+package frc.FSLib2025.chassis;
+
+public class SwerveModuleConstants {
+    public int DriveMotorId;
+    public int SteerMotorId;
+    public int CANcoderId;
+    public double CANcoderOffset;
+}

src/main/java/frc/FSLib2025/control/Feedforward.java

@@ -0,0 +1,25 @@
+package frc.FSLib2025.control;
+
+public class Feedforward {
+    private double kA, kV, kP_Pos, kP_Vel, kS;
+
+    public Feedforward (double ikV, double ikA, double ikP_Pos, double ikP_Vel, double ikS){
+        kV = ikV;
+        kA = ikA;
+        kP_Pos = ikP_Pos;
+        kP_Vel = ikP_Vel;
+        kS = ikS;
+    }
+
+    public double calculate(double vel, double accel, double posError, double velError){
+        return (kV * vel + kA * accel + kP_Pos * posError + kP_Vel * velError + kS * Math.signum(vel));
+    }
+
+    public void setGains(double ikV, double ikA, double ikP_Pos, double ikP_Vel, double ikS){
+        kV = ikV;
+        kA = ikA;
+        kP_Pos = ikP_Pos;
+        kP_Vel = ikP_Vel;
+        kS = ikS;
+    }
+}

src/main/java/frc/FSLib2025/control/PID.java

@@ -0,0 +1,34 @@
+package frc.FSLib2025.control;
+
+public class PID {
+    private double lastError = 0, d = 0, i = 0;
+    double output;
+    private double kP, kI, kD, windup, limit;    
+
+    public PID(double ikP, double ikI, double ikD, double iwindup, double ilimit){
+        kP = ikP;
+        kI = ikI;
+        kD = ikD;
+        windup = iwindup;
+        limit = ilimit;
+    }
+
+    public PID(double ikP, double ikI, double ikD){
+        this(ikP, ikI, ikD, 0, 0);
+    }
+
+    public double calculate (double currentValue, double target) {
+        return calculate(target - currentValue);
+    }
+
+    public double calculate (double error){
+        i = (Math.abs(error) <= windup) ? i += error : 0;
+        double iOut = i * kI;
+        iOut = (iOut >= limit) ? limit : iOut;
+        iOut = (iOut <= -limit) ? -limit : iOut;
+        d = error - lastError;
+        lastError = error;
+        output = (error * kP) + iOut + (d * kD);
+        return output;
+    }
+}

src/main/java/frc/FSLib2025/math/FS_Math.java

@@ -0,0 +1,21 @@
+package frc.FSLib2025.math;
+
+public class FS_Math {
+
+    public static boolean isWithin (double value, double min, double max) {
+        return Math.max(min, value) == Math.min(value, max);
+    }
+
+    public static double clamp (double value, double min, double max) {
+        return Math.min(Math.max(value, min), max);
+    }
+
+    public static double clamp (int value, int min, int max) {
+        return Math.min(Math.max(value, min), max);
+    }
+
+    public static double constrainAngleDegrees(double angle) {
+        return Math.atan2(Math.sin(angle / 180.0 * Math.PI), Math.cos(angle / 180.0 * Math.PI)) * 180 / Math.PI;
+    }
+
+}

src/main/java/frc/FSLib2025/math/LinearRegression.java

@@ -0,0 +1,22 @@
+package frc.FSLib2025.math;
+
+public class LinearRegression {
+
+    private double[][] data;
+
+    public LinearRegression (double[][] mapValues) {
+        data = mapValues;
+    }
+
+    public double calculate (double xValues) {
+        int index = 0;
+        for (double[] i : data) {
+            if(i[0] >= xValues) break;
+            index++;
+        }
+        double dx = xValues - data[index-1][0];
+        double x = data[index][0] - data[index-1][0];
+        return data[index-1][1] * (1-dx/x) + data[index][1] * dx/x;
+    }
+
+}

src/main/java/frc/FSLib2025/math/PolynomialRegression.java

@@ -0,0 +1,65 @@
+package frc.FSLib2025.math;
+
+public class PolynomialRegression {
+
+    private double[] coefficients;
+
+    public PolynomialRegression (double[] xValues, double[] yValues, int degree) {
+        coefficients = fitPolynomial(xValues, yValues, degree);
+    }
+
+    public double predictDeg(double xValue) {
+        return evaluatePolynomial(coefficients, xValue);
+    }
+
+    private double[] fitPolynomial(double[] x, double[] y, int degree) {
+        int n = x.length;
+        double[][] matrix = new double[degree + 1][degree + 2];
+        double[] result = new double[degree + 1];
+
+        for (int i = 0; i <= degree; i++) {
+            for (int j = 0; j <= degree; j++) {
+                matrix[i][j] = 0;
+                for (int k = 0; k < n; k++) {
+                    matrix[i][j] += Math.pow(x[k], i + j);
+                }
+            }
+            matrix[i][degree + 1] = 0;
+            for (int k = 0; k < n; k++) {
+                matrix[i][degree + 1] += Math.pow(x[k], i) * y[k];
+            }
+        }
+
+        for (int i = 0; i <= degree; i++) {
+            for (int k = i + 1; k <= degree; k++) {
+                if (Math.abs(matrix[i][i]) < Math.abs(matrix[k][i])) {
+                    double[] temp = matrix[i];
+                    matrix[i] = matrix[k];
+                    matrix[k] = temp;
+                }
+            }
+            for (int k = i + 1; k <= degree; k++) {
+                double t = matrix[k][i] / matrix[i][i];
+                for (int j = 0; j <= degree + 1; j++) {
+                    matrix[k][j] -= t * matrix[i][j];
+                }
+            }
+        }
+        for (int i = degree; i >= 0; i--) {
+            result[i] = matrix[i][degree + 1] / matrix[i][i];
+            for (int k = 0; k < i; k++) {
+                matrix[k][degree + 1] -= matrix[k][i] * result[i];
+            }
+        }
+
+        return result;
+    }
+
+    private double evaluatePolynomial(double[] coefficients, double x) {
+        double result = 0;
+        for (int i = coefficients.length - 1; i >= 0; i--) {
+            result = result * x + coefficients[i];
+        }
+        return result;
+    }
+}