From ccba9f76009ea0ce38ea7c663b991bca33e095bf Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Mon, 15 Sep 2025 09:28:22 -0400 Subject: [PATCH 1/9] controller edits --- src/main/java/frc/robot/RobotContainer.java | 50 ++++++++++----------- 1 file changed, 24 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 73a7347..e3b882f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ 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.*; @@ -37,7 +38,7 @@ public class RobotContainer { true, drivetrain); private int lastButtonPressed; - // slows down robot durring pickup + // slows down robot during pickup private Command pickup = coral .pickUpCoral() @@ -78,7 +79,7 @@ public RobotContainer() { // all controller bindings private void configureBindings() { - CommandScheduler.getInstance() + /*CommandScheduler.getInstance() .schedule( Commands.run( () -> { @@ -88,10 +89,10 @@ private void configureBindings() { } } }) - .ignoringDisable(true)); + .ignoringDisable(true));*/ // coral pickup - controller - .a() + CommandScheduler.getInstance().schedule(Commands.run(()->{}).ignoringDisable(true)); + new Trigger(() -> controller.getRightY() > 0.9) .onTrue( Commands.runOnce( () -> { @@ -107,8 +108,7 @@ private void configureBindings() { } })); // cancel coral pickup - controller - .b() + new Trigger(() -> controller.getRightY() < -0.9) .onTrue( Commands.runOnce( () -> { @@ -124,7 +124,7 @@ private void configureBindings() { })); // enabling the safe state controller - .x() + .povUp() .onTrue( Commands.runOnce( () -> { @@ -136,6 +136,7 @@ private void configureBindings() { } })); + /* // picking up algae controller .rightTrigger() @@ -147,64 +148,61 @@ private void configureBindings() { // releasing the algae controller.y().onTrue(algae.releaseAlgae().andThen(algae.returnToUp())); + */ // scoring on all levels - totalController - .button(15) + controller.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); } })); - totalController - .button(16) + 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); } })); - totalController - .button(17) + 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); } })); - totalController - .button(18) + 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); } })); // clearing algae off reef - totalController.button(14).onTrue(coral.l1ReefClear(controller.leftTrigger())); - totalController.button(13).onTrue(coral.l2ReefClear(controller.leftTrigger())); + controller.povDown().onTrue(coral.l1ReefClear(controller.leftStick())); + controller.povRight().onTrue(coral.l2ReefClear(controller.leftStick())); // driving the robot drivetrain.setDefaultCommand( @@ -219,15 +217,15 @@ private void configureBindings() { drivetrain)); // deploy climber, releases algae too - controller.povUp().whileTrue(climb.pivotClimb()); + controller.leftBumper().whileTrue(climb.pivotClimb()); 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)); + controller.povLeft().onTrue(Commands.runOnce(() -> autoDisabled = !autoDisabled)); + //controller.povRight().onTrue(Commands.runOnce(() -> autoDisabled = true)); } // deadbands for driving From 165f8c3f40478ee31bf690bcddba053b6c9594f4 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Mon, 15 Sep 2025 14:48:50 -0400 Subject: [PATCH 2/9] formatting --- src/main/java/frc/robot/RobotContainer.java | 40 +++++++++++---------- 1 file changed, 22 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e3b882f..4dffcd0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,19 +80,19 @@ public RobotContainer() { // all controller bindings private void configureBindings() { /*CommandScheduler.getInstance() - .schedule( - Commands.run( - () -> { - for (int i = 0; i < 12; i++) { - if (totalController.getHID().getRawButtonPressed(i + 1)) { - lastButtonPressed = i; - } - } - }) - .ignoringDisable(true));*/ + .schedule( + Commands.run( + () -> { + for (int i = 0; i < 12; i++) { + if (totalController.getHID().getRawButtonPressed(i + 1)) { + lastButtonPressed = i; + } + } + }) + .ignoringDisable(true));*/ // coral pickup - CommandScheduler.getInstance().schedule(Commands.run(()->{}).ignoringDisable(true)); - new Trigger(() -> controller.getRightY() > 0.9) + CommandScheduler.getInstance().schedule(Commands.run(() -> {}).ignoringDisable(true)); + new Trigger(() -> controller.getRightY() > 0.9) .onTrue( Commands.runOnce( () -> { @@ -108,7 +108,7 @@ private void configureBindings() { } })); // cancel coral pickup - new Trigger(() -> controller.getRightY() < -0.9) + new Trigger(() -> controller.getRightY() < -0.9) .onTrue( Commands.runOnce( () -> { @@ -151,7 +151,8 @@ private void configureBindings() { */ // scoring on all levels - controller.x() + controller + .x() .whileTrue( Commands.deferredProxy( () -> { @@ -163,7 +164,8 @@ private void configureBindings() { return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 3); } })); - controller.y() + controller + .y() .whileTrue( Commands.deferredProxy( () -> { @@ -175,7 +177,8 @@ private void configureBindings() { return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 2); } })); - controller.a() + controller + .a() .whileTrue( Commands.deferredProxy( () -> { @@ -187,7 +190,8 @@ private void configureBindings() { return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 1); } })); - controller.b() + controller + .b() .whileTrue( Commands.deferredProxy( () -> { @@ -225,7 +229,7 @@ private void configureBindings() { .pivotRelease() .alongWith(algae.deployForClimb().unless(() -> DriverStation.getMatchTime() > 25))); controller.povLeft().onTrue(Commands.runOnce(() -> autoDisabled = !autoDisabled)); - //controller.povRight().onTrue(Commands.runOnce(() -> autoDisabled = true)); + // controller.povRight().onTrue(Commands.runOnce(() -> autoDisabled = true)); } // deadbands for driving From 60fc5fb887c89876d35c55dc64fd1478ed2a137a Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 19 Sep 2025 08:51:27 -0400 Subject: [PATCH 3/9] trigger reef selection - needs to be tested --- src/main/java/frc/robot/RobotContainer.java | 57 ++++++++++----------- 1 file changed, 28 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4dffcd0..269220d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,7 +37,7 @@ public class RobotContainer { drivetrain::followTrajectory, true, drivetrain); - private int lastButtonPressed; + private int lastButtonPressed = 7; // slows down robot during pickup private Command pickup = coral @@ -79,19 +79,33 @@ public RobotContainer() { // all controller bindings private void configureBindings() { - /*CommandScheduler.getInstance() - .schedule( - Commands.run( - () -> { - for (int i = 0; i < 12; i++) { - if (totalController.getHID().getRawButtonPressed(i + 1)) { - lastButtonPressed = i; - } - } - }) - .ignoringDisable(true));*/ + controller + .leftTrigger() + .onTrue( + Commands.runOnce( + () -> { + if (lastButtonPressed == 11) { + lastButtonPressed = 0; + } else { + lastButtonPressed = lastButtonPressed + 1; + } + }) + .ignoringDisable(true)); + + controller + .rightTrigger() + .onTrue( + Commands.runOnce( + () -> { + if (lastButtonPressed == 0) { + lastButtonPressed = 11; + } else { + lastButtonPressed = lastButtonPressed - 1; + } + }) + .ignoringDisable(true)); + // coral pickup - CommandScheduler.getInstance().schedule(Commands.run(() -> {}).ignoringDisable(true)); new Trigger(() -> controller.getRightY() > 0.9) .onTrue( Commands.runOnce( @@ -136,21 +150,6 @@ private void configureBindings() { } })); - /* - // picking up algae - 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 controller .x() .whileTrue( @@ -229,7 +228,7 @@ private void configureBindings() { .pivotRelease() .alongWith(algae.deployForClimb().unless(() -> DriverStation.getMatchTime() > 25))); controller.povLeft().onTrue(Commands.runOnce(() -> autoDisabled = !autoDisabled)); - // controller.povRight().onTrue(Commands.runOnce(() -> autoDisabled = true)); + controller.povRight().onTrue(Commands.runOnce(() -> autoDisabled = true)); } // deadbands for driving From bb5f4a6879f727a9af769ff1306ab63cb1a44269 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 19 Sep 2025 09:55:11 -0400 Subject: [PATCH 4/9] edit --- src/main/java/frc/robot/RobotContainer.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 269220d..50bc05e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ 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; @@ -28,7 +27,6 @@ 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 AutoFactory autos = new AutoFactory( From 236b49c911f3b997fda61569abb2292476493be6 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 19 Sep 2025 11:54:30 -0400 Subject: [PATCH 5/9] commenting --- src/main/java/frc/robot/RobotContainer.java | 60 +++++++++++++-------- 1 file changed, 39 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 50bc05e..34d903c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,7 +35,10 @@ public class RobotContainer { drivetrain::followTrajectory, true, drivetrain); - private int lastButtonPressed = 7; + // 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 @@ -77,33 +80,36 @@ public RobotContainer() { // all controller bindings private void configureBindings() { + // 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( () -> { - if (lastButtonPressed == 11) { - lastButtonPressed = 0; + if (reefSection == 11) { + reefSection = 0; } else { - lastButtonPressed = lastButtonPressed + 1; + reefSection += 1; } }) .ignoringDisable(true)); - + // if right trigger is pressed, shifts selected reef section to score on right one, works while + // robot is disabled for autonomous controller .rightTrigger() .onTrue( Commands.runOnce( () -> { - if (lastButtonPressed == 0) { - lastButtonPressed = 11; + if (reefSection == 0) { + reefSection = 11; } else { - lastButtonPressed = lastButtonPressed - 1; + reefSection -= 1; } }) .ignoringDisable(true)); - // coral pickup + // when right toggle goes up, cancels ground coral pickup or initiates ground coral pickup new Trigger(() -> controller.getRightY() > 0.9) .onTrue( Commands.runOnce( @@ -119,7 +125,7 @@ private void configureBindings() { fieldRelative = false; } })); - // cancel coral pickup + // when right toggle goes down, cancels station coral pickup or initiates station coral pickup new Trigger(() -> controller.getRightY() < -0.9) .onTrue( Commands.runOnce( @@ -134,7 +140,7 @@ private void configureBindings() { speedMultiplier = 0.4; } })); - // enabling the safe state + // when upper plus button is pressed, cancels safe state or initiates safe state controller .povUp() .onTrue( @@ -147,7 +153,9 @@ private void configureBindings() { speedMultiplier = 1.0; } })); - + // 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 .x() .whileTrue( @@ -158,9 +166,12 @@ private void configureBindings() { () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) .withDeadline(coral.l4Score(controller.leftStick(), false)); } else { - return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 3); + return coral.goToReef(drivetrain, () -> reefSection, () -> 3); } })); + // 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( @@ -171,9 +182,12 @@ private void configureBindings() { () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) .withDeadline(coral.l3Score(controller.leftStick())); } else { - return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 2); + return coral.goToReef(drivetrain, () -> reefSection, () -> 2); } })); + // 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( @@ -184,9 +198,12 @@ private void configureBindings() { () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) .withDeadline(coral.l2Score(controller.leftStick())); } else { - return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 1); + return coral.goToReef(drivetrain, () -> reefSection, () -> 1); } })); + // 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( @@ -197,11 +214,11 @@ private void configureBindings() { () -> speedMultiplier = 0.25, () -> speedMultiplier = 1.0) .withDeadline(coral.l1Score(controller.leftStick())); } else { - return coral.goToReef(drivetrain, () -> lastButtonPressed, () -> 0); + return coral.goToReef(drivetrain, () -> reefSection, () -> 0); } })); - // clearing algae off reef + // 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())); @@ -217,16 +234,17 @@ private void configureBindings() { true), drivetrain)); - // deploy climber, releases algae too + // deploy climber controller.leftBumper().whileTrue(climb.pivotClimb()); + // release climber and deploy algae for climb unless not nearing end of match controller .rightBumper() .whileTrue( climb .pivotRelease() .alongWith(algae.deployForClimb().unless(() -> DriverStation.getMatchTime() > 25))); + // when left plus pressed, disable or enable auto, defaults at disabled controller.povLeft().onTrue(Commands.runOnce(() -> autoDisabled = !autoDisabled)); - controller.povRight().onTrue(Commands.runOnce(() -> autoDisabled = true)); } // deadbands for driving @@ -236,12 +254,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)); } } From 2386a84c0bd62305cc3f29d42a98f1ff03fc7bf2 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 19 Sep 2025 16:31:34 -0400 Subject: [PATCH 6/9] gyro zero --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34d903c..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; @@ -28,6 +29,7 @@ public class RobotContainer { private Algae algae = new Algae(); private CommandXboxController controller = new CommandXboxController(0); private Drivetrain drivetrain = new Drivetrain(); + private final AHRS gyro = new AHRS(AHRS.NavXComType.kMXP_SPI); private AutoFactory autos = new AutoFactory( drivetrain::getPose, @@ -245,6 +247,7 @@ private void configureBindings() { .alongWith(algae.deployForClimb().unless(() -> DriverStation.getMatchTime() > 25))); // 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 From 03a00004e469bd30e61d5e154b47a4e002ff0a14 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Sat, 20 Sep 2025 14:52:22 -0400 Subject: [PATCH 7/9] elevator changes --- src/main/java/frc/robot/subsystems/Elevator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 38c8485..88bb7bd 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -41,7 +41,7 @@ public Elevator() { elevator.configure( new SparkFlexConfig() .smartCurrentLimit(ElevatorConfig.ELEVATOR_CURRENT_LIMIT) - .idleMode(ElevatorConfig.ELEVATOR_IDLE_MODE) + .idleMode(ElevatorConfig.ELEVATOR_IDLE_MODE).inverted(false) .apply( new EncoderConfig() .positionConversionFactor(ElevatorConfig.ENCODER_POSITION_CONVERSION_FACTOR) From 95fc8c29ff7298015aa6c83ae2f1120b41bb050e Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Sat, 20 Sep 2025 14:52:28 -0400 Subject: [PATCH 8/9] elevator changes --- src/main/java/frc/robot/config/CANMappings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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; From 58f3c94d73e343e5e95fbb9f1469fac16a2216c5 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Sun, 21 Sep 2025 11:08:13 -0400 Subject: [PATCH 9/9] tuning changes --- src/main/java/frc/robot/subsystems/Coral.java | 13 +++++++------ .../java/frc/robot/subsystems/CoralGrabber.java | 2 +- src/main/java/frc/robot/subsystems/Elevator.java | 3 ++- 3 files changed, 10 insertions(+), 8 deletions(-) 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 88bb7bd..af710a4 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -41,7 +41,8 @@ public Elevator() { elevator.configure( new SparkFlexConfig() .smartCurrentLimit(ElevatorConfig.ELEVATOR_CURRENT_LIMIT) - .idleMode(ElevatorConfig.ELEVATOR_IDLE_MODE).inverted(false) + .idleMode(ElevatorConfig.ELEVATOR_IDLE_MODE) + .inverted(false) .apply( new EncoderConfig() .positionConversionFactor(ElevatorConfig.ENCODER_POSITION_CONVERSION_FACTOR)