diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 73a7347..fa59748 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,7 @@ import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; +import com.studica.frc.AHRS; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DataLogManager; @@ -15,8 +16,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.config.ControllerConfig; import frc.robot.config.Positions; import frc.robot.subsystems.*; @@ -27,8 +28,8 @@ public class RobotContainer { private Coral coral = new Coral(); private Algae algae = new Algae(); private CommandXboxController controller = new CommandXboxController(0); - private CommandGenericHID totalController = new CommandGenericHID(1); private Drivetrain drivetrain = new Drivetrain(); + private final AHRS gyro = new AHRS(AHRS.NavXComType.kMXP_SPI); private AutoFactory autos = new AutoFactory( drivetrain::getPose, @@ -36,8 +37,11 @@ public class RobotContainer { drivetrain::followTrajectory, true, drivetrain); - private int lastButtonPressed; - // slows down robot durring pickup + // used to determine which section of the reef to score on, defaults at start to pole to front and + // left of driver + private int reefSection = 7; + + // slows down robot during pickup private Command pickup = coral .pickUpCoral() @@ -78,20 +82,37 @@ public RobotContainer() { // all controller bindings private void configureBindings() { - CommandScheduler.getInstance() - .schedule( - Commands.run( + // if left trigger is pressed, shifts selected reef section to score on left one, works while + // robot is disabled for autonomous + controller + .leftTrigger() + .onTrue( + Commands.runOnce( () -> { - for (int i = 0; i < 12; i++) { - if (totalController.getHID().getRawButtonPressed(i + 1)) { - lastButtonPressed = i; - } + if (reefSection == 11) { + reefSection = 0; + } else { + reefSection += 1; } }) .ignoringDisable(true)); - // coral pickup + // if right trigger is pressed, shifts selected reef section to score on right one, works while + // robot is disabled for autonomous controller - .a() + .rightTrigger() + .onTrue( + Commands.runOnce( + () -> { + if (reefSection == 0) { + reefSection = 11; + } else { + reefSection -= 1; + } + }) + .ignoringDisable(true)); + + // when right toggle goes up, cancels ground coral pickup or initiates ground coral pickup + new Trigger(() -> controller.getRightY() > 0.9) .onTrue( Commands.runOnce( () -> { @@ -106,9 +127,8 @@ private void configureBindings() { fieldRelative = false; } })); - // cancel coral pickup - controller - .b() + // when right toggle goes down, cancels station coral pickup or initiates station coral pickup + new Trigger(() -> controller.getRightY() < -0.9) .onTrue( Commands.runOnce( () -> { @@ -122,9 +142,9 @@ private void configureBindings() { speedMultiplier = 0.4; } })); - // enabling the safe state + // when upper plus button is pressed, cancels safe state or initiates safe state controller - .x() + .povUp() .onTrue( Commands.runOnce( () -> { @@ -135,76 +155,74 @@ private void configureBindings() { speedMultiplier = 1.0; } })); - - // picking up algae + // when x is pressed and if auto is disabled, raises elevator for l4 scoring, when left stick is + // pressed, completes scoring motion + // is auto is enabled, automatically goes to previously selected reef section and l4 scores controller - .rightTrigger() - .onTrue( - algae - .pickUpAlgae() - .until(() -> !controller.rightTrigger().getAsBoolean()) - .andThen(algae.storeAlgae())); - - // releasing the algae - controller.y().onTrue(algae.releaseAlgae().andThen(algae.returnToUp())); - - // scoring on all levels - totalController - .button(15) + .x() .whileTrue( Commands.deferredProxy( () -> { if (autoDisabled) { return Commands.startEnd( () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) - .withDeadline(coral.l4Score(controller.leftTrigger(), false)); + .withDeadline(coral.l4Score(controller.leftStick(), false)); } else { - return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 3); + return coral.goToReef(drivetrain, () -> reefSection, () -> 3); } })); - totalController - .button(16) + // when y is pressed and if auto is disabled, raises elevator for l3 scoring, when left stick is + // pressed, completes scoring motion + // is auto is enabled, automatically goes to previously selected reef section and l3 scores + controller + .y() .whileTrue( Commands.deferredProxy( () -> { if (autoDisabled) { return Commands.startEnd( () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) - .withDeadline(coral.l3Score(controller.leftTrigger())); + .withDeadline(coral.l3Score(controller.leftStick())); } else { - return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 2); + return coral.goToReef(drivetrain, () -> reefSection, () -> 2); } })); - totalController - .button(17) + // when a is pressed and if auto is disabled, raises elevator for l2 scoring, when left stick is + // pressed, completes scoring motion + // is auto is enabled, automatically goes to previously selected reef section and l2 scores + controller + .a() .whileTrue( Commands.deferredProxy( () -> { if (autoDisabled) { return Commands.startEnd( () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) - .withDeadline(coral.l2Score(controller.leftTrigger())); + .withDeadline(coral.l2Score(controller.leftStick())); } else { - return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 1); + return coral.goToReef(drivetrain, () -> reefSection, () -> 1); } })); - totalController - .button(18) + // when b is pressed and if auto is disabled, raises elevator for l1 scoring, when left stick is + // pressed, completes scoring motion + // is auto is enabled, automatically goes to previously selected reef section and l1 scores + controller + .b() .whileTrue( Commands.deferredProxy( () -> { if (autoDisabled) { return Commands.startEnd( () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) - .withDeadline(coral.l1Score(controller.leftTrigger())); + .withDeadline(coral.l1Score(controller.leftStick())); } else { - return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 0); + return coral.goToReef(drivetrain, () -> reefSection, () -> 0); } })); - // clearing algae off reef - totalController.button(14).onTrue(coral.l1ReefClear(controller.leftTrigger())); - totalController.button(13).onTrue(coral.l2ReefClear(controller.leftTrigger())); + // clearing algae off reef, down on plus for l1 clear, right on plus for l2 clear + controller.povDown().onTrue(coral.l1ReefClear(controller.leftStick())); + controller.povRight().onTrue(coral.l2ReefClear(controller.leftStick())); // driving the robot drivetrain.setDefaultCommand( @@ -218,16 +236,18 @@ private void configureBindings() { true), drivetrain)); - // deploy climber, releases algae too - controller.povUp().whileTrue(climb.pivotClimb()); + // deploy climber + controller.leftBumper().whileTrue(climb.pivotClimb()); + // release climber and deploy algae for climb unless not nearing end of match controller - .povDown() + .rightBumper() .whileTrue( climb .pivotRelease() .alongWith(algae.deployForClimb().unless(() -> DriverStation.getMatchTime() > 25))); - controller.povLeft().onTrue(Commands.runOnce(() -> autoDisabled = true)); - controller.povRight().onTrue(Commands.runOnce(() -> autoDisabled = true)); + // when left plus pressed, disable or enable auto, defaults at disabled + controller.povLeft().onTrue(Commands.runOnce(() -> autoDisabled = !autoDisabled)); + controller.back().onTrue(Commands.runOnce(() -> gyro.zeroYaw())); } // deadbands for driving @@ -237,12 +257,12 @@ private double applyDeadband(double value) { // sets the pose for auto-align public void periodic() { - selectedSpot.setRobotPose(Positions.getIndividualReef(lastButtonPressed)); + selectedSpot.setRobotPose(Positions.getIndividualReef(reefSection)); } // autonomous command public Command getAutonomousCommand() { return Commands.sequence( - Commands.waitSeconds(2), coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 3)); + Commands.waitSeconds(2), coral.goToReef(drivetrain, () -> reefSection, () -> 3)); } } diff --git a/src/main/java/frc/robot/config/CANMappings.java b/src/main/java/frc/robot/config/CANMappings.java index 759c332..533553f 100644 --- a/src/main/java/frc/robot/config/CANMappings.java +++ b/src/main/java/frc/robot/config/CANMappings.java @@ -14,8 +14,8 @@ public class CANMappings { public static final int ALGAE_WHEEL_ID = 13; public static final int ALGAE_ARM_ID = 12; - public static final int ELEVATOR_L_ID = 10; // red tape - public static final int ELEVATOR_R_ID = 11; + public static final int ELEVATOR_L_ID = 11; // red tape + public static final int ELEVATOR_R_ID = 10; public static final int CORAL_PIVOT_ID = 16; public static final int CLIMB_PIVOT_ID = 14; diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java index ee7cbc2..abc9c02 100644 --- a/src/main/java/frc/robot/subsystems/Coral.java +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -27,10 +27,10 @@ public class Coral extends SubsystemBase { // rotations for levels private Rotation2d[] coralScoringRotations = { - Rotation2d.fromDegrees(60), - Rotation2d.fromRadians(5.97), - Rotation2d.fromRadians(6.02), - Rotation2d.fromDegrees(64.0) + Rotation2d.fromRadians(1), + Rotation2d.fromRadians(1.7), + Rotation2d.fromRadians(1.7), + Rotation2d.fromRadians(0.3) }; private Rotation2d[] reefClearRotationsPrimary = { Rotation2d.fromDegrees(-35), Rotation2d.fromDegrees(-35) @@ -44,7 +44,7 @@ public Coral() { movementController.setTolerance(CoralConfig.POSITION_TOLERANCE, CoralConfig.VELOCITY_TOLERANCE); movementController.enableContinuousInput(-Math.PI, Math.PI); elevator.setDefaultCommand(elevator.setElevatorHeight(() -> 0.0)); - coralPivot.setDefaultCommand(coralPivot.setPivotAngle(() -> Rotation2d.fromDegrees(75))); + coralPivot.setDefaultCommand(coralPivot.setPivotAngle(() -> Rotation2d.fromRadians(0.35))); grabber.setDefaultCommand( Commands.run( () -> { @@ -62,6 +62,7 @@ private Command score(int idx, BooleanSupplier completeScore) { return Commands.waitSeconds(0.3) .andThen(coralPivot.setPivotAngle(() -> coralScoringRotations[idx])) .alongWith(elevator.setElevatorHeight(() -> elevatorScoringHeights[idx])) + .alongWith(Commands.run(grabber::run, grabber)) .withDeadline( Commands.waitUntil(elevator::isAtPosition) .andThen(Commands.waitUntil(coralPivot::isPivotReady), Commands.waitSeconds(0.8)) @@ -111,7 +112,7 @@ public Command l4Score(BooleanSupplier completeScore, boolean shouldHaveAutoTime if (elevator.getPosition() > 1.0 || shouldComplete) { return coralScoringRotations[3]; } else { - return coralScoringRotations[3].minus(Rotation2d.fromDegrees(15)); + return coralScoringRotations[3].plus(Rotation2d.fromDegrees(15)); } }) .alongWith( diff --git a/src/main/java/frc/robot/subsystems/CoralGrabber.java b/src/main/java/frc/robot/subsystems/CoralGrabber.java index 4ed3379..592e4f7 100644 --- a/src/main/java/frc/robot/subsystems/CoralGrabber.java +++ b/src/main/java/frc/robot/subsystems/CoralGrabber.java @@ -85,6 +85,6 @@ public void stop() { } // Stops motor public void run() { - motor.set(0.1); + motor.set(0.2); } // Sets motor speed to (roughly) 10% of max } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 38c8485..af710a4 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -42,6 +42,7 @@ public Elevator() { new SparkFlexConfig() .smartCurrentLimit(ElevatorConfig.ELEVATOR_CURRENT_LIMIT) .idleMode(ElevatorConfig.ELEVATOR_IDLE_MODE) + .inverted(false) .apply( new EncoderConfig() .positionConversionFactor(ElevatorConfig.ENCODER_POSITION_CONVERSION_FACTOR)