Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2026-Robot-Code

Auto Align, Auto Hood Angle for 3000 RPM, Camera Poses Working
after portland changes
2026-03-17 19:02:03 -04:00 · 2026-03-17 19:01:02 -04:00 · 2026-03-14 19:11:29 -04:00 · 2026-03-14 15:56:08 -04:00 · 2026-03-14 13:14:19 -04:00 · 2026-03-14 09:46:49 -04:00
30 changed files with 753 additions and 108 deletions
--- a/src/main/deploy/pathplanner/autos/Center
+++ b/src/main/deploy/pathplanner/autos/Center
@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intake down"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "spinup"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center to Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot close"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
--- a/src/main/deploy/pathplanner/autos/Left
+++ b/src/main/deploy/pathplanner/autos/Left
@@ -0,0 +1,67 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intake down"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "spinup"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "start to score left"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot close"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left to Outpost"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop spindexer"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "trough to shot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "spinup"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot N jimmy"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
--- a/src/main/deploy/pathplanner/autos/left
+++ b/src/main/deploy/pathplanner/autos/left
@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intake down"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "left start to center"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
--- a/src/main/deploy/pathplanner/paths/Center
+++ b/src/main/deploy/pathplanner/paths/Center
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.622028469750891,
+        "y": 4.008102016607354
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.266975088967973,
+        "y": 4.0403795966785285
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1271055753262162,
+        "y": 4.008102016607354
+      },
+      "prevControl": {
+        "x": 4.0416370106761565,
+        "y": 4.0403795966785285
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "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
+}
--- a/src/main/deploy/pathplanner/paths/Example
+++ b/src/main/deploy/pathplanner/paths/Example
@@ -33,8 +33,8 @@
  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 720.0,
    "nominalVoltage": 12.0,
--- a/src/main/deploy/pathplanner/paths/Left
+++ b/src/main/deploy/pathplanner/paths/Left
@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.2515736040609142,
+        "y": 4.914375634517767
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.4338749089244973,
+        "y": 5.333984175443034
+      },
+      "isLocked": false,
+      "linkedName": "left close"
+    },
+    {
+      "anchor": {
+        "x": 0.77458883248731,
+        "y": 5.927269035532995
+      },
+      "prevControl": {
+        "x": 2.3777086426889733,
+        "y": 6.110175322602984
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "trough"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.3963599595551063,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 4.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0.09706774519716882,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 178.80651057601818
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -31.15930450834445
+  },
+  "useDefaultConstraints": true
+}
--- a/src/main/deploy/pathplanner/paths/My
+++ b/src/main/deploy/pathplanner/paths/My
@@ -69,7 +69,7 @@
      },
      "prevControl": {
        "x": 8.08578683847011,
-        "y": 0.49077040160283747
+        "y": 0.4907704016028374
      },
      "nextControl": {
        "x": 8.318287488908608,
@@ -130,8 +130,8 @@
  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 720.0,
    "nominalVoltage": 12.0,
--- a/src/main/deploy/pathplanner/paths/Outpost
+++ b/src/main/deploy/pathplanner/paths/Outpost
@@ -33,8 +33,8 @@
  "pointTowardsZones": [],
  "eventMarkers": [],
  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 720.0,
    "nominalVoltage": 12.0,
@@ -42,7 +42,7 @@
  },
  "goalEndState": {
    "velocity": 0,
-    "rotation": 24.304549265936604
+    "rotation": 24.304549265936608
  },
  "reversed": false,
  "folder": "Right Outpost",
--- a/src/main/deploy/pathplanner/paths/Start
+++ b/src/main/deploy/pathplanner/paths/Start
@@ -20,7 +20,7 @@
        "y": 0.668228980317108
      },
      "prevControl": {
-        "x": 1.6987119856944102,
+        "x": 1.6987119856944104,
        "y": 1.0414132379199703
      },
      "nextControl": null,
@@ -45,8 +45,8 @@
    }
  ],
  "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
    "maxAngularVelocity": 540.0,
    "maxAngularAcceleration": 720.0,
    "nominalVoltage": 12.0,
--- a/src/main/deploy/pathplanner/paths/left
+++ b/src/main/deploy/pathplanner/paths/left
@@ -0,0 +1,132 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.573857868020305,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.1594923857868027,
+        "y": 6.424507614213196
+      },
+      "isLocked": false,
+      "linkedName": "start left"
+    },
+    {
+      "anchor": {
+        "x": 2.634629441624366,
+        "y": 5.558944162436549
+      },
+      "prevControl": {
+        "x": 2.4136345177664977,
+        "y": 5.908852791878173
+      },
+      "nextControl": {
+        "x": 2.895419769065523,
+        "y": 5.146026143988051
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.522294416243654,
+        "y": 5.558944162436549
+      },
+      "prevControl": {
+        "x": 3.5095162657293457,
+        "y": 5.533304209258972
+      },
+      "nextControl": {
+        "x": 6.704619289340102,
+        "y": 5.614192893401015
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.478101522849547,
+        "y": 7.244030456855094
+      },
+      "prevControl": {
+        "x": 6.765542310662505,
+        "y": 7.465169522706244
+      },
+      "nextControl": {
+        "x": 8.012172588839395,
+        "y": 7.078284263961693
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.671472081218274,
+        "y": 4.7670456852791885
+      },
+      "prevControl": {
+        "x": 7.726720812182741,
+        "y": 7.639979695431472
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.5636363636363326,
+      "rotationDegrees": -45.0
+    },
+    {
+      "waypointRelativePos": 3.0318181818182266,
+      "rotationDegrees": -90.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 2.6208291203235685,
+      "maxWaypointRelativePos": 4.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -89.09061955080092
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}
--- a/src/main/deploy/pathplanner/paths/start
+++ b/src/main/deploy/pathplanner/paths/start
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.573857868020305,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7991959463121194,
+        "y": 6.260219737341258
+      },
+      "isLocked": false,
+      "linkedName": "start left"
+    },
+    {
+      "anchor": {
+        "x": 3.2515736040609142,
+        "y": 4.914375634517767
+      },
+      "prevControl": {
+        "x": 3.44523908448796,
+        "y": 5.430816915656558
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "left close"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -31.15930450834445
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}
--- a/src/main/deploy/pathplanner/paths/trough
+++ b/src/main/deploy/pathplanner/paths/trough
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.77458883248731,
+        "y": 5.927269035532995
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7745888324873098,
+        "y": 5.927269035532995
+      },
+      "isLocked": false,
+      "linkedName": "trough"
+    },
+    {
+      "anchor": {
+        "x": 3.2515736040609142,
+        "y": 4.914375634517767
+      },
+      "prevControl": {
+        "x": 2.6254213197969554,
+        "y": 5.420822335025381
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "left close"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -31.15930450834445
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 178.80651057601818
+  },
+  "useDefaultConstraints": true
+}
--- a/src/main/deploy/pathplanner/settings.json
+++ b/src/main/deploy/pathplanner/settings.json
@@ -6,8 +6,8 @@
    "Right Outpost"
  ],
  "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
+  "defaultMaxVel": 2.0,
+  "defaultMaxAccel": 1.5,
  "defaultMaxAngVel": 540.0,
  "defaultMaxAngAccel": 720.0,
  "defaultNominalVoltage": 12.0,
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,18 +6,27 @@ package frc.robot;

 import edu.wpi.first.wpilibj.DriverStation.Alliance;

+import java.util.Optional;
 import java.util.OptionalDouble;

 import org.littletonrobotics.junction.Logger;

 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.events.EventTrigger;
+import com.pathplanner.lib.path.EventMarker;

+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -58,6 +67,8 @@ public class RobotContainer {

    private Timer shiftTimer;

+    private boolean resetOdometryToVisualPose;
+
    public RobotContainer() {
        vision = new PhotonVision();
        drivetrain = new Drivetrain(null);
@@ -67,15 +78,24 @@ public class RobotContainer {
        intakeRoller = new IntakeRoller();
        spindexer = new Spindexer();
        //climber = new Climber();
+        configureNamedCommands();

-        
-        vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
+        resetOdometryToVisualPose = false;
+         
+        //vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
        vision.addPoseEstimateConsumer((vp) -> {
            Logger.recordOutput(
                "Vision/" + vp.cameraName() + "/Pose", 
                vp.visualPose()
            );
        });
+        vision.addPoseEstimateConsumer((vp) -> {
+            if(resetOdometryToVisualPose) {
+                drivetrain.resetOdometry(vp.visualPose());
+                resetOdometryToVisualPose = false;
+            }
+        });
+        

        driver = new CommandXboxController(OIConstants.kDriverControllerPort);
        secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
@@ -83,13 +103,13 @@ public class RobotContainer {
        shiftTimer = new Timer();
        shiftTimer.reset();

-        //configureBindings();
-        testConfigureBindings();
+        configureBindings();
+        //testConfigureBindings();
        configureShiftDisplay();

        if(AutoConstants.kAutoConfigOk) {
            autoChooser = AutoBuilder.buildAutoChooser();
-            configureNamedCommands();
+            SmartDashboard.putData("Auto Chooser", autoChooser);
        }
    }

@@ -245,8 +265,16 @@ public class RobotContainer {
                () -> true
            )
        );
+        shooter.setDefaultCommand(shooter.stop());
+        intakeRoller.setDefaultCommand(intakeRoller.stop());
+        spindexer.setDefaultCommand(spindexer.stop());

-        driver.a().whileTrue(
+        intakePivot.setDefaultCommand(intakePivot.manualSpeed(() -> secondary.getLeftY()));
+
+        driver.a().onTrue(new InstantCommand(() -> resetOdometryToVisualPose = true));
+        driver.y().whileTrue(drivetrain.zeroHeading());
+
+        driver.leftTrigger().whileTrue(
            drivetrain.lockRotationToHub(
                driver::getLeftY, 
                driver::getLeftX, 
@@ -254,8 +282,12 @@ public class RobotContainer {
            ) 
        );

-        /*
-        driver.b().whileTrue(
+        driver.leftBumper().whileTrue(intakeRoller.runOut());
+        driver.rightBumper().whileTrue(intakeRoller.runIn());
+
+        driver.rightTrigger().whileTrue(spindexer.spinToShooter());
+        driver.b().whileTrue(spindexer.spinToIntake());
+       /*  driver.b().whileTrue(
            drivetrain.lockToYaw(
                () -> {
                    OptionalDouble maybeYaw = vision.getBestYawForTag(Utilities.getHubCenterAprilTagID());
@@ -267,30 +299,39 @@ public class RobotContainer {
            )
        );*/

-        shooter.setDefaultCommand(
-            shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
-        );
+        secondary.a().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
+        secondary.x().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));

+        //hood.setDefaultCommand(hood.trackToAngle(() -> Units.degreesToRadians(MathUtil.clamp(hoodAngle, 0, 40))));
+        //secondary.y().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(40)));
+        //40 good for feeding
+        //secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
+        //30 degrees good for shooter far near outpost
+        secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
+        //10 degrees good for shooting ~33in away from hub
+
+        
        hood.setDefaultCommand(hood.trackToAngle(() -> {
            Pose2d drivetrainPose = drivetrain.getPose();
            Pose2d hubPose = Utilities.getHubPose();

            double distance = drivetrainPose.getTranslation()
-                .plus(CompetitionConstants.kRobotToShooter.getTranslation().toTranslation2d())
                .getDistance(hubPose.getTranslation());
+
+            Logger.recordOutput("Hood/DistanceToHub", distance);
+
+            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
                
-            if(HoodConstants.kUseInterpolatorForAngle) {
-                return HoodConstants.kDistanceToAngle.get(distance);
+            if(currentSpeeds.isPresent()) {
+                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
+
+                if(map != null) {
+                    return MathUtil.clamp(map.get(distance), 0, 40);
+                } else {
+                    return 0;
+                }
            } else {
-                // TODO The average actual speeds isn't <i>really</i> the exit velocity of the ball
-                // on a hooded shooter, based on documentation, it's more like 30-50% depending on
-                // hood material, surface friction, etc.
-                return Utilities.shotAngle(
-                    shooter.getAverageActualSpeeds(), 
-                    distance, 
-                    CompetitionConstants.kHubGoalHeightMeters - ShooterConstants.kShooterHeightMeters, 
-                    false
-                );
+                return 0;
            }
        }));
    }
@@ -308,14 +349,52 @@ public class RobotContainer {
                false // TODO Should this be true by default?
            )
        );
+
+        NamedCommands.registerCommand(
+            "intake down", 
+            intakePivot.manualSpeed(()->0.75)
+                .withTimeout(1)
+        );
+
+        NamedCommands.registerCommand("spinup", 
+            shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
+                .withTimeout(3));
+
+        NamedCommands.registerCommand("shoot close", 
+            spindexer.spinToShooter()
+                .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed))
+                .alongWith(hood.trackToAngle(() -> Units.degreesToRadians(10)))
+                .withTimeout(3).andThen(spindexer.instantaneousStop()));
+        
+       // NamedCommands.registerCommand("Intake Start", intakeRoller.runIn());
+
+        new EventTrigger("Intake Start")
+            .onTrue(intakeRoller.runIn());
+
+        NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop());
+
+        NamedCommands.registerCommand("jimmy",
+            Commands.repeatingSequence(
+                    intakePivot.manualSpeed(() -> -0.75).withTimeout(0.2)
+                    .andThen(intakePivot.manualSpeed(() -> 0.75).withTimeout(0.2))
+                )
+            );
+
+        NamedCommands.registerCommand("shoot N jimmy", 
+            Commands.parallel(
+                Commands.repeatingSequence(
+                    intakePivot.manualSpeed(() -> -0.75).withTimeout(0.5),
+                    intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5)
+                ),
+                spindexer.spinToShooter()
+                    .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed),
+                               hood.trackToAngle(() -> Units.degreesToRadians(10)))
+                
+            ).withTimeout(3).andThen(spindexer.instantaneousStop()));
    }

    public Command getAutonomousCommand() {
-        if(AutoConstants.kAutoConfigOk) {
-            return autoChooser.getSelected();
-        } else {
-            return new PrintCommand("Robot Config loading failed, autonomous disabled");
-        }
+        return autoChooser.getSelected();
    }

    /**
--- a/src/main/java/frc/robot/constants/AutoConstants.java
+++ b/src/main/java/frc/robot/constants/AutoConstants.java
@@ -24,8 +24,6 @@ public class AutoConstants {
    public static final double kPXYController = 3.5;
    public static final double kPThetaController = 5;

-    public static final double kYawPIDTolerance = Units.degreesToRadians(2);
-
    public static final double kAlignPXYController = 2;
    public static final double kAlignPThetaController = 5;

--- a/src/main/java/frc/robot/constants/DrivetrainConstants.java
+++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java
@@ -38,9 +38,10 @@ public class DrivetrainConstants {
    public static final boolean kGyroReversed = true;

    // TODO Hold over from 2025, adjust?
-    public static final double kHeadingP = .1;
+    public static final double kHeadingP = .65;
    public static final double kXTranslationP = .5;
    public static final double kYTranslationP = .5;
+    public static final double kYawPIDTolerance = Units.degreesToRadians(1);

    // TODO How much do we trust gyro and encoders vs vision estimates.
    // NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same
--- a/src/main/java/frc/robot/constants/HoodConstants.java
+++ b/src/main/java/frc/robot/constants/HoodConstants.java
@@ -5,6 +5,7 @@ import java.io.File;
 import java.io.FileReader;
 import java.io.IOException;
 import java.nio.file.Path;
+import java.util.Map;

 import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
@@ -12,7 +13,9 @@ import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;

 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Filesystem;
+import frc.robot.constants.ShooterConstants.ShooterSpeeds;

 public class HoodConstants {
    // TODO Real Values
@@ -43,9 +46,9 @@ public class HoodConstants {

    public static final IdleMode kIdleMode = IdleMode.kBrake;

-    // TODO This needs to be filled in from some source
-    public static final InterpolatingDoubleTreeMap kDistanceToAngle = new InterpolatingDoubleTreeMap();
-
+    public static final Map<ShooterSpeeds, InterpolatingDoubleTreeMap> kHoodInterpolators = Map.of(
+        ShooterSpeeds.kHubSpeed, new InterpolatingDoubleTreeMap()
+    );
    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM

    public static final SparkMaxConfig kConfig = new SparkMaxConfig();
@@ -66,27 +69,24 @@ public class HoodConstants {
            .feedForward
                .sva(kS, kV, kA);

+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 40)), 
+            Double.valueOf(Units.degreesToRadians(10)));

-        File interpolatorFile = Path.of(
-            Filesystem.getDeployDirectory().getAbsolutePath().toString(), 
-            "interpolatorData.csv"
-        ).toFile();
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 60)), 
+            Double.valueOf(Units.degreesToRadians(13)));

-        if(interpolatorFile.exists()) {
-            try (BufferedReader reader = new BufferedReader(new FileReader(interpolatorFile))) {
-                reader.lines().forEach((s) -> {
-                    if(s.trim() != "") { //Empty or whitespace line protection
-                        String[] lineSplit = s.split(",");
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 80)),
+            Double.valueOf(Units.degreesToRadians(17)));

-                        kDistanceToAngle.put(
-                            Double.valueOf(lineSplit[0].replace("\"", "")), 
-                            Double.valueOf(lineSplit[1].replace("\"", ""))
-                        );
-                    }
-                });
-            } catch (IOException e) {
-                // This condition is never reached because of the if exists line above
-            }
-        }
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 100)), 
+            Double.valueOf(Units.degreesToRadians(21)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 120)), 
+            Double.valueOf(Units.degreesToRadians(24)));
    }
 }
--- a/src/main/java/frc/robot/constants/IntakePivotConstants.java
+++ b/src/main/java/frc/robot/constants/IntakePivotConstants.java
@@ -51,7 +51,7 @@ public class IntakePivotConstants {
        KLeftMotorConfig
            .idleMode(kIdleMode)
            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors);
+            .inverted(false);
        KLeftMotorConfig.encoder
            .positionConversionFactor(kConversionFactor)
            .velocityConversionFactor(kConversionFactor / 60);
@@ -67,7 +67,7 @@ public class IntakePivotConstants {
        kRightMotorConfig
            .idleMode(kIdleMode)
            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors)
-            .follow(kLeftMotorCANID);
+            .inverted(true)
+            ;//.follow(kLeftMotorCANID);
    }
 }
--- a/src/main/java/frc/robot/constants/IntakeRollerConstants.java
+++ b/src/main/java/frc/robot/constants/IntakeRollerConstants.java
@@ -5,24 +5,32 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;

 public class IntakeRollerConstants {
    // TODO Real values
-    public static final int kMotorCANID = 20;
+    public static final int kRightMotorCANID = 20;
+    public static final int kLeftMotorCANID = 1;

-    public static final int kCurrentLimit = 40;
+    public static final int kCurrentLimit = 65;

-    public static final boolean kInvertMotors = true;
+    public static final boolean kInvertLeftMotor = false;
+    public static final boolean kInvertRightMotor = true;

-    public static final double kSpeed = .6;
+    public static final double kSpeed = 1;

    public static final IdleMode kIdleMode = IdleMode.kCoast;

    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM

    public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig();

    static {
        leftMotorConfig
            .idleMode(kIdleMode)
            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors);
+            .inverted(kInvertLeftMotor);
+        rightMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kInvertRightMotor)
+        ;
    }
 }
--- a/src/main/java/frc/robot/constants/ModuleConstants.java
+++ b/src/main/java/frc/robot/constants/ModuleConstants.java
@@ -1,6 +1,7 @@
 package frc.robot.constants;

 import com.ctre.phoenix6.configs.AudioConfigs;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
@@ -34,7 +35,7 @@ public class ModuleConstants {
    }

    // DRIVING MOTOR CONFIG (Kraken)
-    public static final double kDrivingMotorReduction = (14.0 * 28.0 * 15.0) / (50 * 16 * 45);
+    public static final double kDrivingMotorReduction = (50 * 16 * 45)/(14.0 * 28.0 * 15.0);

    public static final double kDrivingMotorFeedSpeedRPS = KrakenMotorConstants.kFreeSpeedRPM / 60;
    public static final double kWheelDiameterMeters = Units.inchesToMeters(4);
@@ -45,16 +46,17 @@ public class ModuleConstants {
    public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRPS;
    
    // TODO Hold over from 2025, adjust?
-    public static final double kDriveP = .04;
+    public static final double kDriveP = .06;
    public static final double kDriveI = 0;
    public static final double kDriveD = 0;
    public static final double kDriveS = 0;
    public static final double kDriveV = kDrivingVelocityFeedForward;
    public static final double kDriveA = 0;
+    public static final double kClosedLoopRampRate = .01;

    // TODO Hold over from 2025, adjust?
-    public static final int kDriveMotorStatorCurrentLimit = 100;
-    public static final int kDriveMotorSupplyCurrentLimit = 65;
+    public static final int kDriveMotorStatorCurrentLimit = 90;
+    public static final int kDriveMotorSupplyCurrentLimit = 55;

    // TODO Hold over from 2025, adjust?
    public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
@@ -88,6 +90,7 @@ public class ModuleConstants {
    public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
    public static final AudioConfigs kAudioConfig = new AudioConfigs();
    public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
+    public static final ClosedLoopRampsConfigs kDriveClosedLoopRampConfig = new ClosedLoopRampsConfigs();

    static {
        kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
@@ -109,6 +112,8 @@ public class ModuleConstants {
        kDriveSlot0Config.kV = kDriveV;
        kDriveSlot0Config.kA = kDriveA;

+        kDriveClosedLoopRampConfig.withVoltageClosedLoopRampPeriod(kClosedLoopRampRate);
+
        turningConfig
            .idleMode(kTurnIdleMode)
            .smartCurrentLimit(kTurnMotorCurrentLimit)
--- a/src/main/java/frc/robot/constants/PhotonConstants.java
+++ b/src/main/java/frc/robot/constants/PhotonConstants.java
@@ -8,19 +8,28 @@ import edu.wpi.first.math.util.Units;
 import frc.robot.utilities.PhotonVisionConfig;

 public class PhotonConstants {
-    public static final String kCamera1Name = "pv1";
-    public static final String kCamera2Name = "pv2";
+    public static final String kCamera1Name = "CameraPV1";
+    public static final String kCamera2Name = "CameraPV2";

    // TODO Need actual values for all of this
-    public static final Transform3d kCamera1RobotToCam = new Transform3d();
+    public static final Transform3d kCamera1RobotToCam = new Transform3d(
+        Units.inchesToMeters(1.5),
+        Units.inchesToMeters(-8.5),
+        Units.inchesToMeters(28.5),
+        new Rotation3d(
+            Units.degreesToRadians(0), 
+            Units.degreesToRadians(-24.0), 
+            Units.degreesToRadians(30.0)
+        )
+    );
    public static final Transform3d kCamera2RobotToCam = new Transform3d(
        Units.inchesToMeters(1.5),
        Units.inchesToMeters(-10.5),
        Units.inchesToMeters(28.5),
        new Rotation3d(
-            Units.degreesToRadians(0), 
-            Units.degreesToRadians(24), 
-            Units.degreesToRadians(-10)
+            Units.degreesToRadians(0.0), 
+            Units.degreesToRadians(-24.0), 
+            Units.degreesToRadians(-10.0)
        )
    );

@@ -32,7 +41,7 @@ public class PhotonConstants {
    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM

    public static final List<PhotonVisionConfig> configs = List.of(
-        //new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
+        new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
        new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians)
    );
 }
--- a/src/main/java/frc/robot/constants/ShooterConstants.java
+++ b/src/main/java/frc/robot/constants/ShooterConstants.java
@@ -9,7 +9,8 @@ import edu.wpi.first.math.util.Units;
 public class ShooterConstants {
    public enum ShooterSpeeds {
        kHubSpeed(3000.0),
-        kFeedSpeed(5000.0);
+        kFeedSpeed(5000.0),
+        kIdleSpeed(750.0);

        private double speedMPS;
        private double speedRPM;
--- a/src/main/java/frc/robot/constants/SpindexerConstants.java
+++ b/src/main/java/frc/robot/constants/SpindexerConstants.java
@@ -12,14 +12,14 @@ public class SpindexerConstants {
    public static final int kSpindexerMotorCANID = 0;
    public static final int kFeederMotorCANID = 4;

-    public static final int kSpindexerStatorCurrentLimit = 110;
-    public static final int kSpindexerSupplyCurrentLimit = 60;
-    public static final int kFeederCurrentLimit = 40;
+    public static final int kSpindexerStatorCurrentLimit = 95;
+    public static final int kSpindexerSupplyCurrentLimit = 50;
+    public static final int kFeederCurrentLimit = 30;

    public static final double kSpindexerSpeed = 1;
    public static final double kFeederSpeed = 1;

-    public static final boolean kFeederMotorInverted = true;
+    public static final boolean kFeederMotorInverted = false;

    public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive;
    public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast;
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -82,12 +82,12 @@ public class Drivetrain extends SubsystemBase {
        gyro = new AHRS(NavXComType.kMXP_SPI);

        yawRotationController = new PIDController(
-            AutoConstants.kPThetaController, 
+            DrivetrainConstants.kHeadingP, 
            0, 
            0
        );
        yawRotationController.enableContinuousInput(-Math.PI, Math.PI);
-        yawRotationController.setTolerance(AutoConstants.kYawPIDTolerance);
+        yawRotationController.setTolerance(DrivetrainConstants.kYawPIDTolerance);

        // TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future
        estimator = new SwerveDrivePoseEstimator(
@@ -144,6 +144,7 @@ public class Drivetrain extends SubsystemBase {
        Logger.recordOutput("Drivetrain/Pose", getPose());
        Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
        Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
+        Logger.recordOutput("Drivetrain/Velocity", getCurrentChassisSpeeds());
    }

    /**
@@ -261,10 +262,17 @@ public class Drivetrain extends SubsystemBase {
                        targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
                    }

-                    return yawRotationController.calculate(
+                    Logger.recordOutput("/HubAutoAlign/CurrentHeader", getHeading().getRadians());
+                    Logger.recordOutput("/HubAutoAlign/Setpoint", targetRotation.getRadians());
+
+                    double outputPower = -yawRotationController.calculate(
                        getHeading().getRadians(),
                        targetRotation.getRadians()
                    );
+
+                    Logger.recordOutput("/HubAutoAlign/OutputPower", outputPower);
+
+                    return outputPower;
                }, 
                () -> true
            )
@@ -331,6 +339,10 @@ public class Drivetrain extends SubsystemBase {
    }

    public void consumeVisualPose(VisualPose pose) {
+        if(Math.abs(pose.visualPose().minus(getPose()).getTranslation().getNorm()) > 1) {
+            return;
+        }
+        
        estimator.addVisionMeasurement(
            pose.visualPose(), 
            pose.timestamp()
@@ -375,7 +387,8 @@ public class Drivetrain extends SubsystemBase {
        SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
            fieldRelative ?
                ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, 
-                    estimator.getEstimatedPosition().getRotation()) :
+                    //estimator.getEstimatedPosition().getRotation()) :
+                    Rotation2d.fromDegrees(getGyroValue())) :
                new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)   
        );

--- a/src/main/java/frc/robot/subsystems/Hood.java
+++ b/src/main/java/frc/robot/subsystems/Hood.java
@@ -26,10 +26,10 @@ public class Hood extends SubsystemBase {

    private SparkClosedLoopController controller;

-    private Trigger resetTrigger;
-    private Trigger timerTrigger;
+    //private Trigger resetTrigger;
+    //private Trigger timerTrigger;

-    private Timer resetTimer;
+    //private Timer resetTimer;

    private double currentTargetDegrees;

@@ -47,7 +47,7 @@ public class Hood extends SubsystemBase {

        controller = motor.getClosedLoopController();

-        resetTimer = new Timer();
+        /*resetTimer = new Timer();
        resetTimer.reset();

        resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset));
@@ -61,7 +61,7 @@ public class Hood extends SubsystemBase {
        timerTrigger.onTrue(new InstantCommand(() -> {
            encoder.setPosition(0);
            resetTimer.reset();
-        }));
+        }));*/

        currentTargetDegrees = HoodConstants.kStartupAngle;
    }
@@ -96,7 +96,7 @@ public class Hood extends SubsystemBase {
     * 
     * @return A complete Command structure that performs the specified action
     */
-    public Command automatedRezero() {
+    /*public Command automatedRezero() {
        return manualSpeed(() -> -1)
            .until(timerTrigger)
            .andThen(
@@ -112,11 +112,11 @@ public class Hood extends SubsystemBase {
     * 
     * @return A complete Command structure that performs the specified action
     */
-    public Command automatedRezeroNoTimer() {
+    /*public Command automatedRezeroNoTimer() {
        return manualSpeed(() -> -1)
            .until(() -> motor.getOutputCurrent() >= HoodConstants.kAmpsToTriggerPositionReset)
            .andThen(new InstantCommand(() -> encoder.setPosition(0)));
-    }
+    }*/

    public Command manualSpeed(DoubleSupplier speed) {
        currentTargetDegrees = 0;
--- a/src/main/java/frc/robot/subsystems/IntakePivot.java
+++ b/src/main/java/frc/robot/subsystems/IntakePivot.java
@@ -76,9 +76,17 @@ public class IntakePivot extends SubsystemBase {
            currentTargetPosition = null;
            
            leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
+            rightMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
        });
    }

+    // public Command jimmy(){
+    //     return run(() -> {
+    //         leftMotor.set(0.5 * IntakePivotConstants.kMaxManualSpeedMultiplier);
+    //         rightMotor.set(0.5 * IntakePivotConstants.kMaxManualSpeedMultiplier);
+    //     })
+    // }
+
    public Command stop() {
        return manualSpeed(() -> 0);
    }
--- a/src/main/java/frc/robot/subsystems/IntakeRoller.java
+++ b/src/main/java/frc/robot/subsystems/IntakeRoller.java
@@ -10,33 +10,44 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.IntakeRollerConstants;

 public class IntakeRoller extends SubsystemBase {
-    private SparkMax motor;
+    private SparkMax leftMotor;
+    private SparkMax rightMotor;

    public IntakeRoller() {
-        motor = new SparkMax(IntakeRollerConstants.kMotorCANID, MotorType.kBrushless);
+        leftMotor = new SparkMax(IntakeRollerConstants.kLeftMotorCANID, MotorType.kBrushless);
+        rightMotor = new SparkMax(IntakeRollerConstants.kRightMotorCANID, MotorType.kBrushless);

-        motor.configure(
+        leftMotor.configure(
            IntakeRollerConstants.leftMotorConfig, 
            ResetMode.kResetSafeParameters, 
            PersistMode.kPersistParameters
        );
+
+        rightMotor.configure(
+            IntakeRollerConstants.rightMotorConfig,
+            ResetMode.kResetSafeParameters,
+            PersistMode.kPersistParameters
+        );
    }

    public Command runIn() {
        return run(() -> {
-            motor.set(IntakeRollerConstants.kSpeed);
+            leftMotor.set(IntakeRollerConstants.kSpeed*0.8);
+            rightMotor.set(IntakeRollerConstants.kSpeed*0.8);
        });
    }

    public Command runOut() {
        return run(() -> {
-            motor.set(-IntakeRollerConstants.kSpeed);
+            leftMotor.set(-IntakeRollerConstants.kSpeed);
+            rightMotor.set(-IntakeRollerConstants.kSpeed);
        });
    }

    public Command stop() {
        return run(() -> {
-            motor.set(0);
+            leftMotor.set(0);
+            rightMotor.set(0);
        });
    }
    
--- a/src/main/java/frc/robot/subsystems/PhotonVision.java
+++ b/src/main/java/frc/robot/subsystems/PhotonVision.java
@@ -13,6 +13,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;

+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
--- a/src/main/java/frc/robot/subsystems/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/Spindexer.java
@@ -59,5 +59,12 @@ public class Spindexer extends SubsystemBase {
            feederMotor.set(0);
        });
    }
+
+    public Command instantaneousStop() {
+        return runOnce(() -> {
+            spindexerMotor.setControl(spindexerMotorOutput.withOutput(0));
+            feederMotor.set(0);
+        });
+    }
    
 }
--- a/src/main/java/frc/robot/utilities/SwerveModule.java
+++ b/src/main/java/frc/robot/utilities/SwerveModule.java
@@ -109,6 +109,7 @@ public class SwerveModule {
        drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
        drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
        drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+        drive.getConfigurator().apply(ModuleConstants.kDriveClosedLoopRampConfig);

        turning.configure(
            ModuleConstants.turningConfig,
Author	SHA1	Message	Date
Bradley Bickford	d29acde2df	Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2026-Robot-Code	2026-03-17 19:02:03 -04:00
Bradley Bickford	048c7158ee	Auto Align, Auto Hood Angle for 3000 RPM, Camera Poses Working	2026-03-17 19:01:02 -04:00
Tylr-J42	a8833aaf5b	after portland changes	2026-03-14 19:11:29 -04:00
Tylr-J42	47606ade0f	pose localization trouble	2026-03-14 15:56:08 -04:00
Bradley Bickford	a6dca0925f	Intake Roller Changes	2026-03-14 13:14:19 -04:00
Tylr-J42	72a07b3d7a	second intake motor	2026-03-14 09:46:49 -04:00
Tylr-J42	5e1eadf887	end of pine tree 2	2026-03-08 19:24:00 -04:00
Tylr-J42	21c0421a88	end of pine tree	2026-03-08 19:20:07 -04:00
Tylr-J42	81d6c36436	drive train testing	2026-03-06 00:39:39 -05:00
Bradley Bickford	f1f523de73	Changes from 3/5 meeting	2026-03-05 19:05:09 -05:00
Bradley Bickford	10fb8d4aa5	Putting the Auto Chooser somewhere in networktables	2026-03-05 17:07:19 -05:00
Tylr-J42	77f2c54a90	more bindings n stuff	2026-03-05 00:28:37 -05:00
Tylr-J42	4171da889f	button bindings	2026-03-04 13:31:23 -05:00