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

src/main/deploy/pathplanner/autos/Left Shoot.auto

@@ -27,6 +27,36 @@
           "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"
+          }
         }
       ]
     }

src/main/deploy/pathplanner/autos/left to center.auto

@@ -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
+}

src/main/deploy/pathplanner/paths/Center to Shoot.path

@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/Example Path.path

@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/Left to Outpost.path

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.9872360616844604,
+        "x": 3.2515736040609142,
-        "y": 4.599857651245552
+        "y": 4.914375634517767
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.1695373665480435,
+        "x": 2.4338749089244973,
-        "y": 5.019466192170819
+        "y": 5.333984175443034
       },
       "isLocked": false,
       "linkedName": "left close"
     },
     {
       "anchor": {
-        "x": 1.1043772241992889,
+        "x": 0.77458883248731,
-        "y": 5.815646500593119
+        "y": 5.927269035532995
       },
       "prevControl": {
-        "x": 2.7074970344009497,
+        "x": 2.3777086426889733,
-        "y": 5.998552787663108
+        "y": 6.110175322602984
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "trough"
     }
   ],
   "rotationTargets": [
@@ -34,12 +34,33 @@
       "rotationDegrees": 180.0
     }
   ],
-  "constraintZones": [],
+  "constraintZones": [
-  "pointTowardsZones": [],
+    {
-  "eventMarkers": [],
+      "name": "Constraints Zone",
-  "globalConstraints": {
+      "minWaypointRelativePos": 0.3963599595551063,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
         "maxVelocity": 3.0,
-    "maxAcceleration": 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,
@@ -47,13 +68,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 178.53119928561412
+    "rotation": 178.80651057601818
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -22.619864948040433
+    "rotation": -31.15930450834445
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path

@@ -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,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/Outpost to Ranged Shot.path

@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.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",

src/main/deploy/pathplanner/paths/Start to outpost.path

@@ -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,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/left start to center.path

@@ -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
+}

src/main/deploy/pathplanner/paths/start to score left.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.697342823250297,
+        "x": 3.573857868020305,
-        "y": 6.0
+        "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.922680901542112,
+        "x": 2.7991959463121194,
-        "y": 5.890960854092527
+        "y": 6.260219737341258
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "start left"
     },
     {
       "anchor": {
-        "x": 2.9872360616844604,
+        "x": 3.2515736040609142,
-        "y": 4.599857651245552
+        "y": 4.914375634517767
       },
       "prevControl": {
-        "x": 3.1809015421115063,
+        "x": 3.44523908448796,
-        "y": 5.116298932384343
+        "y": 5.430816915656558
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -22.619864948040433
+    "rotation": -31.15930450834445
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -90.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/trough to shot.path

@@ -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
+}

src/main/deploy/pathplanner/settings.json

@@ -6,8 +6,8 @@
     "Right Outpost"
   ],
   "autoFolders": [],
-  "defaultMaxVel": 3.0,
+  "defaultMaxVel": 2.0,
-  "defaultMaxAccel": 3.0,
+  "defaultMaxAccel": 1.5,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,

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

@@ -13,6 +13,8 @@ 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;
@@ -24,6 +26,7 @@ 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;
@@ -79,7 +82,7 @@ public class RobotContainer {
 
         resetOdometryToVisualPose = false;
          
-        vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
+        //vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
         vision.addPoseEstimateConsumer((vp) -> {
             Logger.recordOutput(
                 "Vision/" + vp.cameraName() + "/Pose", 
@@ -349,7 +352,7 @@ public class RobotContainer {
 
         NamedCommands.registerCommand(
             "intake down", 
-            intakePivot.manualSpeed(()->-0.75)
+            intakePivot.manualSpeed(()->0.75)
                 .withTimeout(1)
         );
 
@@ -361,7 +364,33 @@ public class RobotContainer {
             spindexer.spinToShooter()
                 .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed))
                 .alongWith(hood.trackToAngle(() -> Units.degreesToRadians(10)))
-                .withTimeout(3));
+                .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() {

src/main/java/frc/robot/constants/SpindexerConstants.java

@@ -12,9 +12,9 @@ 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 kSpindexerStatorCurrentLimit = 95;
-    public static final int kSpindexerSupplyCurrentLimit = 60;
+    public static final int kSpindexerSupplyCurrentLimit = 50;
-    public static final int kFeederCurrentLimit = 40;
+    public static final int kFeederCurrentLimit = 30;
 
     public static final double kSpindexerSpeed = 1;
     public static final double kFeederSpeed = 1;

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

@@ -80,6 +80,13 @@ public class IntakePivot extends SubsystemBase {
         });
     }
 
+    // public Command jimmy(){
+    //     return run(() -> {
+    //         leftMotor.set(0.5 * IntakePivotConstants.kMaxManualSpeedMultiplier);
+    //         rightMotor.set(0.5 * IntakePivotConstants.kMaxManualSpeedMultiplier);
+    //     })
+    // }
+
     public Command stop() {
         return manualSpeed(() -> 0);
     }

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

@@ -60,4 +60,11 @@ public class Spindexer extends SubsystemBase {
         });
     }
 
+    public Command instantaneousStop() {
+        return runOnce(() -> {
+            spindexerMotor.setControl(spindexerMotorOutput.withOutput(0));
+            feederMotor.set(0);
+        });
+    }
+    
 }