Reorganization

lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java

@@ -57,8 +57,8 @@ public class SwerveBuilder {
         return this;
     }
 
-    public SwerveDriveSubsystem build() {
-        return new SwerveDriveSubsystem(pigeonId, canBusObj, maxLinearVelMetersSecond, maxAngularVelRadiansSecond,
+    public SwerveDriveSystem build() {
+        return new SwerveDriveSystem(pigeonId, canBusObj, maxLinearVelMetersSecond, maxAngularVelRadiansSecond,
                 modules.toArray(size -> new SwerveModule[size]));
     }
 

lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java

@@ -16,9 +16,10 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class SwerveDriveSubsystem extends SubsystemBase{
+public class SwerveDriveSystem implements Subsystem{
 
     private SwerveDriveKinematics driveKiniematics;
     private ChassisSpeeds chassisSpeeds;
@@ -35,7 +36,7 @@ public class SwerveDriveSubsystem extends SubsystemBase{
 
     private double mLastSimTime = 0;
 
-    public SwerveDriveSubsystem(int pigeonId, CANBus canBus, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){
+    public SwerveDriveSystem(int pigeonId, CANBus canBus, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){
 
         MAX_LINEAR_SPEED = maxLinearVelMetersSecond;
         MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond;
@@ -54,7 +55,6 @@ public class SwerveDriveSubsystem extends SubsystemBase{
 
     @Override
     public void periodic() {
-        super.periodic();
         SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds);
 
         for(int i=0; i<modules.length;i++){
@@ -71,8 +71,6 @@ public class SwerveDriveSubsystem extends SubsystemBase{
 
     @Override
     public void simulationPeriodic() {
-        super.simulationPeriodic();
-
         double deltaTimeSecond = (mLastSimTime - Utils.getCurrentTimeSeconds());
 
         if(mLastSimTime != 0){
@@ -107,6 +105,10 @@ public class SwerveDriveSubsystem extends SubsystemBase{
         return gyro.getRoll().getValue();
     }
 
+    public Pigeon2 getGyro(){
+        return gyro;
+    }
+
     public void setChassisSpeed(ChassisSpeeds chassisSpeeds){
         this.setChassisSpeed(chassisSpeeds, new Translation2d());
     }

lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java

@@ -39,8 +39,10 @@ public class SwerveModule {
 
     private TalonFX driveMotor;
     private VelocityVoltage driveRequest = new VelocityVoltage(0);
+
     private TalonFX steerMotor;
     private MotionMagicVoltage steerRequest = new MotionMagicVoltage(0);
+
     private CANcoder steerEncoder;
 
     private String moduleName;
@@ -138,6 +140,18 @@ public class SwerveModule {
         requestedModuleState = state;
     }
 
+    public TalonFX getSteerMotor(){
+        return steerMotor;
+    }
+
+    public TalonFX getDriveMotor(){
+        return driveMotor;
+    }
+
+    public CANcoder getSteerEncoder(){
+        return steerEncoder;
+    }
+
     public SwerveModuleState getRequestedModuleState(){
         return requestedModuleState;
     }