added drive command

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;
@@ -45,6 +46,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;
@@ -311,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) {
@@ -332,6 +334,7 @@ private void configureBindings() {
         }
 
         public void putShuffleboardCommands() {
+
                 if (Constants.DEBUG) {
 
                         /*
@@ -351,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)));

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(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, false);
+  }
+
+
+  @Override
+  public boolean isFinished() {
+    return timer.hasElapsed(time);
+  }
+}

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

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