added drive command

src/main/deploy/YAGSL/modules/backright.json

@@ -18,7 +18,7 @@
     "drive": false,
     "angle": true
   },
-  "absoluteEncoderOffset": -108.896,
+  "absoluteEncoderOffset": 107.22,
   "location": {
     "front": -12,
     "left": -12

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

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
@@ -75,5 +76,7 @@ public enum Mode {
     public static final int DRIVE_JOYSTICK_PORT = 0;
     public static final int STEER_JOYSTICK_PORT = 1;
 
+
+
 }
 

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

@@ -7,12 +7,16 @@
 import java.io.File;
 
 import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.drive.Drive;
 import frc.robot.commands.intake.SpinIntake;
 import frc.robot.commands.roller.SpinRoller;
 import frc.robot.commands.tilt.TiltDown;
@@ -99,6 +103,10 @@ private void configureBindings() {
    // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
     Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
     drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
+
+    //basic drive command
+    Command drive = new Drive(drivebase, 0.1,0.1,2, true, 1);
+    driveJoystick.button(1).whileTrue(drive);
   }
   public void putShuffleboardCommands() {
         if (Constants.DEBUG) {
@@ -132,3 +140,4 @@ public RobotVisualizer getRobotVisualizer() {
     return robotVisualizer;
   }
 }
+

src/main/java/frc/robot/commands/drive/Drive.java

@@ -0,0 +1,67 @@
+// 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.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+/**
+ * Drives the robot 
+ * @param speedX Speed in the x direction in meters per second
+ * @param speedY Speed in the Y direction in meters per second
+ * @param radians Rotational speed in radians per second
+ * @param fieldRelative True for field-relative, false for robot-relative.
+ * @param time Amount of time before the command ends
+ */
+public class Drive extends Command {
+
+  private final SwerveSubsystem drivebase;
+  private final double speedX;
+  private final double speedY;
+  private final double radians;
+  private final boolean fieldRelative;
+  private final double time;
+  private Timer timer;
+  public Drive(SwerveSubsystem drivebase, double speedX, double speedY,double radians, boolean fieldRelative, double time) {
+    timer = new Timer();
+    this.time = time;
+    this.drivebase = drivebase;
+    this.speedX = speedX;
+    this.speedY = speedY;
+    this.radians = radians;
+    this.fieldRelative = fieldRelative;
+    addRequirements(drivebase);
+  }
+
+
+  @Override
+  public void initialize() {
+    timer.restart();
+  }
+
+
+  @Override
+  public void execute() {
+    drivebase.drive(new Translation2d(speedX,speedY),radians, fieldRelative);
+
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+
+  }
+
+
+  @Override
+  public boolean isFinished() {
+   if (timer.hasElapsed(time)) {
+      return true;
+    } else{
+      return false;
+    }
+  }
+}