final changes after competition

This is the code from after we left the Victoria. Everything should work

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

@@ -1,20 +1,14 @@
 {
   "version": 1.0,
-  "startingPose": {
-    "position": {
-      "x": 0.4584823584061582,
-      "y": 2.616250998267763
-    },
-    "rotation": 0
-  },
+  "startingPose": null,
   "command": {
     "type": "sequential",
     "data": {
       "commands": [
         {
           "type": "path",
           "data": {
-            "pathName": "Leave"
+            "pathName": "SimpleForward"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Score Leave.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "speaker score"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "SimpleForward"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,63 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "ShooterPreload"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeShooterFeedPreload"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "NotePickupFront"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FrontNoteToSpeaker"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "ShooterPreload"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeShooterFeedPreload"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.909982492594682,
+        "y": 5.521816553472107
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9099824925946813,
+        "y": 5.521816553472107
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.5442140365497197,
+        "y": 5.521816553472107
+      },
+      "prevControl": {
+        "x": 2.54421403654972,
+        "y": 5.521816553472107
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.2,
+    "maxAcceleration": 1.2,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -0,0 +1,70 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3826715309960145,
+        "y": 5.609930647410491
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.38267153099602,
+        "y": 5.609930647410491
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.8806111279485536,
+        "y": 5.609930647410491
+      },
+      "prevControl": {
+        "x": 1.8806111279485536,
+        "y": 5.609930647410491
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "New Event Marker",
+      "waypointRelativePos": 0.25,
+      "command": {
+        "type": "sequential",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "PickupNote"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 1.2,
+    "maxAcceleration": 1.2,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3973572133190786,
+        "y": 5.5
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.3973572133190837,
+        "y": 5.5
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.557526116841143,
+        "y": 5.5
+      },
+      "prevControl": {
+        "x": 1.5575261168411432,
+        "y": 5.5
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -19,6 +19,7 @@
 import frc.robot.intake.Intake;
 import frc.robot.intake.commands.IntakePickup;
 import frc.robot.intake.commands.IntakeShooterFeed;
+import frc.robot.intake.commands.IntakeShooterFeedPreload;
 import frc.robot.intake.commands.IntakeAmpScore;
 import frc.robot.shooter.Shooter;
 import frc.robot.shooter.commands.ShooterSpinUp;
@@ -47,6 +48,8 @@ public class RobotContainer {
 
     private final Command intakePickup = new IntakePickup(intake);
     private final Command intakeShooterFeed = new IntakeShooterFeed(intake);
+    private final Command intakeShooterFeedPreload = new IntakeShooterFeedPreload(intake,shooter);
+
     private final Command intakeEject = new IntakeAmpScore(intake);
     private final Command ampScoreKill = new AmpScoreKill(elevator);
     private final Command shooterSpinUp = new ShooterSpinUp(shooter);
@@ -69,10 +72,10 @@ public class RobotContainer {
         new IntakeShooterFeed(intake)
     );
 
-    private final SequentialCommandGroup speakerScorePreload = new SequentialCommandGroup(
-        new ShooterPreload(shooter),
-        new IntakeShooterFeed(intake)
-    );
+     private final SequentialCommandGroup speakerScorePreload = new SequentialCommandGroup(
+         new ShooterPreload(shooter),
+         new IntakeShooterFeedPreload(intake,shooter)
+     );
 
     SendableChooser<Command> autoSelector = new SendableChooser<>();
 
@@ -96,32 +99,22 @@ public RobotContainer() {
     }
 
     private void configureSubsystemCommands() {
-<<<<<<< HEAD
         controller.getLeftBumper().whileTrue(intakeEject);
-        controller.getRightBumper().onTrue(intakePickup);
-        //controller.getRightTrigger().whileTrue(elevatorClimb);
-    // controller.getX().onTrue(elevatorUp);
-        //controller.getA().onTrue(elevatorDown);
+        controller.getRightBumper().whileTrue(intakePickup);
+        //controller.getB().whileTrue(elevatorClimb);
+        controller.getUp().whileTrue(elevatorUp);
+        controller.getDown().whileTrue(elevatorDown);
         //controller.getY().whileTrue(intakeShooterFeed);
-        controller.getA().onTrue(ampScore);
-        controller.getB().whileTrue(shooterSpinUp);
-    //    controller.getX().onTrue(ampScoreKill);
+
+        controller.getA().whileTrue(shooterSpinUp);
+        //controller.getX().onTrue(speakerScorePreload);
         //controller.getX().whileTrue(speakerScorePreload);
-=======
-        controller.getLeftBumper().onTrue(intakePickup);
-        controller.getRightBumper().whileTrue(intakeShooterFeed);
-
-        controller.getX().onTrue(elevatorUp);
-        controller.getA().onTrue(ampScore);
-        controller.getB().onTrue(speakerScore);
-        controller.getY().whileTrue(elevatorDown);
->>>>>>> 909cedc7c1648cad8a086d60bd1c9045cbc7a39d
 
         //controller.getB().onTrue(speakerScore);
         //controller.getB().whileTrue(shooterSpinUp);
 
        // controller.getRightTrigger().onTrue(ElevatorSmallUp);
-        controller.getY().whileTrue(intakeShooterFeed);
+        controller.getX().whileTrue(intakeShooterFeed);
     }
 
     private void configureSwerveDriveCommands() {
@@ -132,13 +125,22 @@ private void configureSwerveDriveCommands() {
                     swerveDrive
                 )
             );
+
+            controller.getLeftStick().whileTrue(new RunCommand(() -> swerveDrive.lockPosition(), swerveDrive));
     }
 
     private void configureAutonomous() {
         NamedCommands.registerCommand("PickupNote", new IntakePickup(intake));
         NamedCommands.registerCommand("AmpNote", ampScore);
+        NamedCommands.registerCommand("IntakeShooterFeedPreload", new IntakeShooterFeedPreload(intake,shooter));
+        NamedCommands.registerCommand("ShooterPreload", new ShooterPreload(shooter));
+        NamedCommands.registerCommand("speaker score", speakerScorePreload);
+
+
 
         autoSelector.addOption("Right Roundhouse Amp", new PathPlannerAuto("Right Roundhouse Amp"));
+        autoSelector.addOption("Score speaker 1x", new PathPlannerAuto("Score Leave"));
+        //autoSelector.addOption("Score speaker 1x", speakerScorePreload);
         autoSelector.addOption("Left Amp", new PathPlannerAuto("Left Amp"));
         autoSelector.setDefaultOption("Leave", new PathPlannerAuto("Leave"));
 

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

@@ -8,5 +8,5 @@ public class ShooterConstants {
     public static final IdleMode IDLE_MODE = IdleMode.kCoast;
 
     public static final double SHOOTER_SPINUP_DELAY = 10;
-    public static final double SHOOTER_RUN_DURATION = 2;
+    public static final double SHOOTER_RUN_DURATION = 1;
 }