Controller configuration

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,17 +28,20 @@ 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,
           (pose) -> drivetrain.poseEstimator.resetPose(pose),
           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));
   }
 }

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;

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(

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
 }

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)