Changes from South Florida Regional

.DataLogTool/datalogtool.json

@@ -0,0 +1,6 @@
+{
+  "download": {
+    "localDir": "C:\\Users\\StuyPulse\\Desktop\\Robot Code\\Izzi\\logs",
+    "serverTeam": "694"
+  }
+}

src/main/deploy/pathplanner/paths/Blay First Piece To C.path

@@ -0,0 +1,60 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.809086415893629,
+        "y": 4.817991630285255
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.037272222692593,
+        "y": 5.040180564691614
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.54,
+        "y": 4.0949044951475315
+      },
+      "prevControl": {
+        "x": 1.5404031317321896,
+        "y": 4.173355934674833
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "C"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "IntakeToShooter",
+      "waypointRelativePos": 0.0,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": []
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "ABCDE",
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/GShoot To H (GHF).path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 8.03782608491143,
-        "y": 0.0954900798690486
+        "x": 8.14162084164624,
+        "y": -0.14032985330927936
       },
       "prevControl": {
-        "x": 6.311005379081795,
-        "y": 0.8475356139984566
+        "x": 6.414800135816606,
+        "y": 0.6117156808201287
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/H To HShoot (HGF).path

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 8.03782608491143,
-        "y": 0.0954900798690486
+        "x": 8.14162084164624,
+        "y": -0.14032985330927936
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.828323542398316,
-        "y": 0.4841551611271623
+        "x": 5.932118299133126,
+        "y": 0.24833522794883434
       },
       "isLocked": false,
       "linkedName": "H"

src/main/deploy/pathplanner/paths/Start To H (HGF).path

@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.7730856827002284,
-        "y": 1.9701820808264021
+        "x": 3.658620903940146,
+        "y": 1.8329545447689473
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 8.03782608491143,
-        "y": 0.0954900798690486
+        "x": 8.14162084164624,
+        "y": -0.14032985330927936
       },
       "prevControl": {
-        "x": 5.93976336910103,
-        "y": 0.5228394692719653
+        "x": 5.120084675408034,
+        "y": -0.14032985330927936
       },
       "nextControl": null,
       "isLocked": false,

src/main/java/com/stuypulse/robot/Robot.java

@@ -80,7 +80,7 @@ public void disabledInit() {
         robot.intake.setIdleMode(IdleMode.kCoast);
         robot.conveyor.setIdleMode(IdleMode.kCoast);
 
-        // scheduler.schedule(new LEDSet(new LEDRainbow()));
+        scheduler.schedule(new LEDSet(new LEDRainbow()));
 
         SmartDashboard.putString("Robot State", "DISABLED");
     }
@@ -91,11 +91,11 @@ public void disabledPeriodic() {
             DriverStation.reportWarning("Amper IR sensor reporting note while disabled!", false);
         }
 
-        if (Settings.LED.LED_AUTON_TOGGLE.get()) {
-            scheduler.schedule(new LEDSet(new LEDAlign()));
-        } else {
-            scheduler.schedule(new LEDSet(new LEDAutonChooser()));
-        }
+        // if (Settings.LED.LED_AUTON_TOGGLE.get()) {
+        //     scheduler.schedule(new LEDSet(new LEDAlign()));
+        // } else {
+        //     scheduler.schedule(new LEDSet(new LEDAutonChooser()));
+        // }
 
         // reload whitelist in case of alliance change
         scheduler.schedule(new VisionReloadWhiteList());

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -26,19 +26,25 @@
 import com.stuypulse.robot.constants.LEDInstructions;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Amper.Lift;
 import com.stuypulse.robot.constants.Settings.Driver;
 import com.stuypulse.robot.constants.Settings.Swerve.Assist;
 import com.stuypulse.robot.subsystems.amper.Amper;
 import com.stuypulse.robot.subsystems.climber.*;
 import com.stuypulse.robot.subsystems.conveyor.Conveyor;
 import com.stuypulse.robot.subsystems.intake.Intake;
 import com.stuypulse.robot.subsystems.leds.LEDController;
+import com.stuypulse.robot.subsystems.leds.instructions.LEDPulseColor;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 import com.stuypulse.robot.subsystems.shooter.Shooter;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.robot.subsystems.vision.AprilTagVision;
 import com.stuypulse.robot.subsystems.vision.NoteVision;
+import com.stuypulse.robot.util.PathUtil;
+import com.stuypulse.robot.util.SLColor;
+import com.stuypulse.robot.util.ShooterSpeeds;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -98,12 +104,22 @@ private void configureButtonBindings() {
 
     private void configureDriverBindings() {
         // intaking with swerve pointing at note
+        // driver.getRightTriggerButton()
+        //     .whileTrue(new WaitCommand(Settings.Intake.TELEOP_DRIVE_STARTUP_DELAY)
+        //         .andThen(new IntakeAcquire()
+        //             .deadlineWith(new LEDSet(LEDInstructions.DARK_BLUE)))
+        //         .andThen(new BuzzController(driver)
+        //             .alongWith(new LEDSet(LEDInstructions.PICKUP)
+        //                 .withTimeout(3.0))))
+        //     .whileTrue(new SwerveDriveToNote());
+
+        // intaking
         driver.getRightTriggerButton()
-            .whileTrue(new WaitCommand(Settings.Intake.TELEOP_DRIVE_STARTUP_DELAY)
-                .andThen(new IntakeAcquire()
-                    .deadlineWith(new LEDSet(LEDInstructions.DARK_BLUE)))
-                .andThen(new BuzzController(driver)));
-            // .whileTrue(new SwerveDriveToNote());
+            .whileTrue(new IntakeAcquire()
+                    .deadlineWith(new LEDSet(LEDInstructions.DARK_BLUE))
+                .andThen(new BuzzController(driver)
+                    .alongWith(new LEDSet(LEDInstructions.PICKUP)
+                        .withTimeout(3.0))));
 
         driver.getLeftTriggerButton()
             .onTrue(new IntakeDeacquire())
@@ -116,44 +132,45 @@ private void configureDriverBindings() {
             .whileTrue(new WaitCommand(Settings.Shooter.TELEOP_SHOOTER_STARTUP_DELAY)
                 .andThen(new SwerveDriveToShoot()
                     .deadlineWith(new LEDSet(LEDInstructions.ASSIST_FLASH)))
-                .andThen(new ShooterWaitForTarget())
+                .andThen(new ShooterWaitForTarget()
+                    .withTimeout(1.5))
                 .andThen(new ConveyorShoot()))
             .onFalse(new ConveyorStop())
             .onFalse(new IntakeStop())
             .onFalse(new ShooterStop());
 
         // note to amper and align then score
         driver.getLeftBumper()
-            .whileTrue(new SwerveDriveAmpAlign())
+            .whileTrue(new AmpScoreRoutine())
             .onFalse(new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT))
             .onFalse(new AmperStop());
 
         // score speaker no align
         driver.getRightMenuButton()
             .onTrue(new ShooterPodiumShot())
-            .onTrue(new ShooterWaitForTarget().andThen(new ConveyorShoot()))
+            .onTrue(new ShooterWaitForTarget()
+                    .withTimeout(1.5)
+                .andThen(new ConveyorShoot()))
             .onFalse(new ConveyorStop())
             .onFalse(new IntakeStop())
             .onFalse(new ShooterStop());
 
         // score amp no align
         driver.getLeftMenuButton()
-            .whileTrue(new ConveyorToAmp()
+            .whileTrue(ConveyorToAmp.withCheckLift()
+                .andThen(AmperToHeight.untilDone(Lift.AMP_SCORE_HEIGHT))
                 .andThen(new AmperScore()))
-            .onFalse(new AmperStop());
-
-        // driver.getDPadUp()
-        //     .onTrue(new ClimberToTop());
-        // driver.getDPadDown()
-        //     .onTrue(new ClimberToBottom());
+            .onFalse(new AmperStop())
+            .onFalse(new AmperToHeight(Lift.MIN_HEIGHT));
 
+        driver.getDPadUp()
+            .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(0)));
         driver.getDPadRight()
-            .onTrue(new SwerveDriveResetHeading());
-
-        // driver.getRightButton()
-        //     .whileTrue(new ClimberSetupRoutine());
-        // driver.getBottomButton()
-        //     .whileTrue(new ClimberScoreRoutine());
+            .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(90)));
+        driver.getDPadDown()
+            .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(180)));
+        driver.getDPadLeft()
+            .onTrue(new SwerveDriveResetHeading(Rotation2d.fromDegrees(270)));
 
         driver.getTopButton()
             // on command start
@@ -190,10 +207,12 @@ private void configureOperatorBindings() {
         operator.getRightTriggerButton()
             .whileTrue(new IntakeAcquire()
                 .deadlineWith(new LEDSet(LEDInstructions.DARK_BLUE))
-                .andThen(new BuzzController(driver)));
+                .andThen(new BuzzController(driver)
+                    .alongWith(new LEDSet(LEDInstructions.PICKUP)
+                        .withTimeout(3.0))));
 
         operator.getLeftBumper()
-            .onTrue(ConveyorToAmp.withCheckLift())
+            .onTrue(new ConveyorToAmp())
             .onFalse(new ConveyorStop())
             .onFalse(new IntakeStop())
             .onFalse(new AmperStop());
@@ -210,13 +229,13 @@ private void configureOperatorBindings() {
         operator.getDPadDown()
             .whileTrue(new AmperLiftFineAdjust(operator));
 
-        // operator.getDPadRight()
-        //     .onTrue(new ClimberToTop());
-        // operator.getDPadLeft()
-        //     .onTrue(new ClimberToBottom());
+        operator.getDPadRight()
+            .onTrue(new GandalfToShoot())
+            .onFalse(new ConveyorStop());
+        operator.getDPadLeft()
+            .onTrue(new GandalfToAmp())
+            .onFalse(new ConveyorStop());
 
-        operator.getLeftButton()
-                .onTrue(new AmperToHeight(Settings.Amper.Lift.TRAP_SCORE_HEIGHT));
         operator.getRightButton()
                 .onTrue(new AmperToHeight(Settings.Amper.Lift.AMP_SCORE_HEIGHT));
         operator.getBottomButton()
@@ -227,7 +246,7 @@ private void configureOperatorBindings() {
             .whileTrue(new LEDSet(LEDInstructions.PULSE_PURPLE));
 
         operator.getLeftMenuButton()
-            .onTrue(new AmperIntake())
+            .onTrue(new AmperToConveyor())
             .onFalse(new AmperStop());
     }
 
@@ -240,14 +259,23 @@ public void configureAutons() {
 
         autonChooser.addOption("Mobility", new Mobility());
 
-        autonChooser.setDefaultOption("5 Piece CBAE", new FivePieceCBAE());
-        autonChooser.addOption("4 Piece CBA", new FourPieceCBA());
-        autonChooser.addOption("3 Piece CB", new ThreePieceCB());
-        autonChooser.addOption("2 Piece C", new TwoPieceC());
+        autonChooser.addOption("Blue 5 Piece CBAE", new FivePieceCBAE(
+            PathUtil.loadPaths("First Piece To C", "C to B", "B To A", "A To E", "E To Shoot")));
+
+        autonChooser.addOption("Red 5 Piece CBAE", new FivePieceCBAE(
+            PathUtil.loadPathsRed("First Piece To C", "C to B", "B To A", "A To E", "E To Shoot")));
+
+        autonChooser.addOption("Blue Blay 5 Piece CBAE", new BlayFivePieceCBAE(
+            PathUtil.loadPaths("Blay First Piece To C", "C to B", "B To A", "A To E", "E To Shoot")));
+
+        autonChooser.addOption("Red Blay 5 Piece CBAE", new BlayFivePieceCBAE(
+            PathUtil.loadPathsRed("Blay First Piece To C", "C to B", "B To A", "A To E", "E To Shoot")));
 
-        autonChooser.addOption("4 Piece HGF", new FourPieceHGF());
-        autonChooser.addOption("3 Piece HG", new ThreePieceHG());
-        autonChooser.addOption("2 Piece H", new TwoPieceH());
+        autonChooser.setDefaultOption("Blue 4 Piece HGF", new FourPieceHGF(
+            PathUtil.loadPaths("Start To H (HGF)", "H To HShoot (HGF)", "HShoot To G (HGF)", "G To Shoot (HGF)", "GShoot To F (HGF)", "F To Shoot (HGF)")));
+
+        autonChooser.addOption("Red 4 Piece HGF", new FourPieceHGF(
+            PathUtil.loadPathsRed("Start To H (HGF)", "H To HShoot (HGF)", "HShoot To G (HGF)", "G To Shoot (HGF)", "GShoot To F (HGF)", "F To Shoot (HGF)")));
 
         SmartDashboard.putData("Autonomous", autonChooser);
     }