getting good

team5115 · Dec 7, 2024 · c8e02ae · c8e02ae
1 parent 8a4ffc8
commit c8e02ae
Show file tree

Hide file tree

Showing 4 changed files with 78 additions and 3 deletions.
diff --git a/src/main/deploy/pathplanner/autos/1 and Preload.auto b/src/main/deploy/pathplanner/autos/1 and Preload.auto
@@ -0,0 +1,51 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.225000000000017,
+      "y": 7.915
+    },
+    "rotation": 7.642727713197618e-27
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Stack"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "race",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Intake"
+                      }
+                    },
+                    {
+                      "type": "path",
+                      "data": {
+                        "pathName": "IntakeAltStack1"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": true
+}
diff --git a/src/main/deploy/pathplanner/autos/2 Stack Alt.auto b/src/main/deploy/pathplanner/autos/2 Stack Alt.auto
@@ -2,10 +2,10 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 1.225,
+      "x": 1.225000000000017,
       "y": 7.915
     },
-    "rotation": 2.774373461529055e-26
+    "rotation": 7.642727713197618e-27
   },
   "command": {
     "type": "sequential",
@@ -21,6 +21,12 @@
           "type": "sequential",
           "data": {
             "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeSetpoint"
+                }
+              },
               {
                 "type": "race",
                 "data": {

diff --git a/src/main/java/frc/team5115/RobotContainer.java b/src/main/java/frc/team5115/RobotContainer.java
@@ -164,7 +164,7 @@ private void configureButtonBindings() {
 
         joyManip.x().onTrue(DriveCommands.prepareDispense(dispenser, arm));
 
-        joyManip.rightBumper().onTrue(DriveCommands.stackSequence(dispenser, arm));
+        joyManip.rightBumper().onTrue(DriveCommands.stackSet(dispenser, arm));
 
         joyManip.rightTrigger().onTrue(dispenser.dispense()).onFalse(dispenser.stop());
         joyManip.leftTrigger().onTrue(dispenser.intake()).onFalse(dispenser.stop());
@@ -194,6 +194,7 @@ public static void registerCommands(
         NamedCommands.registerCommand("Dispense", DriveCommands.quickScore(dispenser, arm, drivetrain));
 
         NamedCommands.registerCommand("Intake", DriveCommands.intakeUntilCanister(dispenser, arm));
+        NamedCommands.registerCommand("IntakeSetpoint", DriveCommands.armIntake(arm));
 
         NamedCommands.registerCommand("Stack", DriveCommands.stackSequence(dispenser, arm));
     }

diff --git a/src/main/java/frc/team5115/commands/DriveCommands.java b/src/main/java/frc/team5115/commands/DriveCommands.java
@@ -44,6 +44,14 @@ public static Command quickScore(Dispenser dispenser, Arm arm, Drivetrain drivet
                 .withInterruptBehavior(InterruptionBehavior.kCancelSelf);
     }
 
+    public static Command armIntake(Arm arm) {
+        return Commands.sequence(
+                        arm.intake(),
+                        arm.waitForSetpoint(2.0)
+        )
+                .withInterruptBehavior(InterruptionBehavior.kCancelSelf);
+    }
+
     public static Command stackSequence(Dispenser dispenser, Arm arm) {
         return Commands.sequence(
                         arm.prepareStack(),
@@ -56,6 +64,15 @@ public static Command stackSequence(Dispenser dispenser, Arm arm) {
                 .withInterruptBehavior(InterruptionBehavior.kCancelSelf);
     }
 
+     public static Command stackSet(Dispenser dispenser, Arm arm) {
+        return Commands.sequence(
+                        arm.prepareStack(),
+                        arm.waitForSetpoint(3.0)
+        )
+                .withInterruptBehavior(InterruptionBehavior.kCancelSelf);
+    }
+
+
     public static Command intakeUntilCanister(Dispenser dispenser, Arm arm) {
         return Commands.sequence(
                 dispenser.intake(), arm.intake(), dispenser.waitTillCanister(), dispenser.stop());