add more to swervedrive subsystem and remove dead code.

simgui-ds.json

@@ -88,5 +88,10 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000"
+    }
   ]
 }

src/main/java/frc4388/robot/Constants.java

@@ -62,7 +62,7 @@ public final class Constants {
         public static final double TURBO_SPEED = 1.0;
         
         public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED};
-
+        public static final int STARTING_GEAR = 0;
         // dimensions
         public static final double WIDTH = 18.5; // TODO: validate
         public static final double HEIGHT = 18.5; // TODO: validate
@@ -192,17 +192,13 @@ public final class Constants {
         }
     
         public static final class Configurations {
-            public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
-            public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
+            public static final double OPEN_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help.
+            public static final double CLOSED_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help.
             public static final double NEUTRAL_DEADBAND = 0.04;
 
             // POWER! (limiting)
             private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration()
-                .withCurrentLimits(
-                    new CurrentLimitsConfigs()
-                        .withStatorCurrentLimit(100) // TODO: tune???
-                        .withStatorCurrentLimitEnable(true)
-                ).withMotorOutput(
+                .withMotorOutput(
                     new MotorOutputConfigs()
                         .withNeutralMode(NeutralModeValue.Brake)
                         .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
@@ -229,7 +225,7 @@ public final class Constants {
                         new ClosedLoopRampsConfigs()
                             .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
             );
-            private static final double SLIP_CURRENT = 120; // TODO: Tune??? 
+            private static final double SLIP_CURRENT = 100; // TODO: Tune??? 
         }
 
         // No mans land

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

@@ -54,12 +54,7 @@ public class RobotContainer {
     /* Subsystems */
     // private final LED m_robotLED = new LED();
 
-    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
-                                                                  m_robotMap.rightFront,
-                                                                  m_robotMap.leftBack,
-                                                                  m_robotMap.rightBack,
-                                              
-                                                                  m_robotMap.gyro);
+    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox   = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -88,7 +83,7 @@ public class RobotContainer {
     public RobotContainer() {
         configureButtonBindings();        
         configureVirtualButtonBindings();
-        new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip());
+        new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
         DriverStation.silenceJoystickConnectionWarning(true);
         // CameraServer.startAutomaticCapture();
 
@@ -148,7 +143,7 @@ public class RobotContainer {
         // ? /* Driver Buttons */
 
         DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip()));
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
             
         // ! /* Speed */
         new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final

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

@@ -4,8 +4,6 @@
 
 package frc4388.robot.subsystems;
 
-import java.util.logging.Level;
-
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
@@ -14,9 +12,7 @@ import com.ctre.phoenix6.swerve.SwerveRequest;
 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.util.Units;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
@@ -24,25 +20,22 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.Constants.SwerveDriveConstants.Conversions;
-import frc4388.utility.RobotGyro;
-import frc4388.utility.RobotUnits;
+
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
-import frc4388.utility.Status.Report;
 import frc4388.utility.Status.ReportLevel;
 
 public class SwerveDrive extends Subsystem {
   private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
   
-  private int gear_index;
+  private int gear_index = SwerveDriveConstants.STARTING_GEAR;
   private boolean stopped = false;
 
-  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+  public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
   public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
-  public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+  public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to 25%
   
   public double rotTarget = 0.0;
   public Rotation2d orientRotTarget = new Rotation2d();
@@ -53,25 +46,23 @@ public class SwerveDrive extends Subsystem {
     super();
     
     this.swerveDriveTrain = swerveDriveTrain;
-
-    reset_index();
   }
 
-  public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
-    // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
-    // rightStick.getAngle()
-    double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
-    // System.out.println(ang);
-    // module.go(ang);
-    // Rotation2d rot = Rotation2d.fromRadians(ang);
-    Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
-    SwerveModuleState state = new SwerveModuleState(speed, rot);
-    module.setDesiredState(state);
-  }
+  // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
+  //   // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
+  //   // rightStick.getAngle()
+  //   double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
+  //   // System.out.println(ang);
+  //   // module.go(ang);
+  //   // Rotation2d rot = Rotation2d.fromRadians(ang);
+  //   Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
+  //   SwerveModuleState state = new SwerveModuleState(speed, rot);
+  //   module.setDesiredState(state);
+  // }
 
   public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
     if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
-      swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); // stop the swerve
+      stopModules(); // stop the swerve
     
     if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
       return; // don't bother doing swerve drive math and return early.
@@ -118,97 +109,36 @@ public class SwerveDrive extends Subsystem {
     //   // chassisSpeeds = chassisSpeeds.
     //   chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
     } else {      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+      swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
+        .withVelocityX(leftStick.getX()*speedAdjust)
+        .withVelocityY(leftStick.getY()*speedAdjust)
+        .withRotationalRate(rightStick.getY()*rotSpeedAdjust)
+      );
     }
-    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
   }
 
-  public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-    if (fieldRelative) {
+  public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical reason to have a robot relitive version of this, and no pre provided version 
+    if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
+      stopModules(); // stop the swerve
+  
+    if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) //if no imput
+      return; // don't bother doing swerve drive math and return early.
 
-      double rot = 0;
-      
-      // ! drift correction
-      if (rightStick.getNorm() > 0.05) {
-        rotTarget = gyro.getAngle();
-        rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
-      //  SmartDashboard.putBoolean("drift correction", false);
-        stopped = false;
-      } else if(leftStick.getNorm() > 0.05) {
-        if (!stopped) {
-          stopModules();
-          stopped = true;
-        }
-
-     //   SmartDashboard.putBoolean("drift correction", true);
-        // double rot_correction = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
-        
-
-      }
-
-      // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
-      Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
-      // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
-
-      // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
-    } else {      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
-    }
-    // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
-  }
-
-  public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-
-    // Translation2d rightStick = new Translation2d(-rightX, rightY);
-    double rightX = rightStick.getX();
-    double rightY = rightStick.getY();
-
-    double rot_correction = 0;
-
-    // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
-
-    if(fieldRelative) {
-      double rot = 0;
-      if(rightStick.getNorm() > 0.5) {
-        orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
-        
-        Rotation2d tmp =  orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
-        double min = tmp.getDegrees();
-        min = Math.max(Math.abs(min), 2);
-        if(tmp.getDegrees() < 0)
-          min*=-1;
-        tmp = new Rotation2d(min * Math.PI / 180);
-        rot = tmp.getRadians(); // x   x - y/x
-      }
+    leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
     
-    Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), ((-1 * rightStick.getX()) * SwerveDriveConstants.ROTATION_SPEED) + rot_correction, gyro.getRotation2d()).times(1);
-    } else {      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
-    }
-    // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));  
-    }
-
-  /**
-   * 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) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
-    for (int i = 0; i < desiredStates.length; i++) {
-      SwerveModule module = modules[i];
-      SwerveModuleState state = desiredStates[i];
-      module.setDesiredState(state);
-    }
+    swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+      .withVelocityX(leftStick.getX()*speedAdjust)
+      .withVelocityY(leftStick.getY()*speedAdjust)
+      .withTargetDirection(rightStick.getAngle())
+    ); 
   }
 
   public boolean rotateToTarget(double angle) {
-    double currentAngle = getGyroAngle();
-    double error = angle - currentAngle;
-
-    driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
+    swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+      .withVelocityX(0)
+      .withVelocityY(0)
+      .withTargetDirection(Rotation2d.fromDegrees(angle))
+    );
 
     if (Math.abs(angle - getGyroAngle()) < 5.0) {
       return true;
@@ -218,48 +148,15 @@ public class SwerveDrive extends Subsystem {
   }
 
   public double getGyroAngle() {
-    return -gyro.getAngle();
-  }
-
-  public void add180() {
-    gyro.reset(gyro.getAngle()+180);
-    rotTarget = gyro.getAngle();
-
+    return swerveDriveTrain.getRotation3d().getAngle();
   }
 
   public void resetGyro() {
-    gyro.reset();
-    rotTarget = gyro.getAngle();
+    swerveDriveTrain.tareEverything();
   }
 
-  public void resetGyroFlip() {
-    gyro.resetFlip();
-    rotTarget = gyro.getAngle();
-  }
-
-  public void resetGyroRightBlue() {
-    gyro.resetRightSideBlue();
-    rotTarget = gyro.getAngle();
-  }
-
-  public void resetGyroRightAmp() {
-    gyro.resetAmpSide();
-    rotTarget = gyro.getAngle();
-  }
-  
   public void stopModules() {
-    for (SwerveModule module : this.modules) {
-      module.stop();
-    }
-  }
-
-  public SwerveDriveKinematics getKinematics() {
-    return this.kinematics;
-  }
-
-  public boolean getSpeedState() {
-    
-    return false;
+    swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
   }
 
   @Override
@@ -270,7 +167,7 @@ public class SwerveDrive extends Subsystem {
   }
 
   private void reset_index() {
-    gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?)
+    gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the robot start in?)
   }
 
   public void shiftDown() {
@@ -295,34 +192,18 @@ public class SwerveDrive extends Subsystem {
   }
 
   public void setToSlow() {
-    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
-    for(int i=0;i<5;i++){
-      Log("SLOW");
-    }
+    setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
+    gear_index = 0;
   }
 
   public void setToFast() {
-    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
-    for(int i=0;i<5;i++){
-      Log("FAST");
-    }
+    setPercentOutput(SwerveDriveConstants.FAST_SPEED);
+    gear_index = 1;
   }
 
   public void setToTurbo() {
-    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED;
-        for(int i=0;i<5;i++){
-      Log("TURBO");
-    }
-  }
-
-  public void toggleGear(double angle) {
-    if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
-      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
-      SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
-    } else {
-      this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
-      SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
-    }
+    setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
+    gear_index = 2;
   }
 
   public void shiftUpRot() {
@@ -354,7 +235,7 @@ public class SwerveDrive extends Subsystem {
 
   @Override
   public void queryStatus() {
-    sbGyro.setDouble(this.gyro.getAngle());
+    sbGyro.setDouble(getGyroAngle());
     sbShiftState.setDouble(this.speedAdjust);
   
     //TODO: Add more status things
@@ -363,13 +244,8 @@ public class SwerveDrive extends Subsystem {
   @Override
   public Status diagnosticStatus() {
     Status status = new Status();
-    for (SwerveModule module : modules) {
-      for (Report moduleDignostic : module.diagnosticStatus().reports) {
-        status.addReport(moduleDignostic.reportLevel, "[" + module.getSubsystemName() + "] " + moduleDignostic.description);
-      }
-    }
 
-    status.diagnoseHardwareCTRE("Swerve Gyro", gyro.getPigeon());
+    status.addReport(ReportLevel.ERROR, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
     
     return status;
   }

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

@@ -1,314 +0,0 @@
-// 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 java.util.logging.Level;
-
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.Utils;
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
-import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.FeedbackConfigs;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.ControlRequest;
-import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.MagnetHealthValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import com.ctre.phoenix6.signals.SensorDirectionValue;
-import com.ctre.phoenix6.hardware.CANcoder;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.utility.Gains;
-import frc4388.utility.Status;
-import frc4388.utility.Subsystem;
-import frc4388.utility.Status.ReportLevel;
-
-public class SwerveModule extends Subsystem {
-    private String name = "Null";
-    private TalonFX driveMotor;
-    private TalonFX angleMotor;
-    private CANcoder encoder;
-    // private final StatusSignal<Double> cc_pos;
-    // private final StatusSignal<Double> cc_vel;
-    // private int selfid;
-    // private ConfigurableDouble offsetGetter;
-    private static int swerveId = 0;
-    public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
-  
-    
-    /** Creates a new SwerveModule. */
-    public SwerveModule(String name, TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
-        super();
-        this.name = name;
-        this.driveMotor = driveMotor;
-        this.angleMotor = angleMotor;
-        this.encoder = encoder;
-
-        var motorCfg = new TalonFXConfiguration()
-            .withOpenLoopRamps(
-                new OpenLoopRampsConfigs()
-                    .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
-            ).withClosedLoopRamps(
-                new ClosedLoopRampsConfigs()
-                    .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
-            ).withMotorOutput(
-                new MotorOutputConfigs()
-                    .withNeutralMode(NeutralModeValue.Brake)
-                    .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
-            ).withCurrentLimits(
-                new CurrentLimitsConfigs()
-                    .withStatorCurrentLimit(100)
-                    .withStatorCurrentLimitEnable(true)
-                    .withSupplyCurrentLimit(100)
-                    .withSupplyCurrentLimitEnable(true)
-            );
-
-        driveMotor.getConfigurator().apply(motorCfg);
-        
-        TalonFXConfiguration angleConfig = new TalonFXConfiguration()
-            .withOpenLoopRamps(
-                new OpenLoopRampsConfigs()
-                    .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
-            ).withClosedLoopRamps(
-                new ClosedLoopRampsConfigs()
-                    .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
-            ).withMotorOutput(
-                new MotorOutputConfigs()
-                    .withNeutralMode(NeutralModeValue.Brake)
-                    .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
-            );
-
-        angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-
-        angleConfig.Slot0.kP = swerveGains.kP;
-        angleConfig.Slot0.kI = swerveGains.kI;   
-        angleConfig.Slot0.kD = swerveGains.kD;
-
-        angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID();
-        angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
-        angleMotor.getConfigurator().apply(angleConfig);
-
-        CANcoderConfiguration canconfig = new CANcoderConfiguration();
-        canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
-        canconfig.MagnetSensor.MagnetOffset = offset;
-        encoder.getConfigurator().apply(canconfig);
-
-        rotateToAngle(0);
-    }
-
-    // public void go(double ang){
-    //     // double curang = this.encoder.getAbsolutePosition().getValue();
-    //     System.out.println(getAngle().getDegrees());
-    //     rotateToAngle(ang);
-    // }
-    
-    @Override
-    public void periodic() {
-        //encoder.configMagnetOffset(offsetGetter.get());
-        //SmartDashboard.putString("Error Code: " + selfid, getstuff());
-        // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
-        // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
-        // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
-        // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());
-    }
-    /**
-     * Get the drive motor of the SwerveModule
-     * @return the drive motor of the SwerveModule
-     */
-    public TalonFX getDriveMotor() {
-        return this.driveMotor;
-    }
-
-    /**
-     * Get the angle motor of the SwerveModule
-     * @return the angle motor of the SwerveModule
-     */
-    public TalonFX getAngleMotor() {
-        return this.angleMotor;
-    }
-
-    /**
-     * Get the CANcoder of the SwerveModule
-     * @return the CANcoder of the SwerveModule
-     */
-    public CANcoder getEncoder() {
-        return this.encoder;
-    }
-
-    /**
-     * Get the angle of a SwerveModule as a Rotation2d
-     * @return the angle of a SwerveModule as a Rotation2d
-     */
-    public Rotation2d getAngle() {
-        // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
-        // return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
-        return Rotation2d.fromRotations(encoder.getPosition().getValue().baseUnitMagnitude());
-    }
-    
-    public double getAngularVel() {
-        // return this.angleMotor.getSelectedSensorVelocity();
-        return angleMotor.getVelocity().getValueAsDouble();
-    }
-
-    public double getDrivePos() {
-        // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
-        return driveMotor.getPosition().getValueAsDouble();
-    }
-
-    public double getDriveVel() {
-        // return this.driveMotor.getSelectedSensorVelocity(0);
-        return driveMotor.getVelocity().getValueAsDouble();
-    }
-
-    public void stop() {
-        driveMotor.set(0);
-        angleMotor.set(0);
-    }
-
-    public void rotateToAngle(double angle) {
-        final PositionVoltage m_request = new PositionVoltage(angle);
-        angleMotor.setControl(m_request);
-    }
-
-    /**
-     * Get state of swerve module
-     * @return speed in m/s and angle in degrees
-     */
-    public SwerveModuleState getState() {
-        return new SwerveModuleState(
-            Units.inchesToMeters(driveMotor.getVelocity().getValue().baseUnitMagnitude() *
-            SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV *
-            SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV), 
-            getAngle()
-        );
-    }
-
-    // private SwerveModuleState optimizeState(SwerveModuleState desiredState) {
-    //     Rotation2d curRot = this.getAngle();
-        
-    // }
-
-    /**
-     * Returns the current position of the SwerveModule
-     * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
-    //  */
-    // public SwerveModulePosition getPosition() {
-    //     return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
-    // }
-
-    /**
-     * Set the speed and rotation of the SwerveModule from a SwerveModuleState object
-     * @param desiredState a SwerveModuleState representing the desired new state of the module
-    //  */
-    public void setDesiredState(SwerveModuleState state) {
-        Rotation2d currentRotation = this.getAngle();
-
-        state.optimize(currentRotation);//SwerveModuleState.optimize(desiredState, currentRotation);
-
-        // calculate the difference between our current rotational position and our new rotational position
-        Rotation2d rotationDelta = state.angle.minus(currentRotation);
-
-        double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND;
-
-        rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations());
-
-        driveMotor.set(Math.max(Math.min(speed, 1.), -1.));
-    }
-
-    public void reset() {
-        // encoder.setPosition(0);
-    }
-
-    @Override
-    public String getSubsystemName() {
-        return this.name;
-    }
-
-    ShuffleboardLayout subsystemLayout;
-    GenericEntry sbSpeed;
-    GenericEntry sbAngle;
-
-    private void createLayout(){
-
-        subsystemLayout = Shuffleboard.getTab("Subsystems")
-        .getLayout(getSubsystemName(), BuiltInLayouts.kList)
-        .withSize(2, 2);
-
-        sbSpeed =  subsystemLayout
-        .add("Drive motor speed", 0)
-        .withWidget(BuiltInWidgets.kNumberBar)
-        .getEntry();
-
-        sbAngle =  subsystemLayout
-        .add("Angle motor angle", 0)
-        .withWidget(BuiltInWidgets.kGyro)
-        .getEntry();
-    }
-
-
-    @Override
-    public void queryStatus() {
-        if(subsystemLayout == null)
-            createLayout();
-
-        // Shuffleboard.getTab("Subsystems").set
-
-        // sbSpeed.setDouble(this.driveMotor.get());
-        sbAngle.setDouble(this.angleMotor.getRotorPosition().getValueAsDouble());
-        // SmartDashboard.putNumber("[" + getSubsystemName() + "] Drive motor speed", this.driveMotor.get());
-        // SmartDashboard.putNumber("[" + getSubsystemName() + "] Angle motor angle", this.angleMotor.getRotorPosition().getValueAsDouble());
-        //TODO: Add more status things
-    }
-
-    public boolean motorsAlive() {
-        return this.driveMotor.isAlive() && this.angleMotor.isAlive();
-    }
-
-    @Override
-    public Status diagnosticStatus() {
-        Status status = new Status();
-        
-        status.diagnoseHardwareCTRE("Drive", this.driveMotor);
-        status.diagnoseHardwareCTRE("Angle", this.angleMotor);
-        status.diagnoseHardwareCTRE("Steer", this.encoder);
-        
-        return status;
-    }
-
-    // public double getCurrent() {
-    //     return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
-    // }
-
-    // public double getVoltage() {
-    //     return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
-    // }
-
-    // public String getstuff() {
-    //     encoder.getPosition();
-    //     return "" + encoder.getLastError().value;
-    // }
-}