Merge branch 'main' of https://github.com/Team334/R2024

Team334 · Mar 28, 2024 · 7e6637b · 7e6637b
2 parents b3913b8 + 4d51116
commit 7e6637b
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 22 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
 import frc.robot.Constants.Presets;
@@ -141,17 +142,20 @@ private void configureBindings() {
     _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
     _operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.INTAKE));
 
-    _operatorController.R1().whileTrue(
-      Commands.parallel(
-        new SetShooter(_shooterSubsystem, () -> Presets.SHOOTER_AMP_HANDOFF),
-        new SetElevator(_elevatorSubsystem, () -> Presets.ELEVATOR_AMP_HANDOFF)
-      )
-    );
+    // _operatorController.R1().whileTrue(
+    //   Commands.parallel(
+    //     new SetShooter(_shooterSubsystem, () -> Presets.SHOOTER_AMP_HANDOFF),
+    //     new SetElevator(_elevatorSubsystem, () -> Presets.ELEVATOR_AMP_HANDOFF)
+    //   )
+    // );
 
     // driver bindings
     _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem));
     _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem));
     _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
+    _driveController.square().onTrue(
+      Commands.runOnce(() -> {AutonShoot.isIntaked = false; AutonShoot.isRevved = false;})
+    );
 
     // TESTING ONLY!!!
     _driveController.triangle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetGyro(180), _swerveSubsystem));

diff --git a/src/main/java/frc/robot/commands/auto/AutoAim.java b/src/main/java/frc/robot/commands/auto/AutoAim.java
@@ -24,7 +24,7 @@
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class AutoAim extends SequentialCommandGroup {
+public class AutoAim extends ParallelCommandGroup {
   /** 
    * Creates a new AutoAim.
    * 
@@ -59,11 +59,9 @@ private AutoAim(
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-      new ParallelCommandGroup(
-        new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce),
-        new SetShooter(shooter, shooterAngle, runOnce),
-        new SetElevator(elevator, elevatorHeight, runOnce)
-      ).beforeStarting(() -> UtilFuncs.SetLEDs(LEDState.AIM)).finallyDo(() -> UtilFuncs.SetLEDs(LEDState.DEFAULT))
+      // new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce),
+      new SetShooter(shooter, shooterAngle, runOnce),
+      new SetElevator(elevator, elevatorHeight, runOnce)
     );
   }
 

diff --git a/src/main/java/frc/robot/commands/auto/AutonShoot.java b/src/main/java/frc/robot/commands/auto/AutonShoot.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.intake.FeedActuate;
@@ -15,6 +16,7 @@
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
+import frc.robot.subsystems.IntakeSubsystem.ActuatorState;
 import frc.robot.subsystems.IntakeSubsystem.FeedMode;
 import frc.robot.subsystems.ShooterSubsystem.ShooterState;
 import frc.robot.utils.UtilFuncs;
@@ -41,16 +43,12 @@ public AutonShoot(
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
       new ParallelCommandGroup(
-        // This command will squeeze the note while revving up the shooter. It will only run if the note can't be shot right away. 
-        new ParallelCommandGroup(
-          new SpinShooter(shooter, ShooterState.SHOOT, true).andThen(new WaitCommand(2)).andThen(
-            () -> isRevved = true
-          ).onlyIf(() -> !isRevved),
-          new FeedActuate(intake, FeedMode.INTAKE).onlyIf(() -> !isIntaked).withTimeout(1)
-        ),
-
+        new SpinShooter(shooter, ShooterState.SHOOT, true).alongWith(new WaitCommand(2)).andThen(
+          () -> isRevved = true
+        ).andThen(new PrintCommand("DONE REVVING")).onlyIf(() -> !isRevved),
+        // new FeedActuate(intake, ActuatorState.STOWED, FeedMode.INTAKE).withTimeout(1),
         new AutoAim(swerve, shooter, elevator, leds)
-      ),
+      ).andThen(new PrintCommand("DONE PRE-SHOT")),
 
       new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(1),
       new SpinShooter(shooter, ShooterState.IDLE, true)

diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -232,7 +232,7 @@ else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDiff
       }
 
       SmartDashboard.putNumber("STD", xyStds);
-      System.out.println("UPDATING POSE");
+      // System.out.println("UPDATING POSE");
 
       _estimator.addVisionMeasurement(UtilFuncs.ToPose(llBotpose), _visionSubsystem.getLatency(), VecBuilder.fill(xyStds, xyStds, yawStd));
     }