From cffa2586f98af2077239c0f246f9b69a0f3fbcd2 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Tue, 12 Mar 2024 15:36:03 -0500 Subject: [PATCH] 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 | 2 +- src/main/java/frc/robot/Robot.java | 3 + src/main/java/frc/robot/RobotContainer.java | 179 ++++++++++++++++-- .../frc/robot/subsystems/ClimbSubsystem.java | 2 + .../robot/subsystems/ConveyorSubsystem.java | 63 +++--- .../frc/robot/subsystems/DumpSubsystem.java | 2 + .../frc/robot/subsystems/IntakeSubsystem.java | 57 +++++- 7 files changed, 251 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8fb8671..008f00b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 24ab7d4..8951cfa 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aa3da2a..a2ede02 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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. + * + *

This controller is used to drive the robot and control the swerve drive. + * + *

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. + * + *

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 + *

This controller is used to control the intake, conveyor, dump, and climber. + * + *

The trigger is used to extend the dump. The thumb trigger is used to retract the dump. + * + *

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. + * + *

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. + * + *

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"); } } diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index b6ee755..f45985a 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/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); } diff --git a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java index a12b0c5..2bc0aad 100644 --- a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/DumpSubsystem.java b/src/main/java/frc/robot/subsystems/DumpSubsystem.java index 7d58e4a..84f2872 100644 --- a/src/main/java/frc/robot/subsystems/DumpSubsystem.java +++ b/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); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 4dcc3ef..739e060 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -17,6 +17,7 @@ public class IntakeSubsystem extends SubsystemBase { new CANSparkMax(Intake.INTAKE_FRONT, MotorType.kBrushless); public IntakeSubsystem() { + System.out.println("[INTAKE] IntakeSubsystem initialized."); intakeSushi.setInverted(Intake.INTAKE_SUSHI_INVERTED); intakeFront.setInverted(Intake.INTAKE_FRONT_INVERTED); @@ -29,10 +30,20 @@ public void periodic() { // This method will be called once per scheduler run } - /** Run the intake at a set speed in the {@link Intake} class. */ - public void runIntake() { - intakeSushi.set(Intake.INTAKE_SUSHI_SPEED); - intakeFront.set(Intake.INTAKE_FRONT_SPEED); + /** + * Run the intake at a set speed in the {@link Intake} class. + * + * @param isReversed Whether or not to reverse the intake. + */ + public void runIntake(boolean isReversed) { + double intakeSushiSpeed = Intake.INTAKE_SUSHI_SPEED; + double intakeFrontSpeed = Intake.INTAKE_FRONT_SPEED; + if (isReversed) { + intakeSushiSpeed *= -1; + intakeFrontSpeed *= -1; + } + intakeSushi.set(intakeSushiSpeed); + intakeFront.set(intakeFrontSpeed); } /** Stop the intake. */ @@ -41,9 +52,39 @@ public void stopIntake() { intakeFront.set(0); } - /** Reverse the intake at a set speed in the {@link Intake} class. */ - public void reverseIntake() { - intakeSushi.set(-Intake.INTAKE_SUSHI_SPEED); - intakeFront.set(-Intake.INTAKE_FRONT_SPEED); + /** + * Run the intake sushi at a set speed in the {@link Intake} class. + * + * @param isReversed Whether or not to reverse the intake sushi. + */ + public void runIntakeSushi(boolean isReversed) { + double intakeSushiSpeed = Intake.INTAKE_SUSHI_SPEED; + if (isReversed) { + intakeSushiSpeed *= -1; + } + intakeSushi.set(intakeSushiSpeed); + } + + /** Stop the intake sushi. */ + public void stopIntakeSushi() { + intakeSushi.set(0); + } + + /** + * Run the intake front at a set speed in the {@link Intake} class. + * + * @param isReversed Whether or not to reverse the intake front. + */ + public void runIntakeFront(boolean isReversed) { + double intakeFrontSpeed = Intake.INTAKE_FRONT_SPEED; + if (isReversed) { + intakeFrontSpeed *= -1; + } + intakeFront.set(intakeFrontSpeed); + } + + /** Stop the intake front. */ + public void stopIntakeFront() { + intakeFront.set(0); } }