feat: autos enabled

flamingchickens1540 · Sep 27, 2024 · d784c8c · d784c8c
1 parent 89b2806
commit d784c8c
Show file tree

Hide file tree

Showing 10 changed files with 7,148 additions and 7,135 deletions.
diff --git a/paths.chor b/paths.chor
diff --git a/src/main/deploy/choreo/WeirdSourceLanePHGF.1.traj b/src/main/deploy/choreo/WeirdSourceLanePHGF.1.traj
diff --git a/src/main/deploy/choreo/WeirdSourceLanePHGF.2.traj b/src/main/deploy/choreo/WeirdSourceLanePHGF.2.traj
diff --git a/src/main/deploy/choreo/WeirdSourceLanePHGF.3.traj b/src/main/deploy/choreo/WeirdSourceLanePHGF.3.traj
diff --git a/src/main/deploy/choreo/WeirdSourceLanePHGF.traj b/src/main/deploy/choreo/WeirdSourceLanePHGF.traj
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -190,15 +190,16 @@ private void configureButtonBindings() {
 //            driver.leftTrigger().whileTrue(
 //                    new DriveWithCorrectionCommand(drivetrain, driver, ()->-noteVision.getLatestDetection().rotation().toRotation2d().getDegrees())
 //            );
-            driver.leftTrigger().whileTrue(
-                    new DriveWithCorrectionCommand2(drivetrain, driver, ()->-noteVision.getLatestDetection().rotation().toRotation2d().getDegrees())
-            );
+
         }
+        driver.rightTrigger().whileTrue(
+                new DriveWithCorrectionCommand2(drivetrain, driver, ()->-noteVision.getLatestDetection().rotation().toRotation2d().getDegrees())
+        );
 
 
 //        driver.povDown().and(() -> !DriverStation.isFMSAttached()).onTrue(Commands.runOnce(() -> drivetrain.setPose(new Pose2d(Units.inchesToMeters(260), Units.inchesToMeters(161.62), Rotation2d.fromRadians(0)))).ignoringDisable(true));
 
-        driver.rightTrigger(0.95).toggleOnTrue(counterShuffleDrive);
+        driver.leftTrigger(0.95).toggleOnTrue(counterShuffleDrive);
 
         driver.rightStick().onTrue(cancelAlignment);
 
@@ -336,40 +337,45 @@ private void configureAutoRoutines() {
                     )
             );
 
+            autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE)));
+
         }
-        autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE)));
         autos.addDefault(new AutoCommand("Dwayne :skull:"));
+        autos.add(new AutoCommand("Subwoofer Shot", ShootSequence.forAutoSubwoofer(shooter, indexer)));
 //        autos.addDefault(new ATestAuto(drivetrain, shooter, indexer));
-//        autos.add(new AmpLanePADEF(drivetrain, shooter, indexer));
-//        autos.add(new AmpLanePAEDF(drivetrain, shooter, indexer));
-//        autos.add(new AmpLanePAEDSprint(drivetrain, shooter, indexer));
+        autos.add(new AmpLanePADESprint(drivetrain, shooter, indexer));
+        autos.add(new AmpLanePAEDSprint(drivetrain, shooter, indexer));
+        autos.add(new AmpLanePADEF(drivetrain, shooter, indexer));
+        autos.add(new AmpLanePAEDF(drivetrain, shooter, indexer));
 ////        autos.add(new AutoCommand("SubwooferShot", new ShootSequence(shooter, indexer)));
 ////        autos.add(new DriveSinglePath("Taxi", drivetrain));
 ////        autos.add(new DriveSinglePath("Sprint", drivetrain));
 ////        autos.add(new DriveSinglePath("CenterLaneSprint", drivetrain, true, true));
-////        autos.add(new AmpLanePADESprint(drivetrain, shooter, indexer));
 ////        autos.add(new CenterLanePSubSprint(drivetrain, shooter, indexer));
 ////        autos.add(new CenterLanePCBADSprint(drivetrain, shooter, indexer));
-//        autos.add(new CenterLanePCBAFSprint(drivetrain, shooter, indexer));
-//        autos.add(new CenterLanePCBAFE(drivetrain, shooter, indexer));
-//        autos.add(new CenterLanePDEABC(drivetrain, shooter, indexer));
-//        autos.add(new CenterLanePCBAFG(drivetrain, shooter, indexer));
-//        autos.add(new CenterLanePCBAEF(drivetrain, shooter, indexer));
-//        autos.add(new CenterLanePCBAGF(drivetrain, shooter, indexer));
+        autos.add(new CenterLanePCBA(drivetrain, shooter, indexer));
+        autos.add(new CenterLanePCBFSprint(drivetrain, shooter, indexer));
+        autos.add(new CenterLanePCBAFSprint(drivetrain, shooter, indexer));
+        autos.add(new CenterLanePBFCA(drivetrain, shooter, indexer));
+        autos.add(new CenterLanePDEABC(drivetrain, shooter, indexer));
+        autos.add(new CenterLanePCBAEF(drivetrain, shooter, indexer));
+        autos.add(new CenterLanePCBAFE(drivetrain, shooter, indexer));
+        autos.add(new CenterLanePCBAGF(drivetrain, shooter, indexer));
+        autos.add(new CenterLanePCBAFG(drivetrain, shooter, indexer));
 //        autos.add(new CenterLanePCBADE(drivetrain, shooter, indexer));
 //        autos.add(new CenterLanePCBAED(drivetrain, shooter, indexer));
 //        autos.add(new CenterLanePCBA(drivetrain, shooter, indexer));
-////        autos.add(new CenterLanePCBA(drivetrain, shooter, indexer));
 ////        autos.add(new CenterLanePBDA(drivetrain, shooter, indexer));
 ////        autos.add(new CenterLanePSubCSubBSubASubFSub(drivetrain, shooter, indexer));
 //////        autos.add(new CenterLanePSubCSubBSubFSub(drivetrain, shooter, indexer));
 ////        autos.add(new CenterLanePSubCSubBSubASub(drivetrain, shooter, indexer));
-//        autos.add(new SourceLanePHGF(drivetrain, shooter, indexer));
-//        autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer));
-//        autos.add(new SourceLanePGFE(drivetrain, shooter, indexer));
-//        autos.add(new SourceLanePGFH(drivetrain, shooter, indexer));
+        autos.add(new SourceLanePGHSprint(drivetrain, shooter, indexer));
+        autos.add(new SourceLanePHGF(drivetrain, shooter, indexer));
+        autos.add(new SourceLanePGFE(drivetrain, shooter, indexer));
+        autos.add(new SourceLanePGFH(drivetrain, shooter, indexer));
+        autos.add(new SourceLaneCBASprint(drivetrain, shooter, indexer));
+        autos.add(new WeirdSourceLanePHGF(drivetrain, shooter, indexer));
 ////        autos.addDefault(new ATestAuto(drivetrain, shooter, indexer));
-//        autos.add(new AutoCommand("Subwoofer Shot", ShootSequence.forAutoSubwoofer(shooter, indexer)));
     }
 
 

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBFCA.java b/src/main/java/org/team1540/robot2024/commands/autos/CenterLanePBFCA.java
@@ -1,5 +1,6 @@
 package org.team1540.robot2024.commands.autos;
 
+import org.team1540.robot2024.commands.shooter.ShootSequence;
 import org.team1540.robot2024.util.auto.AutoCommand;
 import org.team1540.robot2024.subsystems.drive.Drivetrain;
 import org.team1540.robot2024.subsystems.shooter.Shooter;
@@ -19,6 +20,7 @@ public CenterLanePBFCA(Drivetrain drivetrain, Shooter shooter, Indexer indexer)
                 PathHelper.fromChoreoPath("CenterLanePBFCA.4")
         );
         addCommands(
+                ShootSequence.forAutoSubwoofer(shooter, indexer),
                 Commands.parallel(
                         new LeadingShootPrepare(drivetrain, shooter),
                         Commands.sequence(

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/SourceLaneCBASprint.java b/src/main/java/org/team1540/robot2024/commands/autos/SourceLaneCBASprint.java
@@ -27,7 +27,7 @@ public SourceLaneCBASprint(Drivetrain drivetrain, Shooter shooter, Indexer index
                                 createSegmentSequence(drivetrain, shooter, indexer, 1),
                                 createSegmentSequence(drivetrain, shooter, indexer, 2),
                                 createSegmentSequence(drivetrain, shooter, indexer, 3),
-                                createSegmentSequence(drivetrain, shooter, indexer, 4)
+                                getPath(4).getCommand(drivetrain)
                         )
                 )
         );

diff --git a/src/main/java/org/team1540/robot2024/commands/autos/WeirdSourceLanePHGF.java b/src/main/java/org/team1540/robot2024/commands/autos/WeirdSourceLanePHGF.java
@@ -1,5 +1,8 @@
 package org.team1540.robot2024.commands.autos;
 
+import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.commands.shooter.DefaultShooterCommand;
+import org.team1540.robot2024.commands.shooter.SpitShoot;
 import org.team1540.robot2024.util.auto.AutoCommand;
 import org.team1540.robot2024.subsystems.drive.Drivetrain;
 import org.team1540.robot2024.subsystems.shooter.Shooter;
@@ -18,6 +21,7 @@ public WeirdSourceLanePHGF(Drivetrain drivetrain, Shooter shooter, Indexer index
                 PathHelper.fromChoreoPath("WeirdSourceLanePHGF.3")
         );
         addCommands(
+                new SpitShoot(shooter, indexer, Constants.Shooter.Flywheels.LEFT_RPM, Constants.Shooter.Flywheels.RIGHT_RPM, 1, 1, 0.25),
                 Commands.parallel(
                         new LeadingShootPrepare(drivetrain, shooter),
                         Commands.sequence(

diff --git a/src/main/java/org/team1540/robot2024/commands/shooter/SpitShoot.java b/src/main/java/org/team1540/robot2024/commands/shooter/SpitShoot.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import org.team1540.robot2024.Constants;
 import org.team1540.robot2024.commands.indexer.IntakeAndFeed;
@@ -13,14 +14,14 @@
 import java.util.function.Supplier;
 
 public class SpitShoot extends ParallelCommandGroup {
-    public SpitShoot(Shooter shooter, Indexer indexer, double leftSetpoint, double rightSetpoint, double indexerPercent, double feederPercent) {
+    public SpitShoot(Shooter shooter, Indexer indexer, double leftSetpoint, double rightSetpoint, double indexerPercent, double feederPercent, double waitSeconds) {
         addCommands(
-                new IntakeAndFeed(indexer, ()->indexerPercent, ()->feederPercent),
+                Commands.waitSeconds(waitSeconds).andThen(new IntakeAndFeed(indexer, ()->indexerPercent, ()->feederPercent)),
                 new PrepareShooterCommand(shooter, ()-> new ShooterSetpoint(0, leftSetpoint, rightSetpoint))
         );
     }
 
     public SpitShoot(Shooter shooter, Indexer indexer){
-        this(shooter, indexer, Constants.Shooter.Flywheels.LEFT_RPM, Constants.Shooter.Flywheels.RIGHT_RPM, 1, 1);
+        this(shooter, indexer, Constants.Shooter.Flywheels.LEFT_RPM, Constants.Shooter.Flywheels.RIGHT_RPM, 1, 1, 0);
     }
 }