Algae one piece center auto

Author: Anuraag3345

src/main/deploy/pathplanner/autos/One Piece Barge.auto

@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "OneP Center"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "One piece part 2 barge"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "One piece part 3 barge"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "One piece part 4 barge"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/One piece part 4 barge.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.408,
+        "y": 5.134
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.523031014796091,
+        "y": 6.049442112922044
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.041803278688524,
+        "y": 6.788165983606557
+      },
+      "prevControl": {
+        "x": 7.339851338398133,
+        "y": 6.1537541705130065
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.752,
+    "maxAcceleration": 5.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 178.91907581333933
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/team1403/robot/RobotContainer.java

@@ -207,19 +207,13 @@ private Command getAlignCommandCenter() {
     return Commands.sequence(
       Blackbox.setAligningCmd(true),
       new DeferredCommand(() -> {
-        
         Pose2d currentPose = m_swerve.getPose();
         Pose2d target = Blackbox.getNearestAlignPositionReef(currentPose);
-        if (target == null) return Commands.none();
-          
-        
+        if (target == null) return Commands.none(); 
           target = CougarUtil.addDistanceToPoseLeft(target,((m_coralIntake.getAlignOffset() - 0.201)) + 0.13);
           target = CougarUtil.addDistanceToPose(target, 0.15);
-        
-
         Command finalAlign = new AlignCommand(m_swerve, target);
         if (DriverStation.isAutonomous()) finalAlign = finalAlign.withTimeout(timeout);
-
         if(CougarUtil.getDistance(target, m_swerve.getPose()) > 0.2)
           return Commands.sequence(
             AutoBuilder.pathfindToPose(target, TunerConstants.kAutoAlignConstraints),
@@ -524,14 +518,22 @@ private void configureBindings() {
     NamedCommands.registerCommand("ReefAlignR", getAlignCommand(Blackbox.ReefSelect.RIGHT));
     NamedCommands.registerCommand("ReefAlignCenter", getAlignCommandCenter());
     NamedCommands.registerCommand("Loading", Blackbox.robotStateCmd(Blackbox.State.loading));
+    
     NamedCommands.registerCommand("Barge L3", Commands.sequence(
       Blackbox.robotStateCmd(State.MoveElevator),
-      new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.L3Algae).asProxy(), 
-      new WristCommand(m_wrist, Constants.Wrist.Setpoints.Source).asProxy()
-  ));
+        new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.L3Algae).asProxy(), 
+        new WristCommand(m_wrist, Constants.Wrist.Setpoints.Source).asProxy()));
+  
+    NamedCommands.registerCommand("Net Shot", Commands.sequence(
+      Blackbox.robotStateCmd(State.MoveElevator),
+        new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.Barge),
+        new WristCommand(m_wrist, Constants.Wrist.Setpoints.Barge)));
 
-  NamedCommands.registerCommand("Algae Harvest", 
-      new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.release).asProxy());
+    NamedCommands.registerCommand("Algae Harvest", 
+        new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.release).asProxy());
+    
+    NamedCommands.registerCommand("Algae Expel", 
+      new CoralIntakeSpeed(m_coralIntake, -Constants.CoralIntake.release).asProxy());
 
     NamedCommands.registerCommand("AutoWiggle", 
       Commands.sequence(
@@ -552,12 +554,11 @@ private void configureBindings() {
 
     m_autoChooser.addOption("MOVE AUTO ANYWHERE", AutoHelper.getMoveAuto(m_swerve));
     m_autoChooser.addOption("THREE PIECE BACK FINAL NON PROCESSOR SIDE UNTESTED", AutoHelper.getThreePieceBackNonProc(m_swerve));
-    m_autoChooser.addOption("ONE PIECE CENTER UNTESTED", AutoHelper.getOnePCenter(m_swerve));
+    m_autoChooser.addOption("ONE PIECE CENTER ALGAE UNTESTED", AutoHelper.getOnePCenterAlgae(m_swerve));
     m_autoChooser.addOption("THREE PIECE SIDE PROCESSOR UNTESTED", AutoHelper.getThreePieceSideProc(m_swerve));
     m_autoChooser.addOption("TWO PIECE PROCESSOR UNTESTED", AutoHelper.getTwoPieceProc(m_swerve));
     m_autoChooser.addOption("TWO PIECE PROCESSOR + ALGAE REMOVAL UNTESTED", AutoHelper.getTwoPieceProc_algaeRemoval(m_swerve));
     m_autoChooser.addOption("wiggle test", AutoHelper.wiggle(m_swerve));
-    m_autoChooser.addOption("One piece algae", AutoHelper.getOnePCenterAlgae(m_swerve));
     //m_autoChooser.addOption("Testing Auto Align", AutoHelper.testAutoAlign(m_swerve));
     //m_autoChooser.addOption("Test 2 Piece", AutoHelper.getTwoPieceProcTest(m_swerve));
 

src/main/java/team1403/robot/commands/auto/AutoHelper.java

@@ -34,7 +34,7 @@ public static Command getMoveAuto(SwerveSubsystem m_swerve) {
         });
     }
 
-    public static Command getOnePCenter(SwerveSubsystem m_swerve) {
+    public static Command getOnePCenterAlgae(SwerveSubsystem m_swerve) {
         try {
             return Commands.sequence(
                 Commands.parallel(
@@ -47,7 +47,6 @@ public static Command getOnePCenter(SwerveSubsystem m_swerve) {
                 NamedCommands.getCommand("ReefAlignR"),
                 NamedCommands.getCommand("WaitForSetpoint"),
                 NamedCommands.getCommand("CoralScore"),
-                Commands.waitSeconds(1),
                 NamedCommands.getCommand("Loading"),
                 AutoUtil.loadPathPlannerPath("One piece part 2 barge", m_swerve),
                 NamedCommands.getCommand("Barge L3"),
@@ -57,43 +56,21 @@ public static Command getOnePCenter(SwerveSubsystem m_swerve) {
                     Commands.sequence(
                         Commands.waitSeconds(3.0),
                         AutoUtil.loadPathPlannerPath("One piece part 3 barge", m_swerve),
-                        NamedCommands.getCommand("AutoBarge")
+                        NamedCommands.getCommand("Net Shot"),
+                        NamedCommands.getCommand("WaitForSetpoint"),
+                        NamedCommands.getCommand("Algae Expel")
                     )
                 ),
+                NamedCommands.getCommand("Loading"),
                 AutoUtil.loadPathPlannerPath("One piece part 4 barge", m_swerve)
-
             );
         } catch (Exception e) {
             System.err.println("Could not load auto: " + e.getMessage());
             return Commands.none();
         }
     }
 
-    public static Command getOnePCenterAlgae(SwerveSubsystem m_swerve) {
-        try {
-            return Commands.sequence(
-                Commands.parallel(
-                    AutoUtil.loadPathPlannerPath("OneP Center", m_swerve, true),
-                    Commands.sequence(
-                        Commands.waitSeconds(0.75),
-                        NamedCommands.getCommand("CoralL4"))
-                ),
-                NamedCommands.getCommand("ReefAlignR"),
-                NamedCommands.getCommand("WaitForSetpoint"),
-                NamedCommands.getCommand("CoralScore"),
-                Commands.waitSeconds(1),
-                NamedCommands.getCommand("Loading"),
-                AutoUtil.loadPathPlannerPath("One piece part 2 barge", m_swerve),
-                NamedCommands.getCommand("Barge L3")
-                // NamedCommands.getCommand("ReefAlignCenter"),
-                // NamedCommands.getCommand("Algae Harvest"),
-                // Commands.waitSeconds(4)      
-            );
-        } catch (Exception e) {
-            System.err.println("Could not load auto: " + e.getMessage());
-            return Commands.none();
-        }
-    }
+    
 
     public static Command getThreePieceSideProc(SwerveSubsystem m_swerve) {
         try {