Merge pull request #20 from Spikes-2212-Programming-Guild/gil-path-pl…

…anner-swerve added commands to named commands
Spikes-2212-Programming-Guild · Feb 15, 2025 · 48997cf · 48997cf
2 parents 6718a09 + fe6d3b0
commit 48997cf
Showing 1 changed file with 32 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -7,22 +7,21 @@
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.commands.*;
+import frc.robot.subsystems.*;
 
 public class Robot extends TimedRobot {
 
+    private Elevator elevator;
+    private Storage storage;
+    private Gripper gripper;
+    private CoralJoint coralJoint;
+    private AlgaeJoint algaeJoint;
+
     @Override
     public void robotInit() {
-        NamedCommands.registerCommand("ElevateToL4", new InstantCommand());
-        NamedCommands.registerCommand("OuttakeCoralAngle", new InstantCommand());
-        NamedCommands.registerCommand("ReleaseCoral", new InstantCommand());
-        NamedCommands.registerCommand("ElevateToFeeder", new InstantCommand());
-        NamedCommands.registerCommand("IntakeCoralAngle", new InstantCommand());
-        NamedCommands.registerCommand("IntakeCoral", new InstantCommand());
-        NamedCommands.registerCommand("IntakeAlgaeAngle", new InstantCommand());
-        NamedCommands.registerCommand("ElevateToL3", new InstantCommand());
-        NamedCommands.registerCommand("TakeAlgae", new InstantCommand());
-        NamedCommands.registerCommand("PlaceAlgae", new InstantCommand());
+        getInstances();
+        registerNamedCommands();
     }
 
     @Override
@@ -79,4 +78,26 @@ public void simulationInit() {
     public void simulationPeriodic() {
 
     }
+
+    private void registerNamedCommands() {
+        NamedCommands.registerCommand("ElevateToL4", new MoveToHeight(elevator, Elevator.ElevatorLevel.L4));
+        NamedCommands.registerCommand("OuttakeCoralAngle",
+                new RotateStorage(coralJoint, CoralJoint.StoragePose.PLACEMENT));
+        NamedCommands.registerCommand("ReleaseCoral", new ReleaseCoral(storage));
+        NamedCommands.registerCommand("ElevateToFeeder", new MoveToHeight(elevator, Elevator.ElevatorLevel.FEEDER));
+        NamedCommands.registerCommand("IntakeCoralAngle", new RotateStorage(coralJoint, CoralJoint.StoragePose.INTAKE));
+        NamedCommands.registerCommand("IntakeCoral", new IntakeCoral(storage));
+        NamedCommands.registerCommand("IntakeAlgaeAngle", new RotateAlgaeJointToBottom(algaeJoint));
+        NamedCommands.registerCommand("ElevateToL3", new MoveToHeight(elevator, Elevator.ElevatorLevel.L3));
+        NamedCommands.registerCommand("TakeAlgae", new IntakeAlgae(gripper));
+        NamedCommands.registerCommand("PlaceAlgae", new ReleaseAlgae(gripper));
+    }
+
+    private void getInstances() {
+        elevator = Elevator.getInstance();
+        storage = Storage.getInstance();
+        gripper = Gripper.getInstance();
+        coralJoint = CoralJoint.getInstance();
+        algaeJoint = AlgaeJoint.getInstance();
+    }
 }