lots of auto tuning. trying to fix leds and still don't know why path command so weird...

DriverStation3950 · DriverStation3950 · commit ab395d26ff3e · 2025-10-16T19:24:32.000-04:00
diff --git a/src/main/deploy/pathplanner/autos/4pc North.auto b/src/main/deploy/pathplanner/autos/4pc North.auto
@@ -11,31 +11,30 @@
           }
         },
         {
-          "type": "sequential",
+          "type": "parallel",
           "data": {
             "commands": [
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoLeft"
+                  "name": "SlowlyRaise"
                 }
               },
               {
-                "type": "sequential",
+                "type": "named",
                 "data": {
-                  "commands": [
-                    {
-                      "type": "named",
-                      "data": {
-                        "name": "AutoL4"
-                      }
-                    }
-                  ]
+                  "name": "AutoLeft"
                 }
               }
             ]
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoL4"
+          }
+        },
         {
           "type": "parallel",
           "data": {
@@ -62,31 +61,30 @@
           }
         },
         {
-          "type": "sequential",
+          "type": "parallel",
           "data": {
             "commands": [
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoLeft"
+                  "name": "HalfRaise"
                 }
               },
               {
-                "type": "sequential",
+                "type": "named",
                 "data": {
-                  "commands": [
-                    {
-                      "type": "named",
-                      "data": {
-                        "name": "AutoL4"
-                      }
-                    }
-                  ]
+                  "name": "AutoLeft"
                 }
               }
             ]
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoL4"
+          }
+        },
         {
           "type": "parallel",
           "data": {
@@ -113,31 +111,30 @@
           }
         },
         {
-          "type": "sequential",
+          "type": "parallel",
           "data": {
             "commands": [
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoRight"
+                  "name": "HalfRaise"
                 }
               },
               {
-                "type": "sequential",
+                "type": "named",
                 "data": {
-                  "commands": [
-                    {
-                      "type": "named",
-                      "data": {
-                        "name": "AutoL4"
-                      }
-                    }
-                  ]
+                  "name": "AutoRight"
                 }
               }
             ]
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoL4"
+          }
+        },
         {
           "type": "parallel",
           "data": {
@@ -164,31 +161,30 @@
           }
         },
         {
-          "type": "sequential",
+          "type": "parallel",
           "data": {
             "commands": [
               {
                 "type": "named",
                 "data": {
-                  "name": "AutoRight"
+                  "name": "SlowlyRaise"
                 }
               },
               {
-                "type": "sequential",
+                "type": "named",
                 "data": {
-                  "commands": [
-                    {
-                      "type": "named",
-                      "data": {
-                        "name": "AutoL4"
-                      }
-                    }
-                  ]
+                  "name": "AutoRight"
                 }
               }
             ]
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoL4"
+          }
+        },
         {
           "type": "parallel",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/TEst Auto.auto b/src/main/deploy/pathplanner/autos/TEst Auto.auto
@@ -0,0 +1,50 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "NSource_2+3L4 Max V"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "AutoL4"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoSource"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Rest"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/NPoof_1L4 Max V.path b/src/main/deploy/pathplanner/paths/NPoof_1L4 Max V.path
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.027339612023305,
-        "y": 7.223202049126059
+        "x": 7.02,
+        "y": 6.17387784090909
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.53873165747785,
-        "y": 6.585020230944241
+        "x": 6.451619318181818,
+        "y": 5.814900568181818
       },
       "isLocked": false,
       "linkedName": "PoofStart"
     },
     {
       "anchor": {
-        "x": 5.903181818181818,
-        "y": 5.874730113636364
+        "x": 5.70375,
+        "y": 5.535696022727272
       },
       "prevControl": {
-        "x": 6.312017045454545,
-        "y": 6.263622159090909
+        "x": 6.012869318181819,
+        "y": 5.964474431818181
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,7 +34,7 @@
   "eventMarkers": [
     {
       "name": "SlowlyRaise",
-      "waypointRelativePos": 0.1363895716292136,
+      "waypointRelativePos": 0.0,
       "endWaypointRelativePos": null,
       "command": null
     }
diff --git a/src/main/deploy/pathplanner/paths/NSource_2+3L4 Max V.path b/src/main/deploy/pathplanner/paths/NSource_2+3L4 Max V.path
@@ -34,7 +34,7 @@
   "eventMarkers": [
     {
       "name": "HalfRaise",
-      "waypointRelativePos": 0.37663857677902457,
+      "waypointRelativePos": 0.0,
       "endWaypointRelativePos": null,
       "command": null
     }
@@ -48,7 +48,7 @@
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 1.0,
+    "velocity": 1.75,
     "rotation": -55.603007071007404
   },
   "reversed": false,
diff --git a/src/main/deploy/pathplanner/paths/NSource_4L4 Max V.path b/src/main/deploy/pathplanner/paths/NSource_4L4 Max V.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 4.856164772727273,
-        "y": 6.143963068181818
+        "x": 4.636789772727273,
+        "y": 6.004360795454545
       },
       "prevControl": {
-        "x": 4.846193181818182,
-        "y": 6.981576704545454
+        "x": 4.626818181818182,
+        "y": 6.841974431818182
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -175,10 +175,10 @@ public RobotContainer() {
 
     NamedCommands.registerCommand(
         "SlowlyRaise",
-        elevator.setElevatorPositionCommand(elevator.L4_inMotorRotations, 40, 10, 0));
+        elevator.setElevatorPositionCommand(elevator.L4_inMotorRotations, 200, 135, 0));
     NamedCommands.registerCommand(
         "HalfRaise",
-        elevator.setElevatorPositionCommand(elevator.L4_inMotorRotations / 2, 80, 40, 0));
+        elevator.setElevatorPositionCommand(elevator.L4_inMotorRotations / 2, 200, 135, 0));
     NamedCommands.registerCommand("AutoL4", new AutoL4(elevator, mailbox));
     NamedCommands.registerCommand("Rest", elevator.setElevatorToRestCommand());
 
@@ -365,7 +365,7 @@ private void configureButtonBindings() {
         .onTrue(
             Commands.runOnce(() -> climber.bringInTheClimb(), climber)
                 .onlyIf(climber::isReadyToClimb));
-    
+
     SmartDashboard.putData(
         "Climber Reset Button", new InstantCommand(() -> climber.goBackToRest(), climber));
     //   operator.start().
diff --git a/src/main/java/frc/robot/commands/ScoreCommand.java b/src/main/java/frc/robot/commands/ScoreCommand.java
@@ -1,6 +1,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.MailBox;
 import frc.robot.subsystems.elevator.Elevator;
 
@@ -47,7 +48,7 @@ public void execute() {
     // mailbox.nothingInIntake())
     if (mailboxStarted && mailbox.nothingInIntake()) {
       scored = true;
-      elevator.setElevatorToRestCommand(); // Go back down
+      CommandScheduler.getInstance().schedule(elevator.setElevatorToRestCommand()); // Go back down
     }
   }
 

Original file line number	Diff line number	Diff line change
`@@ -11,31 +11,30 @@`
`11`	`11`	`}`
`12`	`12`	`},`
`13`	`13`	`{`
`14`		`- "type": "sequential",`
	`14`	`+ "type": "parallel",`
`15`	`15`	`"data": {`
`16`	`16`	`"commands": [`
`17`	`17`	`{`
`18`	`18`	`"type": "named",`
`19`	`19`	`"data": {`
`20`		`- "name": "AutoLeft"`
	`20`	`+ "name": "SlowlyRaise"`
`21`	`21`	`}`
`22`	`22`	`},`
`23`	`23`	`{`
`24`		`- "type": "sequential",`
	`24`	`+ "type": "named",`
`25`	`25`	`"data": {`
`26`		`- "commands": [`
`27`		`- {`
`28`		`- "type": "named",`
`29`		`- "data": {`
`30`		`- "name": "AutoL4"`
`31`		`- }`
`32`		`- }`
`33`		`- ]`
	`26`	`+ "name": "AutoLeft"`
`34`	`27`	`}`
`35`	`28`	`}`
`36`	`29`	`]`
`37`	`30`	`}`
`38`	`31`	`},`
	`32`	`+ {`
	`33`	`+ "type": "named",`
	`34`	`+ "data": {`
	`35`	`+ "name": "AutoL4"`
	`36`	`+ }`
	`37`	`+ },`
`39`	`38`	`{`
`40`	`39`	`"type": "parallel",`
`41`	`40`	`"data": {`
`@@ -62,31 +61,30 @@`
`62`	`61`	`}`
`63`	`62`	`},`
`64`	`63`	`{`
`65`		`- "type": "sequential",`
	`64`	`+ "type": "parallel",`
`66`	`65`	`"data": {`
`67`	`66`	`"commands": [`
`68`	`67`	`{`
`69`	`68`	`"type": "named",`
`70`	`69`	`"data": {`
`71`		`- "name": "AutoLeft"`
	`70`	`+ "name": "HalfRaise"`
`72`	`71`	`}`
`73`	`72`	`},`
`74`	`73`	`{`
`75`		`- "type": "sequential",`
	`74`	`+ "type": "named",`
`76`	`75`	`"data": {`
`77`		`- "commands": [`
`78`		`- {`
`79`		`- "type": "named",`
`80`		`- "data": {`
`81`		`- "name": "AutoL4"`
`82`		`- }`
`83`		`- }`
`84`		`- ]`
	`76`	`+ "name": "AutoLeft"`
`85`	`77`	`}`
`86`	`78`	`}`
`87`	`79`	`]`
`88`	`80`	`}`
`89`	`81`	`},`
	`82`	`+ {`
	`83`	`+ "type": "named",`
	`84`	`+ "data": {`
	`85`	`+ "name": "AutoL4"`
	`86`	`+ }`
	`87`	`+ },`
`90`	`88`	`{`
`91`	`89`	`"type": "parallel",`
`92`	`90`	`"data": {`
`@@ -113,31 +111,30 @@`
`113`	`111`	`}`
`114`	`112`	`},`
`115`	`113`	`{`
`116`		`- "type": "sequential",`
	`114`	`+ "type": "parallel",`
`117`	`115`	`"data": {`
`118`	`116`	`"commands": [`
`119`	`117`	`{`
`120`	`118`	`"type": "named",`
`121`	`119`	`"data": {`
`122`		`- "name": "AutoRight"`
	`120`	`+ "name": "HalfRaise"`
`123`	`121`	`}`
`124`	`122`	`},`
`125`	`123`	`{`
`126`		`- "type": "sequential",`
	`124`	`+ "type": "named",`
`127`	`125`	`"data": {`
`128`		`- "commands": [`
`129`		`- {`
`130`		`- "type": "named",`
`131`		`- "data": {`
`132`		`- "name": "AutoL4"`
`133`		`- }`
`134`		`- }`
`135`		`- ]`
	`126`	`+ "name": "AutoRight"`
`136`	`127`	`}`
`137`	`128`	`}`
`138`	`129`	`]`
`139`	`130`	`}`
`140`	`131`	`},`
	`132`	`+ {`
	`133`	`+ "type": "named",`
	`134`	`+ "data": {`
	`135`	`+ "name": "AutoL4"`
	`136`	`+ }`
	`137`	`+ },`
`141`	`138`	`{`
`142`	`139`	`"type": "parallel",`
`143`	`140`	`"data": {`
`@@ -164,31 +161,30 @@`
`164`	`161`	`}`
`165`	`162`	`},`
`166`	`163`	`{`
`167`		`- "type": "sequential",`
	`164`	`+ "type": "parallel",`
`168`	`165`	`"data": {`
`169`	`166`	`"commands": [`
`170`	`167`	`{`
`171`	`168`	`"type": "named",`
`172`	`169`	`"data": {`
`173`		`- "name": "AutoRight"`
	`170`	`+ "name": "SlowlyRaise"`
`174`	`171`	`}`
`175`	`172`	`},`
`176`	`173`	`{`
`177`		`- "type": "sequential",`
	`174`	`+ "type": "named",`
`178`	`175`	`"data": {`
`179`		`- "commands": [`
`180`		`- {`
`181`		`- "type": "named",`
`182`		`- "data": {`
`183`		`- "name": "AutoL4"`
`184`		`- }`
`185`		`- }`
`186`		`- ]`
	`176`	`+ "name": "AutoRight"`
`187`	`177`	`}`
`188`	`178`	`}`
`189`	`179`	`]`
`190`	`180`	`}`
`191`	`181`	`},`
	`182`	`+ {`
	`183`	`+ "type": "named",`
	`184`	`+ "data": {`
	`185`	`+ "name": "AutoL4"`
	`186`	`+ }`
	`187`	`+ },`
`192`	`188`	`{`
`193`	`189`	`"type": "parallel",`
`194`	`190`	`"data": {`
Original file line number	Diff line number	Diff line change
`@@ -1,6 +1,7 @@`
`1`	`1`	`package frc.robot.commands;`
`2`	`2`
`3`	`3`	`import edu.wpi.first.wpilibj2.command.Command;`
	`4`	`+import edu.wpi.first.wpilibj2.command.CommandScheduler;`
`4`	`5`	`import frc.robot.MailBox;`
`5`	`6`	`import frc.robot.subsystems.elevator.Elevator;`
`6`	`7`
`@@ -47,7 +48,7 @@ public void execute() {`
`47`	`48`	`// mailbox.nothingInIntake())`
`48`	`49`	`if (mailboxStarted && mailbox.nothingInIntake()) {`
`49`	`50`	`scored = true;`
`50`		`- elevator.setElevatorToRestCommand(); // Go back down`
	`51`	`+ CommandScheduler.getInstance().schedule(elevator.setElevatorToRestCommand()); // Go back down`
`51`	`52`	`}`
`52`	`53`	`}`
`53`	`54`