From 907d0e2ca8c83717258f47d892aee60e9d8d1d43 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Thu, 5 Mar 2026 19:02:06 -0500 Subject: [PATCH 1/3] added Drive command --- src/main/java/frc/robot/RobotContainer.java | 14 +++++ .../frc/robot/commands/drive/DriveSwerve.java | 55 +++++++++++++++++++ .../robot/constants/enums/DriveDirection.java | 9 +++ 3 files changed, 78 insertions(+) create mode 100644 src/main/java/frc/robot/commands/drive/DriveSwerve.java create mode 100644 src/main/java/frc/robot/constants/enums/DriveDirection.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0edebafe..13177143 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc.robot.commands.hopper.DefaultSpinHopper; import frc.robot.commands.hopper.SpinHopper; import frc.robot.commands.drive.DriveDirectionTime; +import frc.robot.commands.drive.DriveSwerve; import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.drive.FakeVision; import frc.robot.commands.intake.SpinIntake; @@ -47,6 +48,7 @@ import frc.robot.commands.shooter.SpinShooter; import frc.robot.constants.Constants; import frc.robot.constants.enums.DeploymentState; +import frc.robot.constants.enums.DriveDirection; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.AnglerSubsystem; @@ -334,6 +336,18 @@ private void configureBindings() { } public void putShuffleboardCommands() { + SmartDashboard.putData( + "Drive/Forward", + new DriveSwerve(drivebase, DriveDirection.FORWARD, 1)); + SmartDashboard.putData( + "Drive/Backward", + new DriveSwerve(drivebase, DriveDirection.BACKWARD, 1)); + SmartDashboard.putData( + "Drive/Left", + new DriveSwerve(drivebase, DriveDirection.LEFT, 1)); + SmartDashboard.putData( + "Drive/Right", + new DriveSwerve(drivebase, DriveDirection.RIGHT, 1)); if (Constants.DEBUG) { /* diff --git a/src/main/java/frc/robot/commands/drive/DriveSwerve.java b/src/main/java/frc/robot/commands/drive/DriveSwerve.java new file mode 100644 index 00000000..b83238e4 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/DriveSwerve.java @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.drive; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.enums.DriveDirection; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + + +public class DriveSwerve extends LoggableCommand { + + private final SwerveSubsystem drivebase; + private final double time; + private final DriveDirection dir; + private Timer timer; + public DriveSwerve(SwerveSubsystem drivebase, DriveDirection dir, double time) { + timer = new Timer(); + this.time = time; + this.dir = dir; + this.drivebase = drivebase; + addRequirements(drivebase); + } + + + @Override + public void initialize() { + timer.restart(); + } + + + @Override + public void execute() { + switch (dir){ + case BACKWARD -> drivebase.drive(new Translation2d(2,0),0, true); + case FORWARD -> drivebase.drive(new Translation2d(-2,0),0, true); + case LEFT -> drivebase.drive(new Translation2d(0,-2),0, true); + case RIGHT -> drivebase.drive(new Translation2d(0,2),0, true); + } + } + + @Override + public void end(boolean interrupted) { + drivebase.drive(new Translation2d(),0, true); + } + + + @Override + public boolean isFinished() { + return timer.hasElapsed(time); + } +} diff --git a/src/main/java/frc/robot/constants/enums/DriveDirection.java b/src/main/java/frc/robot/constants/enums/DriveDirection.java new file mode 100644 index 00000000..17926d98 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/DriveDirection.java @@ -0,0 +1,9 @@ +package frc.robot.constants.enums; + + +public enum DriveDirection { + FORWARD, //Drives the robot forward + BACKWARD, //Drives the robot backward + LEFT, //Drives the robot left + RIGHT //Drives the robot right +} From fcc3a75cfce634caa2ef35f907d140f3d74d6807 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Thu, 5 Mar 2026 20:38:19 -0500 Subject: [PATCH 2/3] commit constants --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- src/main/java/frc/robot/commands/drive/DriveSwerve.java | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9740529..a99e41ff 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -336,16 +336,16 @@ private void configureBindings() { public void putShuffleboardCommands() { SmartDashboard.putData( "Drive/Forward", - new DriveSwerve(drivebase, DriveDirection.FORWARD, 1)); + new DriveSwerve(drivebase, DriveDirection.FORWARD, 0.5)); SmartDashboard.putData( "Drive/Backward", - new DriveSwerve(drivebase, DriveDirection.BACKWARD, 1)); + new DriveSwerve(drivebase, DriveDirection.BACKWARD, 0.5)); SmartDashboard.putData( "Drive/Left", - new DriveSwerve(drivebase, DriveDirection.LEFT, 1)); + new DriveSwerve(drivebase, DriveDirection.LEFT, 0.5)); SmartDashboard.putData( "Drive/Right", - new DriveSwerve(drivebase, DriveDirection.RIGHT, 1)); + new DriveSwerve(drivebase, DriveDirection.RIGHT, 0.5)); if (Constants.DEBUG) { /* diff --git a/src/main/java/frc/robot/commands/drive/DriveSwerve.java b/src/main/java/frc/robot/commands/drive/DriveSwerve.java index b83238e4..edf23224 100644 --- a/src/main/java/frc/robot/commands/drive/DriveSwerve.java +++ b/src/main/java/frc/robot/commands/drive/DriveSwerve.java @@ -35,10 +35,10 @@ public void initialize() { @Override public void execute() { switch (dir){ - case BACKWARD -> drivebase.drive(new Translation2d(2,0),0, true); - case FORWARD -> drivebase.drive(new Translation2d(-2,0),0, true); - case LEFT -> drivebase.drive(new Translation2d(0,-2),0, true); - case RIGHT -> drivebase.drive(new Translation2d(0,2),0, true); + case BACKWARD -> drivebase.drive(new Translation2d(-1,0),0, true); + case FORWARD -> drivebase.drive(new Translation2d(1,0),0, true); + case LEFT -> drivebase.drive(new Translation2d(0,-1),0, true); + case RIGHT -> drivebase.drive(new Translation2d(0,1),0, true); } } From fbc76fc49034457d88061a1f8908f1ddbf9968ab Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Mon, 9 Mar 2026 18:40:45 -0400 Subject: [PATCH 3/3] Added Drive command --- src/main/java/frc/robot/RobotContainer.java | 35 ++++++++++--------- .../frc/robot/commands/drive/DriveSwerve.java | 10 +++--- .../frc/robot/constants/GameConstants.java | 2 +- 3 files changed, 24 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a99e41ff..05b7c41d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -313,11 +313,11 @@ private void configureBindings() { } intakeSubsystem.setDefaultCommand(new SpinIntake(intakeSubsystem, intakeDeployer)); if (controllerSubsystem != null) { - anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); - shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); - turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); - hopperSubsystem.setDefaultCommand(new DefaultSpinHopper(hopperSubsystem, controllerSubsystem)); - feederSubsystem.setDefaultCommand(new DefaultSpinFeeder(feederSubsystem, controllerSubsystem)); + //anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); + //shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); + //turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); + //hopperSubsystem.setDefaultCommand(new DefaultSpinHopper(hopperSubsystem, controllerSubsystem)); + //feederSubsystem.setDefaultCommand(new DefaultSpinFeeder(feederSubsystem, controllerSubsystem)); } if (!Constants.TESTBED) { @@ -334,18 +334,7 @@ private void configureBindings() { } public void putShuffleboardCommands() { - SmartDashboard.putData( - "Drive/Forward", - new DriveSwerve(drivebase, DriveDirection.FORWARD, 0.5)); - SmartDashboard.putData( - "Drive/Backward", - new DriveSwerve(drivebase, DriveDirection.BACKWARD, 0.5)); - SmartDashboard.putData( - "Drive/Left", - new DriveSwerve(drivebase, DriveDirection.LEFT, 0.5)); - SmartDashboard.putData( - "Drive/Right", - new DriveSwerve(drivebase, DriveDirection.RIGHT, 0.5)); + if (Constants.DEBUG) { /* @@ -365,6 +354,18 @@ public void putShuffleboardCommands() { // TODO: These commands do not REQUIRE the subsystem therefore cannot be used in// production + SmartDashboard.putData( + "Drive/Forward", + new DriveSwerve(drivebase, DriveDirection.FORWARD, 0.5)); + SmartDashboard.putData( + "Drive/Backward", + new DriveSwerve(drivebase, DriveDirection.BACKWARD, 0.5)); + SmartDashboard.putData( + "Drive/Left", + new DriveSwerve(drivebase, DriveDirection.LEFT, 0.5)); + SmartDashboard.putData( + "Drive/Right", + new DriveSwerve(drivebase, DriveDirection.RIGHT, 0.5)); SmartDashboard.putData( "Intake/Spin Forward", new InstantCommand(() -> intakeSubsystem.setSpeed(1.0))); diff --git a/src/main/java/frc/robot/commands/drive/DriveSwerve.java b/src/main/java/frc/robot/commands/drive/DriveSwerve.java index edf23224..0fa263af 100644 --- a/src/main/java/frc/robot/commands/drive/DriveSwerve.java +++ b/src/main/java/frc/robot/commands/drive/DriveSwerve.java @@ -35,16 +35,16 @@ public void initialize() { @Override public void execute() { switch (dir){ - case BACKWARD -> drivebase.drive(new Translation2d(-1,0),0, true); - case FORWARD -> drivebase.drive(new Translation2d(1,0),0, true); - case LEFT -> drivebase.drive(new Translation2d(0,-1),0, true); - case RIGHT -> drivebase.drive(new Translation2d(0,1),0, true); + case BACKWARD -> drivebase.drive(new Translation2d(0.2,0),0, false); + case FORWARD -> drivebase.drive(new Translation2d(-0.2,0),0, false); + case LEFT -> drivebase.drive(new Translation2d(0,-0.2),0, false); + case RIGHT -> drivebase.drive(new Translation2d(0,0.2),0, false); } } @Override public void end(boolean interrupted) { - drivebase.drive(new Translation2d(),0, true); + drivebase.drive(new Translation2d(),0, false); } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 88049dd4..223478a9 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -34,7 +34,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick