From 6873242e062bbf9e5bc53089da690e2446b18eef Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Tue, 5 Sep 2023 18:11:26 -0700 Subject: [PATCH] start adding button bindings --- src/main/java/frc/robot/Robot.java | 54 ++++++++++++++++--- .../java/frc/robot/intake/IntakeState.java | 7 ++- .../superstructure/SuperstructureManager.java | 42 +++++++++++---- .../frc/robot/shoulder/ShoulderSubsystem.java | 8 --- .../java/frc/robot/wrist/WristSubsystem.java | 8 --- 5 files changed, 86 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 22d25c7..30d62d8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,6 +15,7 @@ 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; @@ -22,7 +23,12 @@ 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 index e12eb08..f7c4d3b 100644 --- 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 index 9daff0d..efa606e 100644 --- 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 index 6aeb06c..0c601c6 100644 --- 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 index dbf12b0..7351ccd 100644 --- 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;