add middle FU

Team3128 · Sep 24, 2024 · 0c1eda6 · 0c1eda6
1 parent fd0418c
commit 0c1eda6
Show file tree

Hide file tree

Showing 8 changed files with 183 additions and 84 deletions.
diff --git a/src/main/deploy/pathplanner/autos/middle_FU.auto b/src/main/deploy/pathplanner/autos/middle_FU.auto
@@ -0,0 +1,79 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.4,
+      "y": 5.55
+    },
+    "rotation": 180.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "ramShootNoStop"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "middle-note1.3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "note1.3-middle"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ramShootNoStop"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "middle-note1.2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "note1.2-middle"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ramShootNoStop"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "middle-note1.1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "note1.1-middle"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "ramShoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/middle-note1.1.path b/src/main/deploy/pathplanner/paths/middle-note1.1.path
@@ -32,7 +32,7 @@
   "constraintZones": [],
   "eventMarkers": [
     {
-      "name": "Intake",
+      "name": "intakeAndStop",
       "waypointRelativePos": 0,
       "command": {
         "type": "parallel",
@@ -41,7 +41,7 @@
             {
               "type": "named",
               "data": {
-                "name": "Intake"
+                "name": "intakeAndStop"
               }
             }
           ]

diff --git a/src/main/deploy/pathplanner/paths/middle-note1.2.path b/src/main/deploy/pathplanner/paths/middle-note1.2.path
@@ -41,7 +41,7 @@
             {
               "type": "named",
               "data": {
-                "name": "Intake"
+                "name": "intakeAndStop"
               }
             }
           ]

diff --git a/src/main/deploy/pathplanner/paths/middle-note1.3.path b/src/main/deploy/pathplanner/paths/middle-note1.3.path
@@ -23,32 +23,16 @@
         "x": 2.5792893218813453,
         "y": 4.470710678118655
       },
-      "nextControl": {
-        "x": 2.911629509039022,
-        "y": 4.138370490960978
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.2,
-        "y": 4.78
-      },
-      "prevControl": {
-        "x": 2.1292893218813456,
-        "y": 4.850710678118655
-      },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "note1.3-diag"
+      "linkedName": null
     }
   ],
   "rotationTargets": [],
   "constraintZones": [],
   "eventMarkers": [
     {
-      "name": "Intake",
+      "name": "intakeAndStop",
       "waypointRelativePos": 0,
       "command": {
         "type": "parallel",
@@ -57,7 +41,7 @@
             {
               "type": "named",
               "data": {
-                "name": "Intake"
+                "name": "intakeAndStop"
               }
             }
           ]

diff --git a/src/main/deploy/pathplanner/paths/note1.2-middle.path b/src/main/deploy/pathplanner/paths/note1.2-middle.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.55,
+        "y": 5.55
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.4694476010769155,
+        "y": 5.537726865962991
+      },
+      "isLocked": false,
+      "linkedName": "note1.2"
+    },
+    {
+      "anchor": {
+        "x": 1.4,
+        "y": 5.55
+      },
+      "prevControl": {
+        "x": 1.5166108734633559,
+        "y": 5.53796254771329
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "middle"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.6,
+    "maxAcceleration": 6.0,
+    "maxAngularVelocity": 720.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": true
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/...in/deploy/pathplanner/paths/New Path.path → ...loy/pathplanner/paths/note1.3-middle.path b/...in/deploy/pathplanner/paths/New Path.path → ...loy/pathplanner/paths/note1.3-middle.path
@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.0,
-        "y": 7.0
+        "x": 2.65,
+        "y": 4.4
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.0,
-        "y": 7.0
+        "x": 2.5769342825867363,
+        "y": 4.404664289380663
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 1.4,
+        "y": 5.55
       },
       "prevControl": {
-        "x": 3.0,
-        "y": 6.0
+        "x": 1.4791690231166925,
+        "y": 5.471893715633497
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "middle"
     }
   ],
   "rotationTargets": [],
@@ -39,8 +39,8 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0,
-    "rotateFast": false
+    "rotation": 180.0,
+    "rotateFast": true
   },
   "reversed": false,
   "folder": null,

diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java
@@ -35,34 +35,20 @@
 import frc.team3128.Constants.ShooterConstants;
 import frc.team3128.Robot;
 import frc.team3128.RobotContainer;
+import frc.team3128.commands.CmdManager;
 import frc.team3128.commands.CmdSwerveDrive;
 
 import java.util.function.DoubleSupplier;
 
 import frc.team3128.subsystems.Swerve;
+import frc.team3128.subsystems.Intake;
 
 /**
  * Store trajectories for autonomous. Edit points here. 
  * @author Daniel Wang
  */
 public class Trajectories {
 
-    //USED FOR HARDCODED SHOTS BITCHES
-    public enum ShootPosition {
-        // find values
-        WING(5.09),      //Change this for top 
-        BOTTOM(7.5),    //Change this for bottom auto
-        RAM(24.5);
-
-        private final double height;
-        ShootPosition(double height) {
-            this.height = height;
-        }
-        public double getHeight() {
-            return height;
-        }
-    }
-
     private static final Swerve swerve = Swerve.getInstance();
     private static double vx = 0, vy = 0;
     private static boolean turning = false;
@@ -71,20 +57,9 @@ public static void initTrajectories() {
         Pathfinding.setPathfinder(new LocalADStar());
 
         // TODO: add commands
-        // NamedCommands.registerCommand("Intake", intake.intakeAuto());
-        // NamedCommands.registerCommand("Shoot", autoShoot(0.75));
-        // NamedCommands.registerCommand("RamShoot", ramShotAuto());
-        // NamedCommands.registerCommand("BottomShoot", autoShootPreset(ShootPosition.BOTTOM));
-        // NamedCommands.registerCommand("WingRamp", rampUpAuto(ShootPosition.WING));
-        // NamedCommands.registerCommand("Align", align());
-        // NamedCommands.registerCommand("AlignCCW", alignSearch(true));
-        // NamedCommands.registerCommand("AlignCW", alignSearch(false));
-        // NamedCommands.registerCommand("Outtake", outtakeAuto());
-        // NamedCommands.registerCommand("Retract", intake.retractAuto());
-        // NamedCommands.registerCommand("Neutral", neutralAuto());
-        // NamedCommands.registerCommand("AlignPreload", alignPreload(false));
-        // NamedCommands.registerCommand("Drop", shooter.shoot(MAX_RPM));
-        // NamedCommands.registerCommand("RamShootMax", autoShootPreset(ShootPosition.RAM));
+        NamedCommands.registerCommand("ramShoot", CmdManager.ramShoot(true));
+        NamedCommands.registerCommand("ramShootNoStop", CmdManager.ramShootNoStop(true));
+        NamedCommands.registerCommand("intakeAndStop", CmdManager.intakeAndStop(Intake.Setpoint.GROUND));
 
         AutoBuilder.configureHolonomic(
             swerve::getPose,
@@ -108,7 +83,6 @@ public static Command resetAuto() {
             runOnce(()-> swerve.zeroGyro(Robot.getAlliance() == Alliance.Red ? 0 : 180)),
             runOnce(()-> swerve.resetEncoders())
         );
-
     }
 
     public static void drive(ChassisSpeeds velocity) {
@@ -130,7 +104,6 @@ public static Command turnDegrees(boolean counterClockwise, double angle) {
             setpoint, //setpoint
             (double output) -> {
                 Swerve.getInstance().drive(new ChassisSpeeds(vx, vy, Units.degreesToRadians(output)));
-                NAR_Shuffleboard.addData("HElp", "help", output, 0, 0);
             }
         ).beforeStarting(runOnce(()-> CmdSwerveDrive.disableTurn()));
     }
@@ -146,7 +119,6 @@ public static Command turnInPlace() {
             setpoint, //setpoint
             (double output) -> {
                 Swerve.getInstance().drive(new ChassisSpeeds(vx, vy, Units.degreesToRadians(output)));
-                NAR_Shuffleboard.addData("HElp", "help", output, 0, 0);
             }
         ).beforeStarting(runOnce(()-> CmdSwerveDrive.disableTurn()));
     }
@@ -157,11 +129,11 @@ public static Command getPathPlannerAuto(String name) {
 
     public static Command goToPoint(Pose2d pose) {
         return AutoBuilder.pathfindToPose(
-                pose,
-                AutoConstants.constraints,
-                0.0, // Goal end velocity in meters/sec
-                0.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
-            );
+            pose,
+            AutoConstants.constraints,
+            0.0, // Goal end velocity in meters/sec
+            0.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate.
+        );
     }
 
 }