diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 39434f4c..05b7c41d 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; @@ -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))); 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..0fa263af --- /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(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); + } +} 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 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 +}