Final comp changes.

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

@@ -0,0 +1,174 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.77,
+      "y": 6.66
+    },
+    "rotation": 60.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "ShootSubwooferShot"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 10.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Amp Start to Amp Spike"
+          }
+        },
+        {
+          "type": "race",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 2.0
+                }
+              },
+              {
+                "type": "deadline",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "IntakeUntilNoteDetected"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "AutoOrientToSpeaker"
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "SetShooterContinuous"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 2.0
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ShootAtCurrentRPM"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Amp Spike to Amp Note"
+          }
+        },
+        {
+          "type": "race",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 2.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeUntilNoteDetectedNoAutoCentering"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Amp Note to Amp Farshot"
+          }
+        },
+        {
+          "type": "race",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 2.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "ShootAmpFarShot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Amp Farshot to Mid Amp Note"
+          }
+        },
+        {
+          "type": "race",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeUntilNoteDetectedNoAutoCentering"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Mid Amp Note To Amp Farshot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ShootAmpFarShot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -23,6 +24,7 @@ public class Robot extends TimedRobot {
 
   private Command autonomousCommand;
   PowerDistribution powerDistribution = new PowerDistribution(1, ModuleType.kRev);
+  private Timer gcTimer = new Timer();
 
   private RobotContainer robotContainer;
 
@@ -40,6 +42,7 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     robotContainer = new RobotContainer();
+    gcTimer.start();
   }
 
   /**
@@ -57,6 +60,9 @@ public void robotPeriodic() {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+    if (gcTimer.advanceIfElapsed(5)) {
+      System.gc();
+    }
   }
 
   /** This function is called once each time the robot enters Disabled mode. */

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -36,13 +36,15 @@ public class ClimberSubsystem extends SubsystemBase {
 
   private final StatusSignal<Double> rightEncoder = winchRightMotor.getPosition();
   private double currentPosition;
+  private double positionOffset;
 
   /** Creates a new ClimberSubsystem. */
   public ClimberSubsystem() {
     winchLeftMotor.setInverted(true);
     winchRightMotor.setInverted(false);
     winchLeftMotor.setNeutralMode(NeutralModeValue.Brake);
     winchRightMotor.setNeutralMode(NeutralModeValue.Brake);
+    resetCurrentPosition();
 
     // TalonFXConfiguration config = new TalonFXConfiguration();
     // config.CurrentLimits =
@@ -70,7 +72,11 @@ public void stopMotors() {
   }
 
   public double getCurrentPosition() {
-    return currentPosition;
+    return currentPosition - positionOffset;
+  }
+
+  public void resetCurrentPosition() {
+    positionOffset = rightEncoder.refresh().getValueAsDouble();
   }
 
   @Override