barge auto fixes

Author: Etaash-mathamsetty

src/main/deploy/pathplanner/paths/One piece part 2 barge.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.912717627679558,
-        "y": 4.14838350636806
+        "x": 6.58380744421885,
+        "y": 3.97072622940022
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.449385245901639,
-        "y": 4.186834016393442
+        "x": 6.521380450274493,
+        "y": 4.020078421927787
       },
       "prevControl": {
-        "x": 5.587703157977192,
-        "y": 4.227767040157686
+        "x": 5.692719585551794,
+        "y": 4.197501730233285
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -178.80651057601796
+    "rotation": 180.0
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -178.8982938847937
+    "rotation": 180.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/One piece part 4 barge.path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.523031014796091,
-        "y": 6.049442112922044
+        "x": 6.415690074084619,
+        "y": 5.303542827299003
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.041803278688524,
-        "y": 6.788165983606557
+        "x": 5.835833575274493,
+        "y": 5.905737634607262
       },
       "prevControl": {
-        "x": 7.339851338398133,
-        "y": 6.1537541705130065
+        "x": 5.911689711991,
+        "y": 5.5599909301266
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,7 +42,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 178.91907581333933
+    "rotation": 126.20121880708717
   },
   "reversed": false,
   "folder": null,

src/main/java/team1403/robot/RobotContainer.java

@@ -518,6 +518,12 @@ private void configureBindings() {
     NamedCommands.registerCommand("ReefAlignR", getAlignCommand(Blackbox.ReefSelect.RIGHT));
     NamedCommands.registerCommand("ReefAlignCenter", getAlignCommandCenter());
     NamedCommands.registerCommand("Loading", Blackbox.robotStateCmd(Blackbox.State.loading));
+    NamedCommands.registerCommand("LoadingFromBarge", Commands.sequence(
+      Blackbox.robotStateCmd(State.MoveElevator),
+      new WristCommand(m_wrist, Constants.Wrist.Setpoints.Barge).asProxy(),
+      new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.Source).asProxy(),
+      Blackbox.robotStateCmd(State.loading)
+    ));
 
     NamedCommands.registerCommand("Barge L3", Commands.sequence(
       Blackbox.robotStateCmd(State.MoveElevator),

src/main/java/team1403/robot/commands/auto/AutoHelper.java

@@ -57,11 +57,12 @@ public static Command getOnePCenterAlgae(SwerveSubsystem m_swerve) {
                         Commands.waitSeconds(3.0),
                         AutoUtil.loadPathPlannerPath("One piece part 3 barge", m_swerve),
                         NamedCommands.getCommand("Net Shot"),
-                        NamedCommands.getCommand("WaitForSetpoint"),
-                        NamedCommands.getCommand("Algae Expel")
+                        NamedCommands.getCommand("WaitForSetpoint")
+                        //NamedCommands.getCommand("Algae Expel") // default command will expel
                     )
                 ),
-                NamedCommands.getCommand("Loading"),
+                Commands.waitSeconds(0.5),
+                NamedCommands.getCommand("LoadingFromBarge"), //make sure we don't kill ourselves on the barge
                 AutoUtil.loadPathPlannerPath("One piece part 4 barge", m_swerve)
             );
         } catch (Exception e) {