two and three peice from left sub works

src/main/deploy/pathplanner/autos/Left Sub 3s.auto

@@ -30,9 +30,22 @@
           }
         },
         {
-          "type": "path",
+          "type": "parallel",
           "data": {
-            "pathName": "L Sub to Note 1"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Sub to Note 1"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
           }
         },
         {
@@ -53,10 +66,29 @@
             "name": "Speaker Note Shot"
           }
         },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Sub to Note 2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
+          }
+        },
         {
           "type": "path",
           "data": {
-            "pathName": "L Sub to Note 2"
+            "pathName": "Note 2 to L Sub"
           }
         },
         {

src/main/deploy/pathplanner/autos/Left Subwoofer Center 2S.auto

@@ -30,9 +30,22 @@
           }
         },
         {
-          "type": "path",
+          "type": "parallel",
           "data": {
-            "pathName": "L Sub to Note 1"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Sub to Note 1"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
           }
         },
         {

src/main/deploy/pathplanner/paths/L Sub to Note 1.path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.0900457128549583,
+        "x": 1.4408703461281565,
-        "y": 6.560420642208806
+        "y": 6.583808951093687
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.5344235816676761,
+        "x": 1.8852482149408742,
-        "y": 7.098351746561042
+        "y": 7.121740055445923
       },
       "isLocked": false,
       "linkedName": null
@@ -43,14 +43,7 @@
       "command": {
         "type": "parallel",
         "data": {
-          "commands": [
+          "commands": []
-            {
-              "type": "named",
-              "data": {
-                "name": "Auto Intake"
-              }
-            }
-          ]
         }
       }
     }

src/main/deploy/pathplanner/paths/L Sub to Note 2.path

@@ -3,41 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.0900457128549583,
+        "x": 1.5578118905525558,
-        "y": 6.548726487766366
+        "y": 6.4083966344570875
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.4993411183403562,
+        "x": 1.9671072960379536,
-        "y": 6.490255715554167
+        "y": 6.349925862244889
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.247767002656512,
+        "x": 2.5284267092750707,
-        "y": 5.87046553010485
+        "y": 5.671664904583371
       },
       "prevControl": {
-        "x": 1.8852482149408738,
+        "x": 2.1659079215594326,
-        "y": 6.256372626705368
+        "y": 6.057572001183889
-      },
-      "nextControl": {
-        "x": 2.61028579037215,
-        "y": 5.484558433504332
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.0900457128549583,
-        "y": 6.548726487766366
-      },
-      "prevControl": {
-        "x": 2.095742994904793,
-        "y": 6.805997885500045
       },
       "nextControl": null,
       "isLocked": false,
@@ -46,31 +30,13 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 1.1,
+      "waypointRelativePos": 0.7,
       "rotationDegrees": 157.35591349901955,
       "rotateFast": true
     }
   ],
   "constraintZones": [],
-  "eventMarkers": [
+  "eventMarkers": [],
-    {
-      "name": "New Event Marker",
-      "waypointRelativePos": 0.1,
-      "command": {
-        "type": "parallel",
-        "data": {
-          "commands": [
-            {
-              "type": "named",
-              "data": {
-                "name": "Auto Intake"
-              }
-            }
-          ]
-        }
-      }
-    }
-  ],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,
@@ -79,7 +45,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -153.43494882292208,
+    "rotation": 159.0754982550787,
     "rotateFast": false
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Left Subwoofer.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.148516485067158,
+        "x": 1.2771521839339972,
-        "y": 6.560420642208806
+        "y": 6.268066781147808
       },
       "prevControl": {
-        "x": 1.5812001994374358,
+        "x": 1.709835898304275,
-        "y": 6.864468657712245
+        "y": 6.572114796651247
       },
       "nextControl": null,
       "isLocked": false,
@@ -39,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -128.39665211123375,
+    "rotation": -141.11550356628527,
     "rotateFast": false
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Note 1 to Speaker.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.136822330624718,
+        "x": 1.3122346472613169,
-        "y": 6.478561561111727
+        "y": 6.232984317820488
       },
       "prevControl": {
-        "x": 1.581200199437436,
+        "x": 1.756612516074035,
-        "y": 6.712444649960526
+        "y": 6.466867406669287
       },
       "nextControl": null,
       "isLocked": false,
@@ -39,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -145.88552705465875,
+    "rotation": -141.34019174590983,
     "rotateFast": false
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Note 2 to L Sub.path

@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.1776020760018726,
+        "y": 5.964018765644369
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9671072960379539,
+        "y": 6.209596008935608
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2888463383764373,
+        "y": 6.232984317820488
+      },
+      "prevControl": {
+        "x": 1.6630592805345157,
+        "y": 6.373314171129768
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": -164.3011806576524,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -144.293308599397,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 150.25511870305772,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/frc/robot/Commands/autoIntaking.java

@@ -28,4 +28,5 @@ public class autoIntaking extends Command{
             return true;
         }else {return false;}
     }
+
 }

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

@@ -27,6 +27,7 @@ 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.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -101,9 +102,40 @@ public class RobotContainer {
 
         climber = new Climber(shooter::getShooterAngle);
 
-        NamedCommands.registerCommand("Charge Shooter 2 Sec", shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0).withTimeout(2.0));
+        NamedCommands.registerCommand(
-        NamedCommands.registerCommand("Speaker Note Shot", Commands.parallel(shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0), indexer.shootNote(() -> 1.0)).withTimeout(2.0));
+            "Charge Shooter 2 Sec", 
-        NamedCommands.registerCommand("Auto Intake", new autoIntaking(intake, indexer::getBeamBreak).andThen(intake.intakeUpCommand()));
+            shooter.angleSpeedsSetpoints(
+                () -> ShooterConstants.kShooterLoadAngle, 
+                1.0, 
+                1.0
+            ).withTimeout(2.0)
+        );
+
+        NamedCommands.registerCommand(
+            "Speaker Note Shot", 
+            Commands.parallel(
+                shooter.angleSpeedsSetpoints(
+                    () -> ShooterConstants.kShooterLoadAngle, 
+                    1.0, 
+                    1.0
+                ), 
+                indexer.shootNote(() -> 1.0)
+            ).withTimeout(2.0)
+        );
+
+        
+        NamedCommands.registerCommand(
+            "Auto Intake", 
+            Commands.parallel(
+                intake.intakeControl(
+                    () -> IntakeConstants.kDownAngle, 
+                    () -> 1.0
+                ), 
+                indexer.advanceNote()
+            ).withTimeout(3.0)//.until(indexer::getBeamBreak)
+        );
+
+        //NamedCommands.registerCommand("Auto Intake", new PrintCommand("Intake Note"));
 
         // An example Named Command, doesn't need to remain once we start actually adding real things
         // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
@@ -186,7 +218,7 @@ public class RobotContainer {
             )
         );
 
-        driver.leftTrigger().onFalse(
+        driver.leftTrigger().whileFalse(
             new InstantCommand(
                 () -> {
                     intakeDown = false;
@@ -194,7 +226,7 @@ public class RobotContainer {
             )
         );
 
-        driver.leftTrigger().onTrue(Commands.parallel(
+        driver.leftTrigger().whileTrue(Commands.parallel(
             new InstantCommand(
                 () -> {
                     intakeDown = true;

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

@@ -13,9 +13,9 @@ public final class AutoConstants {
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final double kPXController = 1.05;//5;
+    public static final double kPXController = 5.5;//5;
-    public static final double kPYController = 1.05;//5;
+    public static final double kPYController = 5.5;//5;
-    public static final double kPThetaController = 0.95;//0.5; // needs to be separate from heading control
+    public static final double kPThetaController = 5;//0.5; // needs to be separate from heading control
 
     public static final double kPHeadingController = 0.02; // for heading control NOT PATHING
     public static final double kDHeadingController = 0.0025;

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

@@ -16,7 +16,7 @@ public class IntakeConstants {
     public static final double kGFeedForward = 1;//1.11;
     public static final double kVFeedForward = .5;//0.73;
 
-    public static final double kStartingAngle = Math.toRadians(105.0);
+    public static final double kStartingAngle = Math.toRadians(95.0);
     public static final double kUpAngle = Math.toRadians(90.0);
     public static final double kDownAngle = Math.toRadians(-34.0);