Complete binds on controllers, function refactoring, debugging

Completed button bindings on the copilot joystick. This includes all the button bindings and the POV joystick angles to utilize.

Added print debugging statements for all subsystems.

Refactored motor-based subsystems to have a single function that does the power and reversal of the motors without the need for multiple functions to run different speeds or even reversed speeds.

This commit brings feature parity to commit OakvilleDynamics/2024-Robot at 4e4dccfaea74f86b19fe1bcaa0b81b95e322763d on main.

src/main/java/frc/robot/Constants.java

@@ -111,7 +111,7 @@ public static class Joysticks {
        * software.
        */
       public static class Port {
-        public static final int DRIVER_CONTROLLER = 0;
+        public static final int PILOT_CONTROLLER = 0;
         public static final int COPILOT_CONTROLLER = 1;
       }
 

src/main/java/frc/robot/Robot.java

@@ -115,6 +115,7 @@ public void robotInit() {
     }
 
     Logger.start();
+    System.out.println("[ROBOT] Logger started!");
 
     // Reset and start timers for error checking
     canErrorTimerInit.reset();
@@ -123,6 +124,8 @@ public void robotInit() {
     canErrorTimer.start();
     disabledTimer.reset();
     disabledTimer.start();
+    System.out.println("[ROBOT] Timers started!");
+
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();

src/main/java/frc/robot/RobotContainer.java

@@ -16,7 +16,9 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.OperatorConstants.Deadbands;
 import frc.robot.commands.swerve.AbsoluteDrive;
+import frc.robot.subsystems.ClimbSubsystem;
 import frc.robot.subsystems.ConveyorSubsystem;
+import frc.robot.subsystems.ConveyorSubsystem.FlywheelSpeed;
 import frc.robot.subsystems.DumpSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
@@ -31,7 +33,7 @@
 public class RobotContainer {
   // Controllers and joysticks are defined here
   final CommandXboxController driverXbox =
-      new CommandXboxController(Constants.OperatorConstants.Joysticks.Port.DRIVER_CONTROLLER);
+      new CommandXboxController(Constants.OperatorConstants.Joysticks.Port.PILOT_CONTROLLER);
   final CommandJoystick copilotJoystick =
       new CommandJoystick(Constants.OperatorConstants.Joysticks.Port.COPILOT_CONTROLLER);
 
@@ -41,9 +43,11 @@ public class RobotContainer {
   private final IntakeSubsystem intake = new IntakeSubsystem();
   private final ConveyorSubsystem conveyor = new ConveyorSubsystem();
   private final DumpSubsystem dump = new DumpSubsystem();
+  private final ClimbSubsystem climb = new ClimbSubsystem();
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    System.out.println("[ROBOT] RobotContainer initialized.");
     // Configure the trigger bindings
     configureBindings();
 
@@ -69,6 +73,35 @@ public RobotContainer() {
    * joysticks}.
    */
   private void configureBindings() {
+    System.out.println("[BINDS] Configuring bindings");
+    configurePilotController();
+    configureCopilotController();
+    System.out.println("[BINDS] Bindings configured");
+  }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    // An example command will be run in autonomous
+    return drivebase.getAutonomousCommand("New Auto");
+  }
+
+  /**
+   * Configure the button bindings for the pilot controller.
+   *
+   * <p>This controller is used to drive the robot and control the swerve drive.
+   *
+   * <p>The A button is used to zero the gyro. The X button is used to add a fake vision reading.
+   * The B button is used to drive to a specific pose. The Y button is used to lock the swerve
+   * drive.
+   *
+   * <p>This is using an Xbox controller.
+   */
+  public void configurePilotController() {
+    System.out.println("[BINDS] Configuring pilot controller");
     driverXbox.a().onTrue(Commands.runOnce(drivebase::zeroGyro));
     driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
     driverXbox
@@ -79,22 +112,142 @@ private void configureBindings() {
                     drivebase.driveToPose(
                         new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))));
     driverXbox.y().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
-
-    copilotJoystick.trigger().onTrue(Commands.runOnce(dump::extend).repeatedly());
-    copilotJoystick.trigger().onFalse(Commands.runOnce(dump::retract).repeatedly());
-    copilotJoystick.button(3).whileTrue(Commands.runOnce(intake::runIntake).repeatedly());
-    copilotJoystick.button(4).whileTrue(Commands.runOnce(intake::reverseIntake).repeatedly());
-    copilotJoystick.button(5).whileTrue(Commands.runOnce(conveyor::runConveyor).repeatedly());
-    copilotJoystick.button(6).whileTrue(Commands.runOnce(conveyor::reverseConveyor).repeatedly());
+    System.out.println("[BINDS] Pilot controller configured");
   }
 
   /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
+   * Configure the button bindings for the copilot controller.
    *
-   * @return the command to run in autonomous
+   * <p>This controller is used to control the intake, conveyor, dump, and climber.
+   *
+   * <p>The trigger is used to extend the dump. The thumb trigger is used to retract the dump.
+   *
+   * <p>Button 3 is used to run the intake and conveyor in reverse. Button 4 is used to run the
+   * intake and conveyor forward. Button 5 is used to run the intake sushi in reverse. Button 6 is
+   * used to run the intake sushi forward. Button 7 is used to run the conveyor and flywheel at low
+   * speed. Button 9 is used to run the conveyor forward. Button 10 is used to stop the conveyor.
+   * Button 11 is used to run the conveyor and flywheel at normal speed. Button 12 is used to run
+   * the conveyor and flywheel at high speed.
+   *
+   * <p>The POV is used to control the flywheel. POV at values 315, 0, and 45 are used to run the
+   * flywheels at full speed. POV at values 135, 180, and 225 are used to run the flywheels at slow
+   * speed.
+   *
+   * <p>Throttle is used to control the climber. The values are inverted because of the way the
+   * controller values are laid out. Throttle at 1 is used to run the climber up. Throttle at -1 is
+   * used to run the climber down.
    */
-  public Command getAutonomousCommand() {
-    // An example command will be run in autonomous
-    return drivebase.getAutonomousCommand("New Auto");
+  public void configureCopilotController() {
+    System.out.println("[BINDS] Configuring copilot controller");
+    copilotJoystick.trigger().onTrue(Commands.runOnce(dump::extend).repeatedly());
+    copilotJoystick.trigger().onFalse(Commands.runOnce(dump::retract).repeatedly());
+    copilotJoystick
+        .button(3)
+        .whileTrue(
+            Commands.runOnce(
+                    () -> {
+                      intake.runIntakeFront(false);
+                      conveyor.runConveyor(false);
+                    })
+                .repeatedly());
+    copilotJoystick
+        .button(4)
+        .whileTrue(
+            Commands.runOnce(
+                    () -> {
+                      intake.runIntakeFront(true);
+                      conveyor.runConveyor(true);
+                    })
+                .repeatedly());
+    copilotJoystick
+        .button(5)
+        .whileTrue(
+            Commands.runOnce(
+                    () -> {
+                      intake.runIntakeSushi(false);
+                      conveyor.runConveyor(false);
+                    })
+                .repeatedly());
+    copilotJoystick
+        .button(6)
+        .whileTrue(
+            Commands.runOnce(
+                    () -> {
+                      intake.runIntakeSushi(true);
+                      conveyor.runConveyor(true);
+                    })
+                .repeatedly());
+    copilotJoystick
+        .button(7)
+        .whileTrue(
+            Commands.runOnce(
+                    () -> {
+                      conveyor.runFlywheel(FlywheelSpeed.LOW, false);
+                    })
+                .repeatedly());
+    copilotJoystick
+        .button(9)
+        .whileTrue(
+            Commands.runOnce(
+                    () -> {
+                      conveyor.runConveyor(true);
+                    })
+                .repeatedly());
+    copilotJoystick
+        .button(10)
+        .whileTrue(
+            Commands.runOnce(
+                    () -> {
+                      conveyor.runConveyor(false);
+                    })
+                .repeatedly());
+    copilotJoystick
+        .button(11)
+        .whileTrue(
+            Commands.runOnce(
+                    () -> {
+                      conveyor.runFlywheel(FlywheelSpeed.LOW, false);
+                    })
+                .repeatedly());
+    copilotJoystick
+        .button(12)
+        .whileTrue(
+            Commands.runOnce(
+                    () -> {
+                      conveyor.runFlywheel(FlywheelSpeed.NORMAL, false);
+                    })
+                .repeatedly());
+
+    copilotJoystick
+        .pov(0)
+        .whileTrue(
+            Commands.runOnce(() -> conveyor.runFlywheel(FlywheelSpeed.HIGH, false)).repeatedly());
+    copilotJoystick
+        .pov(45)
+        .whileTrue(
+            Commands.runOnce(() -> conveyor.runFlywheel(FlywheelSpeed.HIGH, false)).repeatedly());
+    copilotJoystick
+        .pov(315)
+        .whileTrue(
+            Commands.runOnce(() -> conveyor.runFlywheel(FlywheelSpeed.HIGH, false)).repeatedly());
+    copilotJoystick
+        .pov(135)
+        .whileTrue(
+            Commands.runOnce(() -> conveyor.runFlywheel(FlywheelSpeed.LOW, false)).repeatedly());
+    copilotJoystick
+        .pov(180)
+        .whileTrue(
+            Commands.runOnce(() -> conveyor.runFlywheel(FlywheelSpeed.LOW, false)).repeatedly());
+    copilotJoystick
+        .pov(225)
+        .whileTrue(
+            Commands.runOnce(() -> conveyor.runFlywheel(FlywheelSpeed.LOW, false)).repeatedly());
+
+    if (copilotJoystick.getThrottle() > 0) {
+      Commands.runOnce(climb::extend).repeatedly();
+    } else if (copilotJoystick.getThrottle() < 0) {
+      Commands.runOnce(climb::retract).repeatedly();
+    }
+    System.out.println("[BINDS] Copilot controller configured");
   }
 }

src/main/java/frc/robot/subsystems/ClimbSubsystem.java

@@ -20,6 +20,8 @@ public class ClimbSubsystem extends SubsystemBase {
   private boolean isClimbing = false;
 
   public ClimbSubsystem() {
+    System.out.println("[CLIMB] ClimbSubsystem initialized.");
+    retract();
     SmartDashboard.putBoolean(getName(), isClimbing);
   }
 

src/main/java/frc/robot/subsystems/ConveyorSubsystem.java

@@ -22,6 +22,7 @@ public class ConveyorSubsystem extends SubsystemBase {
       new CANSparkMax(Flywheel.FLYWHEEL_MOTOR_RIGHT, MotorType.kBrushless);
 
   public ConveyorSubsystem() {
+    System.out.println("[CONVEYOR] ConveyorSubsystem initialized.");
     conveyorMotorLeft.setInverted(Conveyor.CONVEYOR_MOTOR_LEFT_INVERTED);
     conveyorMotorRight.setInverted(Conveyor.CONVEYOR_MOTOR_RIGHT_INVERTED);
     flywheelMotorLeft.setInverted(Flywheel.FLYWHEEL_MOTOR_LEFT_INVERTED);
@@ -38,10 +39,18 @@ public void periodic() {
     // This method will be called once per scheduler run
   }
 
-  /** Run the conveyor at a set speed in the {@link Conveyor} class. */
-  public void runConveyor() {
-    conveyorMotorLeft.set(Conveyor.CONVEYOR_MOTOR_SPEED);
-    conveyorMotorRight.set(Conveyor.CONVEYOR_MOTOR_SPEED);
+  /**
+   * Run the conveyor at a set speed in the {@link Conveyor} class.
+   *
+   * @param isReversed Whether or not to reverse the conveyor.
+   */
+  public void runConveyor(boolean isReversed) {
+    double conveyorMotorSpeed = Conveyor.CONVEYOR_MOTOR_SPEED;
+    if (isReversed) {
+      conveyorMotorSpeed *= -1;
+    }
+    conveyorMotorLeft.set(conveyorMotorSpeed);
+    conveyorMotorRight.set(conveyorMotorSpeed);
   }
 
   /** Stop the conveyor. */
@@ -50,14 +59,13 @@ public void stopConveyor() {
     conveyorMotorRight.set(0);
   }
 
-  /** Reverse the conveyor at a set speed in the {@link Conveyor} class. */
-  public void reverseConveyor() {
-    conveyorMotorLeft.set(-Conveyor.CONVEYOR_MOTOR_SPEED);
-    conveyorMotorRight.set(-Conveyor.CONVEYOR_MOTOR_SPEED);
-  }
-
-  /** Run the flywheel at a set speed in the {@link Flywheel} class. */
-  public void runFlywheel(FlywheelSpeed speed) {
+  /**
+   * Run the flywheel at a set speed in the {@link Flywheel} class.
+   *
+   * @param speed The speed to run the flywheel at.
+   * @param isReversed Whether or not to reverse the flywheel.
+   */
+  public void runFlywheel(FlywheelSpeed speed, boolean isReversed) {
     var flywheelMotorSpeed = 0.0;
     switch (speed) {
       case LOW:
@@ -70,6 +78,9 @@ public void runFlywheel(FlywheelSpeed speed) {
         flywheelMotorSpeed = Flywheel.FLYWHEEL_MOTOR_SPEED_HIGH;
         break;
     }
+    if (isReversed) {
+      flywheelMotorSpeed *= -1;
+    }
     flywheelMotorLeft.set(flywheelMotorSpeed);
     flywheelMotorRight.set(flywheelMotorSpeed);
   }
@@ -80,31 +91,13 @@ public void stopFlywheel() {
     flywheelMotorRight.set(0);
   }
 
-  /** Reverse the flywheel at a set speed in the {@link Flywheel} class. */
-  public void reverseFlywheel(FlywheelSpeed speed) {
-    var flywheelMotorSpeed = 0.0;
-    switch (speed) {
-      case LOW:
-        flywheelMotorSpeed = Flywheel.FLYWHEEL_MOTOR_SPEED_LOW;
-        break;
-      case NORMAL:
-        flywheelMotorSpeed = Flywheel.FLYWHEEL_MOTOR_SPEED_NORMAL;
-        break;
-      case HIGH:
-        flywheelMotorSpeed = Flywheel.FLYWHEEL_MOTOR_SPEED_HIGH;
-        break;
-    }
-    flywheelMotorLeft.set(-flywheelMotorSpeed);
-    flywheelMotorRight.set(-flywheelMotorSpeed);
-  }
-
   /**
    * Run the conveyor and flywheel at a set speed in the {@link Conveyor} and {@link Flywheel}
    * class.
    */
   public void runConveyorAndFlywheel() {
-    runConveyor();
-    runFlywheel(FlywheelSpeed.NORMAL);
+    runConveyor(false);
+    runFlywheel(FlywheelSpeed.NORMAL, false);
   }
 
   /** Stop the conveyor and flywheel. */
@@ -118,11 +111,11 @@ public void stopConveyorAndFlywheel() {
    * class.
    */
   public void reverseConveyorAndFlywheel() {
-    reverseConveyor();
-    reverseFlywheel(FlywheelSpeed.NORMAL);
+    runConveyor(false);
+    runFlywheel(FlywheelSpeed.NORMAL, true);
   }
 
-  enum FlywheelSpeed {
+  public enum FlywheelSpeed {
     LOW,
     NORMAL,
     HIGH

src/main/java/frc/robot/subsystems/DumpSubsystem.java

@@ -20,6 +20,8 @@ public class DumpSubsystem extends SubsystemBase {
   private boolean isDumped = false;
 
   public DumpSubsystem() {
+    System.out.println("[DUMP] DumpSubsystem initialized.");
+    retract();
     SmartDashboard.putBoolean(getName(), isDumped);
   }