start adding button bindings

team581 · Sep 6, 2023 · 6873242 · 6873242
1 parent 522edbc
commit 6873242
Show file tree

Hide file tree

Showing 5 changed files with 86 additions and 33 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -15,14 +15,20 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.autos.Autos;
 import frc.robot.config.Config;
 import frc.robot.controller.DriveController;
 import frc.robot.controller.RumbleControllerSubsystem;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.generated.BuildConstants;
 import frc.robot.imu.ImuSubsystem;
+import frc.robot.intake.HeldGamePiece;
 import frc.robot.intake.IntakeSubsystem;
+import frc.robot.managers.autobalance.Autobalance;
+import frc.robot.managers.autorotate.AutoRotate;
+import frc.robot.managers.superstructure.NodeHeight;
+import frc.robot.managers.superstructure.States;
 import frc.robot.managers.superstructure.SuperstructureManager;
 import frc.robot.managers.superstructure.SuperstructureMotionManager;
 import frc.robot.shoulder.ShoulderSubsystem;
@@ -76,7 +82,6 @@ public class Robot extends LoggedRobot {
       new RumbleControllerSubsystem(new XboxController(1));
 
   private final FmsSubsystem fmsSubsystem = new FmsSubsystem();
-  // TODO: Add all the subsystems & managers here
 
   private final Autos autos = new Autos();
 
@@ -90,12 +95,16 @@ public class Robot extends LoggedRobot {
       new IntakeSubsystem(new CANSparkMax(Config.INTAKE_ID, MotorType.kBrushless));
   private final SuperstructureMotionManager motionManager =
       new SuperstructureMotionManager(shoulder, wrist);
-  private final SuperstructureManager superstructureManager =
+  private final SuperstructureManager superstructure =
       new SuperstructureManager(motionManager, intake);
   private final ImuSubsystem imu =
       new ImuSubsystem(new Pigeon2(Config.PIGEON2_ID, Config.CANIVORE_ID));
   private final SwerveSubsystem swerve =
       new SwerveSubsystem(imu, frontRight, frontLeft, backRight, backLeft);
+
+  private final Autobalance autobalance = new Autobalance(swerve, imu);
+  private final AutoRotate autoRotate = new AutoRotate(swerve);
+
   private Command autoCommand;
 
   public Robot() {
@@ -144,16 +153,47 @@ public Robot() {
   public void robotInit() {}
 
   private void configureButtonBindings() {
+    swerve.setDefaultCommand(swerve.getDriveTeleopCommand(driveController));
+
     // TODO: Start adding button bindings
 
     // Driver controls
-
-    // Floor intake
-    // driveController.leftTrigger(0.3).onTrue();
-
-    // Set mode to cubes
+    // driveController.leftStick();
+    // driveController.rightStick();
+    driveController.leftTrigger().onTrue(superstructure.getIntakeFloorCommand());
+    driveController.leftBumper().onTrue(superstructure.getIntakeShelfCommand());
+    driveController.rightTrigger().onTrue(superstructure.getScoreFinishCommand());
+    driveController.rightBumper().onTrue(superstructure.getIntakeSingleSubstationCommand());
+    driveController.back().onTrue(imu.getZeroCommand());
+
+    driveController.povUp().onTrue(superstructure.setModeCommand(HeldGamePiece.CUBE));
+    driveController.povDown().onTrue(superstructure.setModeCommand(HeldGamePiece.CONE));
+    // Snaps for all cardinal directions
+    driveController.x().onTrue(autoRotate.getCommand(() -> AutoRotate.getLeftAngle()));
+    driveController.b().onTrue(autoRotate.getCommand(() -> AutoRotate.getRightAngle()));
+    driveController.y().onTrue(autoRotate.getCommand(() -> AutoRotate.getForwardAngle()));
+    driveController.a().onTrue(autoRotate.getCommand(() -> AutoRotate.getBackwardsAngle()));
+
+    new Trigger(() -> driveController.getThetaPercentage() == 0)
+        .onFalse(autoRotate.getDisableCommand());
+    // X swerve
+    driveController.start().onTrue(swerve.getXSwerveCommand());
+
+    new Trigger(
+            () ->
+                driveController.getSidewaysPercentage() == 0
+                    && driveController.getForwardPercentage() == 0
+                    && driveController.getThetaPercentage() == 0)
+        .onFalse(swerve.disableXSwerveCommand());
+
+    // Set ;mode to cubes
 
     // Operator controls
+    operatorController.y().onTrue(superstructure.getScoreAlignCommand(NodeHeight.HIGH));
+    operatorController.b().onTrue(superstructure.getScoreAlignCommand(NodeHeight.MID));
+    operatorController.a().onTrue(superstructure.setStateCommand(States.STOWED));
+    // operatorController.leftTrigger().onTrue();
+    // operatorController.rightTrigger().onTrue(autoCommand);
   }
 
   @Override

diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java
@@ -6,8 +6,13 @@
 
 public enum IntakeState {
   STOPPED,
+
   INTAKE_CONE,
   OUTTAKE_CONE,
+
   INTAKE_CUBE,
-  OUTTAKE_CUBE;
+  OUTTAKE_CUBE,
+
+  MANUAL_INTAKE,
+  MANUAL_OUTTAKE;
 }
diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java
@@ -20,6 +20,7 @@ public class SuperstructureManager extends LifecycleSubsystem {
   private IntakeSubsystem intake;
   private SuperstructureState goalState;
   private HeldGamePiece mode = HeldGamePiece.CUBE;
+  private NodeHeight scoringHeight = null;
 
   public SuperstructureManager(SuperstructureMotionManager motionManager, IntakeSubsystem intake) {
     super(SubsystemPriority.SUPERSTRUCTURE_MANAGER);
@@ -56,11 +57,22 @@ public void setMode(HeldGamePiece mode) {
     this.mode = mode;
   }
 
+  // create a set mode command
+  // which just calls set mode
+
+  public Command setModeCommand(HeldGamePiece mode) {
+    return runOnce(
+        () -> {
+          setMode(mode);
+        });
+  }
+
   public HeldGamePiece getMode() {
     return mode;
   }
 
   public Command getIntakeFloorCommand() {
+    // TODO: Refactor to use setStateCommand, since this command never finishes atm
     return Commands.run(
         () -> {
           if (mode == HeldGamePiece.CONE) {
@@ -72,6 +84,7 @@ public Command getIntakeFloorCommand() {
   }
 
   public Command getIntakeShelfCommand() {
+    // TODO: Refactor to use setStateCommand, since this command never finishes atm
     return Commands.run(
         () -> {
           if (mode == HeldGamePiece.CONE) {
@@ -83,7 +96,7 @@ public Command getIntakeShelfCommand() {
   }
 
   public Command getIntakeSingleSubstationCommand() {
-    return Commands.run(() -> setGoal(States.INTAKING_CONE_SINGLE_SUBSTATION));
+    return setStateCommand(States.INTAKING_CONE_SINGLE_SUBSTATION);
   }
 
   private SuperstructureScoringState getScoringState(NodeHeight height) {
@@ -109,16 +122,27 @@ private SuperstructureScoringState getScoringState(NodeHeight height) {
   }
 
   public Command getScoreAlignCommand(NodeHeight height) {
-    return Commands.run(
-        () -> {
-          setGoal(getScoringState(height).aligning);
-        });
+    return Commands.runOnce(
+            () -> {
+              scoringHeight = height;
+            })
+        .andThen(setStateCommand(getScoringState(height).aligning));
+  }
+
+  public Command getScoreFinishCommand() {
+    return getScoreFinishCommand(scoringHeight == null ? NodeHeight.LOW : scoringHeight);
   }
 
   public Command getScoreFinishCommand(NodeHeight height) {
-    return Commands.run(
-        () -> {
-          setGoal(getScoringState(height).scoring);
-        });
+    return Commands.runOnce(
+            () -> {
+              scoringHeight = height;
+            })
+        .andThen(setStateCommand(getScoringState(height).scoring))
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  scoringHeight = null;
+                }));
   }
 }
diff --git a/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java
@@ -9,7 +9,6 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxPIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import org.littletonrobotics.junction.Logger;
@@ -66,13 +65,6 @@ public void set(Rotation2d angle) {
     goalAngle = angle;
   }
 
-  public Command setPositionCommand(Rotation2d angle) {
-    return run(() -> {
-          set(angle);
-        })
-        .until(() -> atAngle(angle));
-  }
-
   public boolean atAngle(Rotation2d angle) {
     double actualAngle = getWristAngle().getDegrees();
     return Math.abs(actualAngle - angle.getDegrees()) < 1;

diff --git a/src/main/java/frc/robot/wrist/WristSubsystem.java b/src/main/java/frc/robot/wrist/WristSubsystem.java
@@ -9,7 +9,6 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxPIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import org.littletonrobotics.junction.Logger;
@@ -53,13 +52,6 @@ public void set(Rotation2d angle) {
     goalAngle = angle;
   }
 
-  public Command setPositionCommand(Rotation2d angle) {
-    return run(() -> {
-          set(angle);
-        })
-        .until(() -> atAngle(angle));
-  }
-
   public boolean atAngle(Rotation2d angle) {
     double actualAngle = getWristAngle().getDegrees();
     return Math.abs(actualAngle - angle.getDegrees()) < 1;