setup auto trajectories

BitBucketsFRC4183 · Jan 25, 2025 · 478be81 · 478be81
1 parent e5f37f9
commit 478be81
Show file tree

Hide file tree

Showing 3 changed files with 123 additions and 0 deletions.
diff --git a/choreopaths/TwoL4COralBottom.traj → choreopaths/TwoL4CoralBottom.traj b/choreopaths/TwoL4COralBottom.traj → choreopaths/TwoL4CoralBottom.traj
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -105,6 +105,7 @@ public Robot() {
   @Override
   public void autonomousInit() {
 
+    autonomousCommand = robotContainer.getAutonomousCommand();
     System.out.println(Arrays.toString(Choreo.availableTrajectories()));
     var trajectory = loadTrajectory("test");
     if (trajectory.isPresent()) {

diff --git a/src/main/java/frc/robot/subsystems/Auto/AutoSubsystem.java b/src/main/java/frc/robot/subsystems/Auto/AutoSubsystem.java
@@ -21,6 +21,7 @@ public class AutoSubsystem extends SubsystemBase {
     private final AutoFactory autoFactory;
 
 
+
     public AutoSubsystem(ClawSubsystem claw,
                          DriveSubsystem drive){
         this.drive = drive;
@@ -46,6 +47,127 @@ public Command claw() {
         );
     }
 
+    public AutoRoutine WaitTaxiBottom(){
+
+    }
+    public AutoRoutine TwoL4CoralTop(){
+
+    }
+    public AutoRoutine TwoL4CoralBottom(){
+
+    }
+    public AutoRoutine ThreeL4CoralTop(){
+
+    }
+    public AutoRoutine ThreeL4CoralBottom(){
+
+    }
+    public AutoRoutine ThreeL4CoralTop(){
+
+    }
+    public AutoRoutine FourL4CoralTop(){
+        var trajectory = loadTrajectory(
+                "FourL4CoralTop");
+
+        AutoRoutine routine =
+                autoFactory.newRoutine(
+                        "FourL4CoralTop");
+
+        // Initialize
+        AutoTrajectory W1toW2toDeposit =
+                routine.trajectory("W1toW2toDeposit");
+
+        AutoTrajectory W2toW3toW4toClaw =
+                routine.trajectory("W2toW3toW4toClaw");
+
+        //** AutoTrajectory W3toW4 = routine
+        // .trajectory();
+        //        AutoTrajectory W4toClaw =
+        //                routine.trajectory();
+
+        AutoTrajectory W4toW5toW6toDeposit =
+                routine.trajectory("W4toW5toW6toDeposit");
+        //AutoTrajectory W5toW6 =
+        //                routine.trajectory();
+        //        AutoTrajectory W6toDeposit =
+        //                routine.trajectory();
+
+        AutoTrajectory W6toW7toW8toClaw =
+                routine.trajectory(
+                        "W6toW7toW8toClaw");
+
+        //AutoTrajectory W7toW8 =
+        //                routine.trajectory();
+        //        AutoTrajectory W8toClaw =
+        //                routine.trajectory();
+
+        AutoTrajectory W8toW9toDeposit =
+                routine.trajectory("W8toW9toDeposit");
+
+        //AutoTrajectory W9toDeposit =
+        //                routine.trajectory();
+
+        AutoTrajectory W9toW10toClaw =
+                routine.trajectory("W9toW10toClaw");
+
+        //AutoTrajectory W10toClaw =
+        //                routine.trajectory();
+
+        AutoTrajectory W10toW11toDeposit =
+                routine.trajectory("W10toW11toDeposit");
+
+        //AutoTrajectory W11toDeposit =
+        //                routine.trajectory();
+//
+//
+//
+//
+        routine.active().onTrue(
+                Commands.sequence(
+                        Commands.print("Started" +
+                                " the routine!"),
+                        W1toW2toDeposit.resetOdometry(),
+                        W1toW2toDeposit.cmd(),
+                        W2toW3toW4toClaw.cmd(),
+                        W4toW5toW6toDeposit.cmd(),
+                        W6toW7toW8toClaw.cmd(),
+                        W8toW9toDeposit.cmd(),
+                        W9toW10toClaw.cmd(),
+                        W10toW11toDeposit.cmd()
+                )
+        );
+        W1toW2toDeposit.active().whileTrue(drive());
+        W1toW2toDeposit.done().onTrue(deposit().andThen(W2toW3toW4toClaw.cmd()));
+
+        W2toW3toW4toClaw.active().whileTrue(drive());
+        W2toW3toW4toClaw.done().onTrue(claw().andThen(W4toW5toW6toDeposit.cmd()));
+
+        W4toW5toW6toDeposit.active().whileTrue(drive());
+        W4toW5toW6toDeposit.done().onTrue(deposit().andThen(W6toW7toW8toClaw.cmd()));
+
+        W6toW7toW8toClaw.active().whileTrue(drive());
+        W6toW7toW8toClaw.done().onTrue(claw().andThen(W8toW9toDeposit.cmd()));
+
+        W8toW9toDeposit.active().whileTrue(drive());
+        W8toW9toDeposit.done().onTrue(deposit().andThen(W9toW10toClaw.cmd()));
+
+        W9toW10toClaw.active().whileTrue(drive());
+        W9toW10toClaw.done().onTrue(claw().andThen(W10toW11toDeposit.cmd()));
+
+        W10toW11toDeposit.active().whileTrue(drive());
+        W10toW11toDeposit.done().onTrue(deposit());
+
+        //TODO
+
+
+        routine.anyActive(W1toW2toDeposit,
+                W4toW5toW6toDeposit,
+                W8toW9toDeposit,
+                W10toW11toDeposit).whileTrue(deposit());
+
+
+        return routine;
+    }
 
     public AutoRoutine FourL4CoralBottom() {