Added drive command

src/main/java/frc4388/robot/RobotContainer.java

@@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.ArcadeDrive;
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
@@ -34,6 +35,17 @@ public class RobotContainer {
     private final ArcadeDrive m_robotArcadeDrive = new ArcadeDrive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor,
             m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive);
 
+    private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
+        m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
+        m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
+        m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor,
+        m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor,
+        m_robotMap.leftFrontEncoder,
+        m_robotMap.rightFrontEncoder,
+        m_robotMap.leftBackEncoder,
+        m_robotMap.rightBackEncoder
+    );
+
     private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
     /* Controllers */
@@ -47,10 +59,16 @@ public class RobotContainer {
         configureButtonBindings();
 
         /* Default Commands */
-        // drives the robot with a two-axis input from the driver controller
+        // drives the arcade drive with a two-axis input from the driver controller
         m_robotArcadeDrive.setDefaultCommand(
                 new RunCommand(() -> m_robotArcadeDrive.driveWithInput(getDriverController().getLeftYAxis(),
                         getDriverController().getRightXAxis()), m_robotArcadeDrive));
+
+        // drives the arcade drive with a two-axis input from the driver controller
+        m_robotSwerveDrive.setDefaultCommand(
+                new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getRightXAxis(),
+                        getDriverController().getRightYAxis(), getDriverController().getLeftXAxis()), m_robotSwerveDrive));
+
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
     }

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -15,9 +15,10 @@ import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 
-public class SwerveDrive
+public class SwerveDrive extends SubsystemBase
 {
     SwerveDriveKinematics m_kinematics;
     private WPI_TalonFX m_leftFrontSteerMotor;
@@ -61,28 +62,32 @@ public class SwerveDrive
         m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
     }
 
-    public void drive(double strafeX, double strafeY, double rotate)
+
+    public void driveWithInput(double strafeX, double strafeY, double rotate)
+    {
+        var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED /*in rad/s */);
+        driveFromSpeeds(speeds);
+    }
+
+    public void driveFromSpeeds(ChassisSpeeds speeds)
     {
         //https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html 
-        var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED /*in rad/s */);
         // Convert to module states
         SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
 
         // Front left module state
         SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], new Rotation2d(m_leftFrontEncoder.getPosition()));
-
         // Front right module state
         SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], new Rotation2d(m_rightFrontEncoder.getPosition()));
-
         // Back left module state
         SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], new Rotation2d(m_leftBackEncoder.getPosition()));
-
         // Back right module state
         SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], new Rotation2d(m_rightBackEncoder.getPosition()));
         
+        //Set the Wheel motor speeds
         m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
         m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
-        m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + lefBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
+        m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
         m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
     
     }