Upstream changes (from AdvantageKit AdvancedSwerveTemplate)

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

@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7266603914356558,
+      "y": 4.424143586134395
+    },
+    "rotation": -58.38161458548115
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "StartGroup"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "FloorPickupPosition"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RtoF5"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "F5toR"
+          }
+        }
+      ]
+    }
+  },
+  "folder": "RealAutons",
+  "choreoAuto": false
+}

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

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 9.565143110103929,
+        "y": 0.7532360903118802
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.912542240632302,
+        "y": 0.9993287790409061
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.7735997987753405,
+        "y": 4.452689378292487
+      },
+      "prevControl": {
+        "x": 2.0431707616188,
+        "y": 1.708662069599849
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -57.443119948220044,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "MoveOutPaths",
+  "previewStartingState": {
+    "rotation": 0.20403802540067167,
+    "velocity": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Point1-3.path

@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 2.2304,
+        "x": 2.23,
         "y": 4.4638
       },
       "prevControl": {
-        "x": 2.2304,
+        "x": 2.23,
         "y": 4.8138
       },
       "nextControl": null,

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

@@ -0,0 +1,64 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7433436901379014,
+        "y": 4.422060029558428
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7214164587111003,
+        "y": 2.0183655192252523
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 9.737110990063995,
+        "y": 0.7516327121051241
+      },
+      "prevControl": {
+        "x": 6.046549120876021,
+        "y": 0.7276489610820663
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "New Constraints Zone",
+      "minWaypointRelativePos": 0,
+      "maxWaypointRelativePos": 0.15,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 0.1,
+        "maxAngularAcceleration": 0.1
+      }
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "MoveOutPaths",
+  "previewStartingState": {
+    "rotation": -61.11046206336895,
+    "velocity": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/frc/robot/RobotContainer.java

@@ -23,10 +23,10 @@
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.ControlCommands.ArmCommands;
 import frc.robot.commands.ControlCommands.DriveCommands;
 import frc.robot.commands.ControlCommands.IntakeShooterControls;
-import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.commands.IntakeNote;
 import frc.robot.commands.IntakeNoteAndAlign;
 import frc.robot.commands.Positions.AmpShot;
@@ -43,6 +43,10 @@
 import frc.robot.subsystems.arm.ArmIO;
 import frc.robot.subsystems.arm.ArmIOSim;
 import frc.robot.subsystems.arm.ArmIOSparkMax;
+import frc.robot.subsystems.climber.Climber;
+import frc.robot.subsystems.climber.ClimberIO;
+import frc.robot.subsystems.climber.ClimberIOSim;
+import frc.robot.subsystems.climber.ClimberIOSparkMax;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIONavX;
@@ -75,6 +79,7 @@ public class RobotContainer {
   private final Flywheel sFlywheel;
   private final Intake sIntake;
   private final Arm sArm;
+  private final Climber sClimber;
 
   // Controllers
   private final CommandXboxController driverController = new CommandXboxController(0);
@@ -96,14 +101,15 @@ public RobotContainer() {
         // Real robot, instantiate hardware IO implementations
         sDrive =
             new Drive(
-                new GyroIONavX(false),
+                new GyroIONavX(),
                 new ModuleIOSparkMax(0),
                 new ModuleIOSparkMax(1),
                 new ModuleIOSparkMax(2),
                 new ModuleIOSparkMax(3));
         sFlywheel = new Flywheel(new FlywheelIOSparkMax());
         sIntake = new Intake(new IntakeIOSparkMax() {}, new ProximitySensorIOV3() {});
         sArm = new Arm(new ArmIOSparkMax() {});
+        sClimber = new Climber(new ClimberIOSparkMax() {});
 
         break;
 
@@ -121,6 +127,7 @@ public RobotContainer() {
         sFlywheel = new Flywheel(new FlywheelIOSim());
         sIntake = new Intake(new IntakeIOSim() {}, new ProximitySensorIOV3() {});
         sArm = new Arm(new ArmIOSim() {});
+        sClimber = new Climber(new ClimberIOSim() {});
 
         break;
 
@@ -136,6 +143,7 @@ public RobotContainer() {
         sFlywheel = new Flywheel(new FlywheelIO() {});
         sIntake = new Intake(new IntakeIO() {}, new ProximitySensorIOV3() {});
         sArm = new Arm(new ArmIO() {});
+        sClimber = new Climber(new ClimberIO() {});
 
         break;
     }
@@ -241,6 +249,7 @@ public RobotContainer() {
 
           // Right Alternate Group
           "RCloseOut",
+          "RFar1S5",
 
           // Test Autos
           "SimpleStraight",
@@ -260,15 +269,66 @@ public RobotContainer() {
     options.forEach(auto -> chooser.addOption(auto.getName(), auto));
     autoChooser = new LoggedDashboardChooser<>("Auto Choices", chooser);
 
-    // Set up feedforward characterization
+    // Set up SysId routines
+    // Drive subsystem
     autoChooser.addOption(
-        "Drive FF Characterization",
-        new FeedForwardCharacterization(
-            sDrive, sDrive::runCharacterizationVolts, sDrive::getCharacterizationVelocity));
+        "Drive SysId (Quasistatic Forward)",
+        sDrive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
     autoChooser.addOption(
-        "Flywheel FF Characterization",
-        new FeedForwardCharacterization(
-            sFlywheel, sFlywheel::runVolts, sFlywheel::getCharacterizationVelocity));
+        "Drive SysId (Quasistatic Reverse)",
+        sDrive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    autoChooser.addOption(
+        "Drive SysId (Dynamic Forward)", sDrive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Drive SysId (Dynamic Reverse)", sDrive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+    // Flywheel subsystem
+    autoChooser.addOption(
+        "Flywheel SysId (Quasistatic Forward)",
+        sFlywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Flywheel SysId (Quasistatic Reverse)",
+        sFlywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    autoChooser.addOption(
+        "Flywheel SysId (Dynamic Forward)",
+        sFlywheel.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Flywheel SysId (Dynamic Reverse)",
+        sFlywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+    // Climber subsystem
+    autoChooser.addOption(
+        "Climber SysId (Quasistatic Forward)",
+        sClimber.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Climber SysId (Quasistatic Reverse)",
+        sClimber.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    autoChooser.addOption(
+        "Climber SysId (Dynamic Forward)", sClimber.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Climber SysId (Dynamic Reverse)", sClimber.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+    // Arm subsystem
+    autoChooser.addOption(
+        "Arm SysId (Quasistatic Forward)", sArm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Arm SysId (Quasistatic Reverse)", sArm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    autoChooser.addOption(
+        "Arm SysId (Dynamic Forward)", sArm.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Arm SysId (Dynamic Reverse)", sArm.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+    // Intake subsystem
+    autoChooser.addOption(
+        "Intake SysId (Quasistatic Forward)",
+        sIntake.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Intake SysId (Quasistatic Reverse)",
+        sIntake.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    autoChooser.addOption(
+        "Intake SysId (Dynamic Forward)", sIntake.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoChooser.addOption(
+        "Intake SysId (Dynamic Reverse)", sIntake.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
     // Configure the button bindings
     configureButtonBindings();
@@ -293,6 +353,12 @@ private void configureButtonBindings() {
 
     driverController.start().onTrue(Commands.runOnce(() -> sDrive.resetRotation()));
 
+    operatorController
+        .start()
+        .whileTrue(Commands.startEnd(() -> sIntake.runFull(), sIntake::stop, sIntake));
+
+    driverController.rightBumper().whileTrue(Commands.runOnce(() -> sDrive.toggleSlowMode()));
+
     // ! TEST <
     driverController.a().whileTrue(sDrive.getZeroAuton());
     // !TEST >

src/main/java/frc/robot/commands/ControlCommands/DriveCommands.java

@@ -18,6 +18,8 @@
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.drive.Drive;
@@ -43,6 +45,10 @@ public static Command joystickDrive(
           double omega;
           boolean SlowMode = drive.isSlowMode();
 
+          boolean isFlipped =
+              DriverStation.getAlliance().isPresent()
+                  && DriverStation.getAlliance().get() == Alliance.Red;
+
           // Apply deadband and slowMode
           if (!SlowMode) {
             linearMagnitude =