SwerveDrive class done

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

@@ -0,0 +1,78 @@
+// 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 frc4388.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.RobotGyro;
+
+public class SwerveDrive extends SubsystemBase {
+  
+  private SwerveModule leftFront;
+  private SwerveModule rightFront;
+  private SwerveModule leftBack;
+  private SwerveModule rightBack;
+
+  private SwerveModule[] modules;
+
+  private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  
+  private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); 
+  
+  private RobotGyro gyro;
+
+  /** Creates a new SwerveDrive. */
+  public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
+    this.leftFront = leftFront;
+    this.rightFront = rightFront;
+    this.leftBack = leftBack;
+    this.rightBack = rightBack;
+
+    this.modules = new SwerveModule[] {leftFront, rightFront, leftBack, rightBack};
+
+    this.gyro = gyro;
+  }
+
+  public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND;
+    double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND;
+
+    SwerveModuleState[] states = kinematics.toSwerveModuleStates(
+      fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) 
+                    : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)
+    );
+
+    SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    setModuleStates(states);
+  }
+
+  /**
+   * Set each module of the swerve drive to the corresponding desired state.
+   * 
+   * @param desiredStates Array of module states to set.
+   */
+  public void setModuleStates(SwerveModuleState[] desiredStates) {
+    for (int i = 0; i < desiredStates.length; i++) {
+      SwerveModule module = modules[i];
+      SwerveModuleState state = desiredStates[i];
+      module.setDesiredState(state);
+    }
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

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

@@ -26,7 +26,6 @@ public class SwerveModule extends SubsystemBase {
 
     public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
   
-  
     /** Creates a new SwerveModule. */
     public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
         this.driveMotor = driveMotor;
@@ -49,6 +48,30 @@ public class SwerveModule extends SubsystemBase {
         canCoder.configAllSettings(canCoderConfig);
     }
 
+    /**
+     * Get the drive motor of the SwerveModule
+     * @return the drive motor of the SwerveModule
+     */
+    public WPI_TalonFX getDriveMotor() {
+        return this.driveMotor;
+    }
+
+    /**
+     * Get the angle motor of the SwerveModule
+     * @return the angle motor of the SwerveModule
+     */
+    public WPI_TalonFX getAngleMotor() {
+        return this.angleMotor;
+    }
+
+    /**
+     * Get the CANcoder of the SwerveModule
+     * @return the CANcoder of the SwerveModule
+     */
+    public CANCoder getEncoder() {
+        return this.canCoder;
+    }
+
     /**
      * Get the angle of a SwerveModule as a Rotation2d
      * @return the angle of a SwerveModule as a Rotation2d