Removed mega cooked autos. Gave up on note tracking 😭

TechnoTitans · Oct 11, 2024 · 5dfb70e · 5dfb70e
1 parent f8ed1c2
commit 5dfb70e
Show file tree

Hide file tree

Showing 11 changed files with 6,197 additions and 6,233 deletions.
diff --git a/choreo.chor b/choreo.chor
diff --git a/src/main/deploy/choreo/AmpCenter3_2Note.1.traj b/src/main/deploy/choreo/AmpCenter3_2Note.1.traj
diff --git a/src/main/deploy/choreo/AmpCenter3_2Note.2.traj b/src/main/deploy/choreo/AmpCenter3_2Note.2.traj
diff --git a/src/main/deploy/choreo/AmpCenter3_2Note.traj b/src/main/deploy/choreo/AmpCenter3_2Note.traj
diff --git a/src/main/deploy/choreo/Speaker2_1_0.1.traj b/src/main/deploy/choreo/Speaker2_1_0.1.traj
diff --git a/src/main/deploy/choreo/Speaker2_1_0.2.traj b/src/main/deploy/choreo/Speaker2_1_0.2.traj
diff --git a/src/main/deploy/choreo/Speaker2_1_0.3.traj b/src/main/deploy/choreo/Speaker2_1_0.3.traj
diff --git a/src/main/deploy/choreo/Speaker2_1_0.traj b/src/main/deploy/choreo/Speaker2_1_0.traj
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -354,12 +354,12 @@ public void configureAutos() {
         autoChooser.addAutoOption(new AutoOption(
                 "Speaker2_1_0",
                 autos.speaker2_1_0(),
-                Constants.CompetitionType.COMPETITION
+                Constants.CompetitionType.TESTING
         ));
         autoChooser.addAutoOption(new AutoOption(
                 "Speaker0_1_2",
                 autos.speaker0_1_2(),
-                Constants.CompetitionType.COMPETITION
+                Constants.CompetitionType.TESTING
         ));
         autoChooser.addAutoOption(new AutoOption(
                 "Speaker0_1_2Center4_3",
@@ -463,34 +463,34 @@ public void configureAutos() {
                 autos.walton(),
                 Constants.CompetitionType.TESTING
         ));
-        autoChooser.addAutoOption(new AutoOption(
-                "FollowNoteAmp",
-                autos.driveAndNoteDetect(),
-                Constants.CompetitionType.TESTING
-        ));
-        autoChooser.addAutoOption(new AutoOption(
-                "FollowNoteSource",
-                autos.AltSourceCenter0_1_2NOTE(),
-                Constants.CompetitionType.TESTING
-        ));
+//        autoChooser.addAutoOption(new AutoOption(
+//                "FollowNoteAmp",
+//                autos.driveAndNoteDetect(),
+//                Constants.CompetitionType.TESTING
+//        ));
+//        autoChooser.addAutoOption(new AutoOption(
+//                "FollowNoteSource",
+//                autos.AltSourceCenter0_1_2NOTE(),
+//                Constants.CompetitionType.TESTING
+//        ));
     }
 
     @SuppressWarnings("RedundantSuppression")
     public void configureButtonBindings(final EventLoop teleopEventLoop) {
         //noinspection SuspiciousNameCombination
-//        this.driverController.leftTrigger(0.5, teleopEventLoop)
-//                .whileTrue(Commands.parallel(
-//                        intake.intakeCommand().asProxy(),
-//                        swerve.teleopDriveAndAssistLineup(
-//                                driverController::getLeftY,
-//                                driverController::getLeftX,
-//                                driverController::getRightX,
-//                                IsRedAlliance,
-//                                () -> photonVision.getBestNotePose(swerve::getPose)
-//                        )
-//                ));
         this.driverController.leftTrigger(0.5, teleopEventLoop)
-                .whileTrue(intake.intakeCommand());
+                .whileTrue(Commands.parallel(
+                        intake.intakeCommand().asProxy(),
+                        swerve.teleopDriveAndAssistLineup(
+                                driverController::getLeftY,
+                                driverController::getLeftX,
+                                driverController::getRightX,
+                                IsRedAlliance,
+                                () -> photonVision.getBestNotePose(swerve::getPose)
+                        )
+                ));
+//        this.driverController.leftTrigger(0.5, teleopEventLoop)
+//                .whileTrue(intake.intakeCommand());
 
         // TODO: this doesn't rumble early enough, or as early as we'd like it to
         //  not sure if we're hardware limited or its behind by a few cycles and we can speed it up

diff --git a/src/main/java/frc/robot/auto/Autos.java b/src/main/java/frc/robot/auto/Autos.java
@@ -24,6 +24,7 @@
 
 import java.util.List;
 import java.util.Set;
+import java.util.concurrent.atomic.AtomicReference;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
@@ -392,45 +393,39 @@ public EventLoop speaker2_1_0() {
                 ).withName("Intake0")
         );
 
-        autoTriggers.atTime(1.25).onTrue(
+        autoTriggers.atTime(1).onTrue(
                 Commands.sequence(
-//                        Commands.waitUntil(noteState.isStored)
-//                                .withTimeout(2),
-                        Commands.waitSeconds(0.5),
+                        Commands.waitSeconds(1),
                         shootCommands.deferredStopAimAndShoot().withName("Shoot0").asProxy(),
                         followPath(autoTriggers.trajectories.get(1), timer).asProxy()
                 ).withName("Shoot0AndFollow1")
         );
 
-        autoTriggers.atTime(2).onTrue(
+        autoTriggers.atTime(2.3).onTrue(
                 Commands.parallel(
                         intake.intakeCommand(),
                         shootCommands.readySuperstructureForShot()
                 ).withName("Intake1")
         );
 
-        autoTriggers.atTime(3.87).onTrue(
+        autoTriggers.atTime(4.03).onTrue(
                 Commands.sequence(
-//                        Commands.waitUntil(noteState.isStored)
-//                                .withTimeout(2),
-                        Commands.waitSeconds(0.5),
+                        Commands.waitSeconds(1),
                         shootCommands.deferredStopAimAndShoot().withName("Shoot1").asProxy(),
                         followPath(autoTriggers.trajectories.get(2), timer).asProxy()
                 ).withName("Shoot1AndFollow2")
         );
 
-        autoTriggers.atTime(4.3).onTrue(
+        autoTriggers.atTime(4.6).onTrue(
                 Commands.parallel(
                         intake.intakeCommand(),
                         shootCommands.readySuperstructureForShot()
                 ).withName("Intake2")
         );
 
-        autoTriggers.atTime(6.02).onTrue(
+        autoTriggers.atTime(6.03).onTrue(
                 Commands.sequence(
-//                        Commands.waitUntil(noteState.isStored)
-//                                .withTimeout(2),
-                        Commands.waitSeconds(0.5),
+                        Commands.waitSeconds(1),
                         shootCommands.deferredStopAimAndShoot().withName("Shoot2").asProxy()
                 ).withName("Shoot2")
         );
@@ -1253,22 +1248,19 @@ public EventLoop driveAndNoteDetect() {
         final String trajectoryName = "AmpCenter3_2Note";
         final Timer timer = new Timer();
         final AutoTriggers autoTriggers = new AutoTriggers(trajectoryName, swerve::getPose, timer::get);
+        final AtomicReference<Pose2d> preNoteTrackPose = new AtomicReference<>(new Pose2d());
 
         autoTriggers.autoEnabled().whileTrue(preloadSubwooferAndFollow0(autoTriggers.trajectories, timer));
 
-        Logger.recordOutput("OInitial pose", autoTriggers.trajectories.get(0).getInitialPose());
-        Logger.recordOutput("Initial pose", autoTriggers.trajectories.get(0).getFlippedInitialPose());
-
-        autoTriggers.atTime(2.23).onTrue(
+        autoTriggers.atTime(2.35).onTrue(
                 Commands.parallel(
                         intake.intakeCommand().asProxy(),
                         Commands.sequence(
+                                Commands.runOnce(() -> preNoteTrackPose.set(swerve.getPose())),
                                 swerve.driveToOptionalPose(() -> photonVision.getBestNotePose(swerve::getPose))
                                         .until(noteState.hasNote),
                                 swerve.driveToPose(
-                                        Robot.IsRedAlliance.getAsBoolean()
-                                                ? autoTriggers.trajectories.get(1)::getFlippedInitialPose
-                                                : autoTriggers.trajectories.get(1)::getInitialPose,
+                                        preNoteTrackPose::get,
                                         new Pose2d(1, 1, Rotation2d.fromDegrees(6))
                                 ),
                                 followPath(autoTriggers.trajectories.get(1), timer)
@@ -1280,7 +1272,7 @@ public EventLoop driveAndNoteDetect() {
                 ).withName("Intake0")
         );
 
-        autoTriggers.atTime(3.37).onTrue(
+        autoTriggers.atTime(3.76).onTrue(
                 Commands.sequence(
                         shootCommands.deferredStopAimAndShoot()
                                 .onlyIf(noteState.hasNote)

diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
@@ -11,7 +11,7 @@
 
 public interface Constants {
     RobotMode CURRENT_MODE = RobotMode.REAL;
-    CompetitionType CURRENT_COMPETITION_TYPE = CompetitionType.TESTING;
+    CompetitionType CURRENT_COMPETITION_TYPE = CompetitionType.COMPETITION;
     double LOOP_PERIOD_SECONDS = 0.02;
 
     enum RobotMode {