got L4 working

team4909 · Feb 4, 2025 · b221a47 · b221a47
1 parent 50bf2e5
commit b221a47
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 15 deletions.
diff --git a/src/main/deploy/pathplanner/autos/Score far L4.auto b/src/main/deploy/pathplanner/autos/Score far L4.auto
@@ -11,9 +11,22 @@
           }
         },
         {
-          "type": "named",
+          "type": "race",
           "data": {
-            "name": "L4"
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "L4"
+                }
+              },
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 2.0
+                }
+              }
+            ]
           }
         },
         {
@@ -23,21 +36,41 @@
           }
         },
         {
-          "type": "named",
+          "type": "race",
           "data": {
-            "name": "score"
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "score"
+                }
+              },
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 1.0
+                }
+              }
+            ]
           }
         },
         {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.0
-          }
-        },
-        {
-          "type": "named",
+          "type": "race",
           "data": {
-            "name": "L1"
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "L1"
+                }
+              },
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              }
+            ]
           }
         }
       ]

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -86,6 +86,7 @@ public RobotContainer(){
     NamedCommands.registerCommand("L3", s_Elevator.goToL3());
     NamedCommands.registerCommand("L2", s_Elevator.goToL2());
     NamedCommands.registerCommand("L1", s_Elevator.goToL1());
+    NamedCommands.registerCommand("test", s_Elevator.testPrint());
 
 
 
@@ -206,6 +207,6 @@ private void configureBindings() {
   }
 
   public Command getAutonomousCommand() {
-    return new PathPlannerAuto("Taxi");//m_chooser.getSelected();
+    return new PathPlannerAuto("Score far L4");//m_chooser.getSelected();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -20,8 +20,8 @@ public class Elevator extends SubsystemBase {
     private final double L1Setpoint = 29;
     private final double L2Setpoint = 38.5;//32.5
     private final double L3Setpoint = 54.5;
-    private final double L4Setpoint = 78
-    ; // 74.5
+    private final double L4Setpoint = 80
+    ; // 74.5 and 78 dp on feb 1
     private final double L2ASetpoint = 37.5; 
     private final double L3ASetpoint = 54;
     //inch to rotations of the motor
@@ -86,6 +86,11 @@ public Command reZero(){
         }).withName("ReZero");
       }
 
+      public Command testPrint(){
+        return this.runOnce(() -> {
+            System.out.println("Test");
+        }).withName("Test");
+      }
       @Override
       public void periodic() {
         super.periodic();