feat: autonomous routines, tuning, vision, oh my

build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2024.3.1"
-    id "com.peterabeles.gversion" version "latest.release"
+    id "com.peterabeles.gversion" version "1.10.2"
 }
 
 java {

src/main/deploy/pathplanner/paths/Go to red speaker.path

@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 12.044476897540559,
+        "y": 2.055995525228976
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 12.646589872786187,
+        "y": 2.276280760074938
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 13.78,
+        "y": 2.58
+      },
+      "prevControl": {
+        "x": 13.043103295508917,
+        "y": 2.511251677243963
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.8,
+      "rotationDegrees": 0.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0,
+    "rotateFast": true
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -3.453579912405768,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/swerve/modules/backleft.json

@@ -15,7 +15,7 @@
     "drive": false,
     "angle": true
   },
-  "absoluteEncoderOffset": 108.369,
+  "absoluteEncoderOffset": 107.842,
   "location": {
     "x": -12.3125,
     "y": 12.3125

src/main/deploy/swerve/modules/backright.json

@@ -15,7 +15,7 @@
     "drive": false,
     "angle": true
   },
-  "absoluteEncoderOffset": 175.342,
+  "absoluteEncoderOffset": 178.330,
   "location": {
     "x": -12.3125,
     "y": -12.3125

src/main/deploy/swerve/modules/frontleft.json

@@ -15,7 +15,7 @@
     "drive": false,
     "angle": true
   },
-  "absoluteEncoderOffset": 250.400,
+  "absoluteEncoderOffset": 250.049,
   "location": {
     "x": 12.3125,
     "y": 12.3125

src/main/deploy/swerve/modules/frontright.json

@@ -15,7 +15,7 @@
     "drive": false,
     "angle": true
   },
-  "absoluteEncoderOffset": 255.850,
+  "absoluteEncoderOffset": 254.092,
   "location": {
     "x": 12.3125,
     "y": -12.3125

src/main/deploy/swerve/modules/physicalproperties.json

@@ -2,15 +2,15 @@
   "optimalVoltage": 12,
   "wheelGripCoefficientOfFriction": 1.19,
   "currentLimit": {
-    "drive": 20,
-    "angle": 10
+    "drive": 40,
+    "angle": 20
   },
   "conversionFactor": {
     "angle": 16.8,
     "drive": 0.047286787200699704
   },
   "rampRate": {
-    "drive": 0.175,
-    "angle": 0.175
+    "drive": 0.25,
+    "angle": 0.25
   }
 }

src/main/java/frc/robot/LocalADStarAK.java

@@ -0,0 +1,149 @@
+package frc.robot;
+
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Translation2d;
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPoint;
+import com.pathplanner.lib.pathfinding.Pathfinder;
+import com.pathplanner.lib.pathfinding.LocalADStar;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+public class LocalADStarAK implements Pathfinder {
+  private final ADStarIO io = new ADStarIO();
+
+  /**
+   * Get if a new path has been calculated since the last time a path was retrieved
+   *
+   * @return True if a new path is available
+   */
+  @Override
+  public boolean isNewPathAvailable() {
+    if (!Logger.hasReplaySource()) {
+      io.updateIsNewPathAvailable();
+    }
+
+    Logger.processInputs("LocalADStarAK", io);
+
+    return io.isNewPathAvailable;
+  }
+
+  /**
+   * Get the most recently calculated path
+   *
+   * @param constraints The path constraints to use when creating the path
+   * @param goalEndState The goal end state to use when creating the path
+   * @return The PathPlannerPath created from the points calculated by the pathfinder
+   */
+  @Override
+  public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) {
+    if (!Logger.hasReplaySource()) {
+      io.updateCurrentPathPoints(constraints, goalEndState);
+    }
+
+    Logger.processInputs("LocalADStarAK", io);
+
+    if (io.currentPathPoints.isEmpty()) {
+      return null;
+    }
+
+    return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState);
+  }
+
+  /**
+   * Set the start position to pathfind from
+   *
+   * @param startPosition Start position on the field. If this is within an obstacle it will be
+   *     moved to the nearest non-obstacle node.
+   */
+  @Override
+  public void setStartPosition(Translation2d startPosition) {
+    if (!Logger.hasReplaySource()) {
+      io.adStar.setStartPosition(startPosition);
+    }
+  }
+
+  /**
+   * Set the goal position to pathfind to
+   *
+   * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
+   *     to the nearest non-obstacle node.
+   */
+  @Override
+  public void setGoalPosition(Translation2d goalPosition) {
+    if (!Logger.hasReplaySource()) {
+      io.adStar.setGoalPosition(goalPosition);
+    }
+  }
+
+  /**
+   * Set the dynamic obstacles that should be avoided while pathfinding.
+   *
+   * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
+   *     opposite corners of a bounding box.
+   * @param currentRobotPos The current position of the robot. This is needed to change the start
+   *     position of the path to properly avoid obstacles
+   */
+  @Override
+  public void setDynamicObstacles(
+      List<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) {
+    if (!Logger.hasReplaySource()) {
+      io.adStar.setDynamicObstacles(obs, currentRobotPos);
+    }
+  }
+
+  private static class ADStarIO implements LoggableInputs {
+    public LocalADStar adStar = new LocalADStar();
+    public boolean isNewPathAvailable = false;
+    public List<PathPoint> currentPathPoints = Collections.emptyList();
+
+    @Override
+    public void toLog(LogTable table) {
+      table.put("IsNewPathAvailable", isNewPathAvailable);
+
+      double[] pointsLogged = new double[currentPathPoints.size() * 2];
+      int idx = 0;
+      for (PathPoint point : currentPathPoints) {
+        pointsLogged[idx] = point.position.getX();
+        pointsLogged[idx + 1] = point.position.getY();
+        idx += 2;
+      }
+
+      table.put("CurrentPathPoints", pointsLogged);
+    }
+
+    @Override
+    public void fromLog(LogTable table) {
+      isNewPathAvailable = table.get("IsNewPathAvailable", false);
+
+      double[] pointsLogged = table.get("CurrentPathPoints", new double[0]);
+
+      List<PathPoint> pathPoints = new ArrayList<>();
+      for (int i = 0; i < pointsLogged.length; i += 2) {
+        pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null));
+      }
+
+      currentPathPoints = pathPoints;
+    }
+
+    public void updateIsNewPathAvailable() {
+      isNewPathAvailable = adStar.isNewPathAvailable();
+    }
+
+    public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) {
+      PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState);
+
+      if (currentPath != null) {
+        currentPathPoints = currentPath.getAllPathPoints();
+      } else {
+        currentPathPoints = Collections.emptyList();
+      }
+    }
+  }
+}

src/main/java/frc/robot/Robot.java

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.pathfinding.Pathfinding;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import org.littletonrobotics.junction.LogFileUtil;
@@ -13,6 +15,7 @@
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 import frc.robot.util.Constants;
+import org.littletonrobotics.urcl.URCL;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -27,7 +30,10 @@ public class Robot extends LoggedRobot {
 
   @Override
   public void robotInit() {
+    DataLogManager.start();
+    URCL.start();
     // Record metadata
+    Pathfinding.setPathfinder(new LocalADStarAK());
     Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
     Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
     Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);