demo controls (all on one controller, lower dt speed)

hammerhead226 · Apr 27, 2024 · 4aa9ab0 · 4aa9ab0
1 parent 785426f
commit 4aa9ab0
Show file tree

Hide file tree

Showing 3 changed files with 120 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2024RobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 258;
-  public static final String GIT_SHA = "ad32eba6a15c1737452fd82c0f431628b1b01939";
-  public static final String GIT_DATE = "2024-04-18 21:52:01 EDT";
-  public static final String GIT_BRANCH = "worlds";
-  public static final String BUILD_DATE = "2024-04-19 18:02:45 EDT";
-  public static final long BUILD_UNIX_TIME = 1713564165427L;
+  public static final int GIT_REVISION = 259;
+  public static final String GIT_SHA = "785426ff47bb5b382c635b74ef8749ce89f480a3";
+  public static final String GIT_DATE = "2024-04-23 20:18:22 EDT";
+  public static final String GIT_BRANCH = "demo";
+  public static final String BUILD_DATE = "2024-04-26 21:11:51 EDT";
+  public static final long BUILD_UNIX_TIME = 1714180311387L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -506,12 +506,122 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
-    driverControls();
-    manipControls();
-
+    // driverControls();
+    // manipControls();
+    demoControls();
     // testControls();
   }
 
+  private void demoControls() {
+    drive.setDefaultCommand(
+        DriveCommands.joystickDrive(
+            drive,
+            () -> -driveController.getLeftY(),
+            () -> -driveController.getLeftX(),
+            () -> -driveController.getRightX()));
+
+    driveController
+        .start()
+        .onTrue(
+            Commands.runOnce(
+                    () ->
+                        drive.setPose(
+                            new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
+                    drive)
+                .ignoringDisable(true));
+
+    driveController
+        .rightBumper()
+        .onTrue(
+            new SequentialCommandGroup(
+                new InstantCommand(() -> climbStateMachine.setClimbState(CLIMB_STATES.NONE)),
+                new InstantCommand(() -> trapStateMachine.setTargetState(TRAP_STATES.PIVOT)),
+                new SetElevatorTarget(0, 1.5, elevator),
+                DriveCommands.intakeCommand(
+                    drive,
+                    shooter,
+                    pivot,
+                    intake,
+                    led,
+                    driveController,
+                    () -> -driveController.getLeftY(),
+                    () -> -driveController.getLeftX(),
+                    () -> -driveController.getRightX())));
+    driveController
+        .rightBumper()
+        .onFalse(
+            new InstantCommand(() -> led.setState(LED_STATE.BLUE))
+                .andThen(new InstantCommand(() -> shooter.setFeedersRPM(500)))
+                .andThen(new WaitCommand(0.02))
+                .andThen(
+                    new ConditionalCommand(
+                        new WaitCommand(0.24),
+                        new WaitCommand(0.06),
+                        () -> (shooter.getLastNoteState() == NoteState.CURRENT)))
+                .andThen(
+                    new ParallelCommandGroup(
+                            new InstantCommand(() -> intake.stopRollers(), intake),
+                            new InstantCommand(() -> shooter.stopFeeders()),
+                            new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot))
+                        .andThen(new PositionNoteInFeeder(shooter, intake))));
+
+    driveController.leftBumper().whileTrue(new PivotIntakeTele(pivot, intake, shooter, led, true));
+    driveController
+        .leftBumper()
+        .onFalse(
+            new InstantCommand(intake::stopRollers)
+                .andThen(new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot))
+                .andThen(new InstantCommand(() -> shooter.stopFeeders())));
+
+    driveController.rightTrigger().onTrue(shootCommands);
+    driveController
+        .rightTrigger()
+        .onFalse(
+            new InstantCommand(() -> shooter.stopFeeders(), shooter)
+                .andThen(new InstantCommand(() -> led.setState(LED_STATE.BLUE)))
+                .andThen(new WaitCommand(0.5))
+                .andThen(new InstantCommand(shooter::stopFlywheels))
+                .andThen(new InstantCommand(() -> shooter.turnOffFan(), shooter)));
+
+    driveController
+        .a()
+        .onTrue(
+            new InstantCommand(() -> pivot.setShootState(SHOOT_STATE.AMP))
+                .andThen(new ScoreAmp(elevator, pivot, shooter, drive)));
+
+    driveController
+        .a()
+        .onFalse(
+            new InstantCommand(() -> pivot.setShootState(SHOOT_STATE.AIMBOT))
+                .andThen(
+                    new SequentialCommandGroup(
+                        new InstantCommand(() -> shooter.turnOffFan()),
+                        new SetElevatorTarget(0, 0.5, elevator),
+                        new InstantCommand(() -> elevator.setConstraints(30, 85)),
+                        new InstantCommand(() -> shooter.stopFlywheels(), shooter),
+                        new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot))));
+
+    driveController
+        .b()
+        .onTrue(
+            new SequentialCommandGroup(
+                new InstantCommand(() -> climbStateMachine.setClimbState(CLIMB_STATES.NONE)),
+                new InstantCommand(() -> trapStateMachine.setTargetState(TRAP_STATES.PIVOT)),
+                new SetElevatorTarget(0, 1.5, elevator),
+                new InstantCommand(() -> pivot.setShootState(SHOOT_STATE.PIVOT_PRESET)),
+                new SetPivotTarget(45, pivot),
+                new InstantCommand(() -> shooter.setFlywheelRPMs(2000, 2800))));
+    driveController
+        .b()
+        .onFalse(
+            new InstantCommand(() -> led.setState(LED_STATE.BLUE))
+                .andThen(new InstantCommand(() -> pivot.setShootState(SHOOT_STATE.AIMBOT)))
+                .andThen(new InstantCommand(() -> shooter.stopFlywheels()))
+                .andThen(new SetPivotTarget(Constants.PivotConstants.STOW_SETPOINT_DEG, pivot)));
+
+    driveController.x().onTrue(climbCommands);
+  }
+
   private void testControls() {
     drive.setDefaultCommand(
         DriveCommands.joystickDrive(

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -60,7 +60,7 @@
 import org.littletonrobotics.junction.Logger;
 
 public class Drive extends SubsystemBase {
-  private static final double MAX_LINEAR_SPEED = Constants.SwerveConstants.MAX_LINEAR_SPEED * 0.86;
+  private static final double MAX_LINEAR_SPEED = Constants.SwerveConstants.MAX_LINEAR_SPEED * 0.45;
   private static final double TRACK_WIDTH_X = Constants.SwerveConstants.TRACK_WIDTH_X;
   private static final double TRACK_WIDTH_Y = Constants.SwerveConstants.TRACK_WIDTH_Y;
   private static final double DRIVE_BASE_RADIUS = Constants.SwerveConstants.DRIVE_BASE_RADIUS;