center auto

Team2338 · Feb 28, 2025 · 55b0087 · 55b0087
1 parent 9432cc6
commit 55b0087
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 27 deletions.
diff --git a/src/main/deploy/pathplanner/autos/C-H4.auto b/src/main/deploy/pathplanner/autos/C-H4.auto
@@ -10,12 +10,6 @@
             "pathName": "c-h4"
           }
         },
-        {
-          "type": "named",
-          "data": {
-            "name": "MoveModulesIn"
-          }
-        },
         {
           "type": "named",
           "data": {

diff --git a/src/main/deploy/pathplanner/paths/c-h4.path b/src/main/deploy/pathplanner/paths/c-h4.path
@@ -3,24 +3,24 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.588217213114754,
+        "x": 7.15,
         "y": 4.013012295081967
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.924147260112099,
+        "x": 6.15,
         "y": 4.013012295081967
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.054750000004353,
+        "x": 5.8,
         "y": 4.013012295081967
       },
       "prevControl": {
-        "x": 7.054750000004353,
+        "x": 6.8,
         "y": 4.013012295081967
       },
       "nextControl": null,
@@ -34,9 +34,14 @@
   "eventMarkers": [
     {
       "name": "AutoElevatorLvl4",
-      "waypointRelativePos": 0.859350850077265,
+      "waypointRelativePos": 0.1277691911385971,
       "endWaypointRelativePos": null,
-      "command": null
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "AutoElevatorLvl4"
+        }
+      }
     }
   ],
   "globalConstraints": {

diff --git a/src/main/java/team/gif/robot/commands/drivetrain/MoveModulesIn.java b/src/main/java/team/gif/robot/commands/drivetrain/MoveModulesIn.java
@@ -8,31 +8,34 @@ public MoveModulesIn() {}
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
-
+        System.out.println("Move Motors");
     }
 
     // Called every time the scheduler runs (~20ms) while the command is scheduled
     @Override
     public void execute() {
-        Robot.swerveDrive.modulesTo90();
+//        Robot.swerveDrive.modulesTo90();
     }
 
     // Return true when the command should end, false if it should continue. Runs every ~20ms.
     @Override
     public boolean isFinished() {
         //The % takes care of wheel that are at or above 90 & 270, the -
-        double fLHeading = Math.abs(Robot.swerveDrive.fL.getTurningHeadingDegrees()) % 90;
-        double fRHeading = Math.abs(Robot.swerveDrive.fR.getTurningHeadingDegrees()) % 90;
-        double rLHeading = Math.abs(Robot.swerveDrive.rL.getTurningHeadingDegrees()) % 90;
-        double rRHeading = Math.abs(Robot.swerveDrive.rR.getTurningHeadingDegrees()) % 90;
-        double tolerance = 10;
-        return (fLHeading < tolerance || Math.abs(90 - fLHeading) < tolerance) &&
-                (fRHeading < tolerance || Math.abs(90 - fRHeading) < tolerance) &&
-                (rLHeading < tolerance || Math.abs(90 - rLHeading) < tolerance) &&
-                (rRHeading < tolerance || Math.abs(90 - rRHeading) < tolerance);
+//        double fLHeading = Math.abs(Robot.swerveDrive.fL.getTurningHeadingDegrees()) % 90;
+//        double fRHeading = Math.abs(Robot.swerveDrive.fR.getTurningHeadingDegrees()) % 90;
+//        double rLHeading = Math.abs(Robot.swerveDrive.rL.getTurningHeadingDegrees()) % 90;
+//        double rRHeading = Math.abs(Robot.swerveDrive.rR.getTurningHeadingDegrees()) % 90;
+//        double tolerance = 10;
+//        return (fLHeading < tolerance || Math.abs(90 - fLHeading) < tolerance) &&
+//                (fRHeading < tolerance || Math.abs(90 - fRHeading) < tolerance) &&
+//                (rLHeading < tolerance || Math.abs(90 - rLHeading) < tolerance) &&
+//                (rRHeading < tolerance || Math.abs(90 - rRHeading) < tolerance);
+        return true;
     }
 
     // Called when the command ends or is interrupted.
     @Override
-    public void end(boolean interrupted) {}
+    public void end(boolean interrupted) {
+        System.out.println("End Move");
+    }
 }
diff --git a/src/main/java/team/gif/robot/commands/shooter/AutonAutoShoot.java b/src/main/java/team/gif/robot/commands/shooter/AutonAutoShoot.java
@@ -1,9 +1,11 @@
 package team.gif.robot.commands.shooter;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import team.gif.lib.drivePace;
 import team.gif.robot.Robot;
+import team.gif.robot.commands.drivetrain.ShortDriveAway;
 import team.gif.robot.commands.elevator.SafeToLower;
 import team.gif.robot.commands.elevator.SetElevatorPosition;
 
@@ -64,11 +66,11 @@ public void end(boolean interrupted) {
 
         // only shoot if the robot found the target during the command
         if (hasTarget) {
-            //new Shoot().schedule(); // run the shooter using the standard shoot command
             new SequentialCommandGroup(
                     new Shoot(),
-                    new SafeToLower(),
-                    new SetElevatorPosition(0)
+                    new ParallelCommandGroup( // running these in parallel provides plenty of time to clear
+                            new ShortDriveAway(),
+                            new SetElevatorPosition(0))
             ).schedule();
         }
     }