back port from 2024 robot code (#101)

* put tuning auto paths in PathPlanner folders
* adjust PathPlanner defaults for 2024 robot
* add start point auto that sets the robot's pose to the specified position
* run follow path warmup command in robotInit to force loading of PathPlanner's Java classes

* update to latest vendor deps
* display LEDs and DIO ports in the sim GUI
* add workaround from CTRE to ensure that the CANivore has been enumerated by the root hub and therefore, hoot files will be properly generated
* add DEMO_MODE to Constants that slows robot's velocity and displays alternative LED patterns

* add names to all scheduled commands to facilitate debugging

* consolidate management of current alliance color in Field2d singleton

* add ability to override rotation during PathPlanner paths (useful for targeting)
* constrain the robot pose to stay on the field, which doesn't happen when the wheels slip when colliding with the field walls
* reset the robot's pose to the previous pose when the pose "teleports" (i.e., changes much more than possible); why this happens rarely is still not understood but may have something to do with vision pose estimates with invalid timestamps
* add accessor to get measured angular velocity of the drivetrain
* add accessor to get the estimated robot pose in the future based on current velocity
* tuned system test for drivetrain
* add enabled inputs for drive and steer motors
* fix bug where swerve states in inputs were not initialized
* use new initial configs for drie and steer motors in CTRE's SwerveModuleConstantsFactory to specify current limits
* fix bug where closed loop reference and error signals were not associated with current control mode and, therefore, returning invalid values
* use TorqueCurrentFOC for steer control in simulation to match physical robot (CTRE fixed bug preventing this)
* add constants for MK4i L3+
* tune current limits for swerve drive motors
* remove swerve drive characterization autos (need to use separate project with SysId)

* add auto command to characterize swerve wheel diameter based on 6328's clever technique

* remove duplicate auto finished pulse in LEDs class
* remove distraction LED pattern
* add fire LED pattern
* add orange pulse LED pattern
* add support for GRB LED strips connected to roboRIO

* count how many times each camera provides a new estimated pose
* add ambiguity threshold for including estimated poses from vision
* attempt to mitigate impact of NT4 bug where timestamps may be in the future by applying a ceiling of the current time
* only provide a vision-based robot pose if the data is less than 0.5 seconds old
* tune function to calculate standard deviation for vision pose estimates
* add input to capture if the estimate is from seeing multiple tags
* fix bug where multi-tag poses would have an ambiguity of 0, which would result in standard deviations that were too low
* fix simulation bug where the PhotonPoseEstimator needs to be provided with the transform from the camera to the robot
* fix simulation bug where cameras added to the vision system sim don't need to have transform applied

* remove 2023 robot configurations
* add 2024 Artemis robot configurations
* add practice board configuration for testing off-robot with basic electrical board

* add method to determine if the robot has complete left the alliance side of the field (useful in auto)

* change behavior of reset pose to vision to wait until a recent vision pose estimate is available before applying
* allow all physical toggle switches to trigger when disabled (avoids need to force toggle when enabled)

* fix bug in DriveToPose to account for blue field origin

.pathplanner/settings.json

@@ -1,12 +1,16 @@
 {
-  "robotWidth": 0.8382,
-  "robotLength": 0.8382,
+  "robotWidth": 0.88,
+  "robotLength": 0.88,
   "holonomicMode": true,
-  "pathFolders": [],
-  "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 10.0,
-  "defaultMaxAngVel": 561.96,
-  "defaultMaxAngAccel": 2066.2,
-  "maxModuleSpeed": 3.644
+  "pathFolders": [
+    "Tuning"
+  ],
+  "autoFolders": [
+    "Tuning"
+  ],
+  "defaultMaxVel": 3.25,
+  "defaultMaxAccel": 6.948,
+  "defaultMaxAngVel": 530.0,
+  "defaultMaxAngAccel": 2005.0,
+  "maxModuleSpeed": 3.88
 }

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.3.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
     id 'com.diffplug.spotless' version '6.11.0'
     id "com.peterabeles.gversion" version "1.10"
 }

simgui.json

@@ -1,4 +1,19 @@
 {
+  "HALProvider": {
+    "Addressable LEDs": {
+      "0": {
+        "columns": 35
+      },
+      "window": {
+        "visible": true
+      }
+    },
+    "DIO": {
+      "window": {
+        "visible": true
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",

src/main/deploy/pathplanner/autos/ChoreoTest.auto

@@ -20,6 +20,6 @@
       ]
     }
   },
-  "folder": null,
+  "folder": "Tuning",
   "choreoAuto": true
 }

src/main/deploy/pathplanner/autos/DistanceTest.auto

@@ -2,7 +2,7 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 1.82,
+      "x": 2.0,
       "y": 4.4
     },
     "rotation": 0
@@ -20,6 +20,6 @@
       ]
     }
   },
-  "folder": null,
+  "folder": "Tuning",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/autos/Start Point.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2.3661772442028295,
+      "y": 7.31444420234812
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start Point"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Tuning.auto

@@ -20,6 +20,6 @@
       ]
     }
   },
-  "folder": null,
+  "folder": "Tuning",
   "choreoAuto": false
 }

src/main/deploy/pathplanner/paths/DistanceTest.path

@@ -43,7 +43,7 @@
     "rotateFast": false
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Tuning",
   "previewStartingState": {
     "rotation": 0,
     "velocity": 0

src/main/deploy/pathplanner/paths/StartPoint.path → src/main/deploy/pathplanner/paths/Start Point.path

@@ -3,53 +3,50 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 4.48,
-        "y": 1.08
+        "x": 2.3661772442028295,
+        "y": 7.31444420234812
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.6000000000000005,
-        "y": 1.08
+        "x": 2.4661772442028296,
+        "y": 7.31444420234812
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.0,
-        "y": 1.08
+        "x": 3.076223024483964,
+        "y": 7.31444420234812
       },
       "prevControl": {
-        "x": 4.479309367425445,
-        "y": 1.08
+        "x": 2.976223024483964,
+        "y": 7.31444420234812
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": null
     }
   ],
-  "rotationTargets": [
-    {
-      "waypointRelativePos": 0,
-      "rotationDegrees": 0,
-      "rotateFast": false
-    }
-  ],
+  "rotationTargets": [],
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 10.0,
-    "maxAngularVelocity": 561.96,
-    "maxAngularAcceleration": 2066.2
+    "maxVelocity": 1.0,
+    "maxAcceleration": 11.805,
+    "maxAngularVelocity": 537.0,
+    "maxAngularAcceleration": 2303.0
   },
   "goalEndState": {
     "velocity": 0,
     "rotation": 0,
     "rotateFast": false
   },
   "reversed": false,
-  "folder": null,
-  "previewStartingState": null,
-  "useDefaultConstraints": true
+  "folder": "Tuning",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/Tuning.path

@@ -75,7 +75,7 @@
     "rotateFast": false
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Tuning",
   "previewStartingState": {
     "rotation": 90.0,
     "velocity": 0

src/main/java/frc/lib/team3015/subsystem/FaultReporter.java

@@ -1,5 +1,6 @@
 package frc.lib.team3015.subsystem;
 
+import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -42,6 +43,8 @@ private static class SubsystemFaults {
   private final Map<String, SubsystemFaults> subsystemsFaults = new HashMap<>();
   private final boolean checkErrors;
 
+  private boolean startedCTRESignalLogger = false;
+
   private FaultReporter() {
     this.checkErrors = RobotBase.isReal();
     setupCallbacks();
@@ -91,13 +94,15 @@ private void setupCallbacks() {
         .schedule(
             Commands.repeatingSequence(
                     Commands.runOnce(this::checkForFaults), Commands.waitSeconds(0.25))
-                .ignoringDisable(true));
+                .ignoringDisable(true)
+                .withName("check for faults"));
 
     CommandScheduler.getInstance()
         .schedule(
             Commands.repeatingSequence(
                     Commands.runOnce(this::publishStatus), Commands.waitSeconds(1.0))
-                .ignoringDisable(true));
+                .ignoringDisable(true)
+                .withName("publish faults"));
   }
 
   private void publishStatus() {
@@ -185,6 +190,15 @@ public void registerHardware(String subsystemName, String label, TalonFX phoenix
         subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults());
     subsystemFaults.hardware.add(new SelfCheckingPhoenixMotor(label, phoenixMotor));
     subsystemsFaults.put(subsystemName, subsystemFaults);
+
+    // The following is the recommended workaround from CTRE to ensure that the CANivore has been
+    // enumerated by the root hub and therefore, hoot files will be properly generated.
+    if (!this.startedCTRESignalLogger) {
+      this.startedCTRESignalLogger = true;
+      phoenixMotor.getVersion().waitForUpdate(0.5);
+      SignalLogger.setPath("/media/sda1");
+      SignalLogger.start();
+    }
   }
 
   public void registerHardware(String subsystemName, String label, PWMMotorController pwmMotor) {