Test fix for swerve turning

Remove unused commands

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

@@ -116,11 +116,7 @@ public class RobotContainer {
   private void configureDefaultCommands() {
     // Swerve Drive with Input
     m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-      if (driveControlEnabled) {
-        m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true);
-      } else {
-        m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false);
-      }
+      if (driveControlEnabled) m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true);
     }, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand"));
 
     // Intake with Triggers

src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java

@@ -1,57 +0,0 @@
-package frc4388.robot.commands.ButtonBoxCommands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-public class RunMiddleSwitch extends CommandBase {
-  
-  private static boolean manual = false;
-  private boolean newManual = false;
-  private boolean changes = false;
-
-  /** Creates a new RunMiddleSwitch. */
-  public RunMiddleSwitch() {
-    // Use addRequirements() here to declare subsystem dependencies.
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-
-    changes = (newManual == manual);
-
-    if (manual) {
-      printManual();
-    } else {
-      printNormal();
-    }
-
-    newManual = manual;
-  }
-
-  public void printNormal(){
-    System.out.println("Normal");
-  }
-
-  public void printManual(){
-    System.out.println("Manual");
-  }
-
-  public static void setManual(boolean set)
-  {
-    manual = set;
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return changes;
-  }
-}

src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java

@@ -1,58 +0,0 @@
-package frc4388.robot.commands.ButtonBoxCommands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Turret;
-
-public class TurretManual extends CommandBase {
-
-  // subsystems
-  private Turret turret;
-
-  // booleans
-  private static boolean manual = false;
-  private boolean newManual = false;
-  private boolean changes = false;
-
-  /** Creates a new TurretManual. */
-  public TurretManual(Turret turret) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    this.turret = turret;
-
-    addRequirements(this.turret);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    changes = (newManual != manual);
-
-    if (manual) {
-      System.out.println("Manual Turret");  // TODO: turret manual controls
-    } else {
-      System.out.println("Auto Turret");    // TODO: turret auto controls;
-    }
-
-    newManual = manual;
-  }
-
-  public static void setManual(boolean set)
-  {
-    manual = set;
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    manual = !manual;
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return changes;
-  }
-}

src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java

@@ -1,48 +0,0 @@
-package frc4388.robot.commands.ClimberCommands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Claws;
-
-public class RunClaw extends CommandBase {
-
-  // parameters
-  public Claws m_claws;
-  public Claws.ClawType clawType;
-  public boolean open;
-
-  /**
-   * Creates a new RunClaw, which runs a claw.
-   * @param sClaws Claws subsystem.
-   * @param which Which claw to run.
-   * @param open Whether to open or close the claw.
-   */
-  public RunClaw(Claws sClaws, Claws.ClawType which, boolean open) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    m_claws = sClaws;
-    clawType = which;
-    this.open = open;
-
-    addRequirements(m_claws);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    // m_claws.runClaw(clawType, open);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    // return m_claws.checkSwitchAndCurrent(clawType);
-    return false; // TODO: real return
-  }
-}

src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java

@@ -1,67 +0,0 @@
-package frc4388.robot.commands.ClimberCommands;
-
-import org.opencv.core.Point;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Claws;
-import frc4388.robot.subsystems.Climber;
-
-public class RunClimberPath extends CommandBase {
-  Climber climber;
-  Claws claws;
-
-  Point[] path;
-  int nextIndex;
-
-  boolean endPath;
-
-  /** Creates a new RunClimberPath. */
-  public RunClimberPath(Climber _climber, Claws _claws, Point[] _path) {
-    path = _path;
-    endPath = false;
-
-    climber = _climber;
-    claws = _claws;
-    addRequirements(climber, claws);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    claws.setOpen(true);
-    nextIndex = 0;
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    // if(!claws.fullyOpen())
-      return;
-    
-    // Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles());
-
-    // Vector2D dir = new Vector2D(path[nextIndex]);
-    // dir.subtract(new Vector2D(climberPos));
-
-    // if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1)
-    //   nextIndex++;
-    // else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) {
-    //   endPath = true;
-    //   claws.setOpen(false);
-    // } else if(!endPath) {
-    //   dir = dir.unit();
-    //   climber.controlWithInput(dir.x, dir.y);
-    // }
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    // return endPath && claws.fullyClosed();
-    return false;
-  }
-}

src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java

@@ -1,63 +0,0 @@
-package frc4388.robot.commands.DriveCommands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.SwerveDrive;
-
-public class DriveWithInputForTime extends CommandBase {
-
-  // subsystems
-  SwerveDrive swerve;
-
-  double[] inputs;
-
-  // timing
-  long start;
-  long elapsed;
-  double duration;
-
-  /**
-   * DriveWithInput for a specified amount of time.
-   * @param inputs Inputs used in DriveWithInput (xspeed, yspeed, xrot, yrot).
-   * @param time Time to DriveWithInput for, in seconds.
-   */
-  public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) {
-    // Use addRequirements() here to declare subsystem dependencies.
-
-    if (inputs.length != 4) {
-      throw new IllegalArgumentException();
-    }
-
-    this.swerve = swerve;
-    this.inputs = inputs;
-    this.duration = duration * 1000; // ! convert seconds to milliseconds, duh
-
-    addRequirements(this.swerve);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    start = System.currentTimeMillis();
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    System.out.println("RUNNING");
-    elapsed = System.currentTimeMillis() - start;
-    this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], false);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    System.out.println("Duration: " + duration);
-    System.out.println("Elapsed: " + elapsed);
-
-    return ((double) elapsed >= duration);
-  }
-}

src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java

@@ -1,49 +0,0 @@
-package frc4388.robot.commands.DriveCommands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.robot.subsystems.VisionOdometry;
-
-public class RotateUntilTarget extends CommandBase {
-
-  // subsystems
-  SwerveDrive swerve;
-  VisionOdometry visionOdometry;
-
-  double rotateSpeed;
-
-  /** Creates a new RotateUntilTarget. */
-  public RotateUntilTarget(SwerveDrive swerve, VisionOdometry visionOdometry, double rotateSpeed) {
-    // Use addRequirements() here to declare subsystem dependencies.
-
-    this.swerve = swerve;
-    this.visionOdometry = visionOdometry;
-
-    this.rotateSpeed = rotateSpeed;
-
-    addRequirements(this.swerve, this.visionOdometry);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    this.visionOdometry.setLEDs(true);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    this.swerve.driveWithInput(0.0, 0.0, rotateSpeed, true);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return this.visionOdometry.getTargetPoints() == null;
-    // return this.visionOdometry.m_camera.getLatestResult().hasTargets();
-  }
-}

src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java

@@ -1,68 +0,0 @@
-package frc4388.robot.commands.StorageCommands;
-
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.BoomBoom;
-import frc4388.robot.subsystems.Storage;
-import frc4388.robot.subsystems.Turret;
-
-public class ManageStorage extends CommandBase {
-
-  // subsystems
-  private Storage storage;
-  private BoomBoom drum;
-  private Turret turret;
-
-  private Alliance alliance;
-  private boolean rightColor;
-
-  /** Creates a new ManageStorage. */
-  public ManageStorage(Storage storage, BoomBoom drum, Turret turret) {
-    // Use addRequirements() here to declare subsystem dependencies.
-
-    this.storage = storage;
-    this.drum = drum;
-    this.turret = turret;
-
-    rightColor = true;
-
-    addRequirements(this.storage, this.drum, this.turret);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-  }
-  
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    checkColor();
-    
-    if (rightColor) {
-      this.storage.manageStorage();
-    } else {
-
-      // * CommandScheduler.getInstance().schedule(new ExampleCommand());
-      // * new ExampleCommand().schedule();
-      // * new ExampleCommand().execute(); (accompanied by initialize and onFinished)
-      
-      new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command
-    }
-  }
-
-  private void checkColor() {
-    // this.alliance = this.storage.getColor();
-    // rightColor = this.alliance.equals(Robot.alliance);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java

@@ -1,68 +0,0 @@
-package frc4388.robot.commands.StorageCommands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.ShooterConstants;
-import frc4388.robot.subsystems.BoomBoom;
-import frc4388.robot.subsystems.Storage;
-import frc4388.robot.subsystems.Turret;
-
-public class SpitOutWrongColor extends CommandBase {
-
-  // subsystems
-  private Storage storage;
-  private BoomBoom drum;
-  private Turret turret;
-
-  // time (in milliseconds)
-  private long initialTime;
-  private long elapsedTime;
-  private long threshold;
-
-  private double initialTurret;
-  private int spitVelocity;
-
-  /** Creates a new SpitOutWrongColor. */
-  public SpitOutWrongColor(Storage storage, BoomBoom drum, Turret turret) {
-    // Use addRequirements() here to declare subsystem dependencies.
-
-    this.storage = storage;
-    this.drum = drum;
-    this.turret = turret;
-
-    addRequirements(this.storage, this.drum, this.turret);
-  }
-  
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    initialTime = System.currentTimeMillis();
-    elapsedTime = 0;
-    threshold = 2000;
-
-    initialTurret = this.turret.getEncoderPosition();
-    spitVelocity = 2000;
-  }
-  
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    elapsedTime = System.currentTimeMillis() - initialTime;
-
-    this.storage.runStorage(0.9);
-    this.turret.gotoMidpoint();
-    this.drum.runDrumShooterVelocityPID(spitVelocity);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    this.storage.runStorage(0.0);
-    this.turret.runShooterRotatePID(initialTurret * ShooterConstants.TURRET_DEGREES_PER_ROT);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return (elapsedTime >= threshold);
-  }
-}

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

@@ -43,16 +43,16 @@ public class SwerveDrive extends SubsystemBase {
     m_backRight = backRight;
     m_gyro = gyro;
 
-    modules = new SwerveModule[] {m_frontLeft, m_frontRight, m_backLeft, m_backRight};
-    
+    modules = new SwerveModule[] { m_frontLeft, m_frontRight, m_backLeft, m_backRight };
+
     // m_poseEstimator = new SwerveDrivePoseEstimator(
-    //     getRegGyro(),//m_gyro.getRotation2d(),
-    //     new Pose2d(),
-    //     m_kinematics,
-    //     VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),  // TODO: tune
-    //     VecBuilder.fill(Units.degreesToRadians(1)),            // TODO: tune
-    //     VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
-    
+    // getRegGyro(),//m_gyro.getRotation2d(),
+    // new Pose2d(),
+    // m_kinematics,
+    // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune
+    // VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune
+    // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
+
     m_odometry = new SwerveDriveOdometry(SwerveDriveConstants.DRIVE_KINEMATICS, m_gyro.getRotation2d());
 
     m_gyro.reset();
@@ -67,11 +67,10 @@ public class SwerveDrive extends SubsystemBase {
   /**
    * Method to drive the robot using joystick info.
    * @link https://github.com/ZachOrr/MK3-Swerve-Example
-   * @param speeds[0]     Speed of the robot in the x direction (forward).
-   * @param speeds[1]     Speed of the robot in the y direction (sideways).
-   * @param rot           Angular rate of the robot.
-   * @param fieldRelative Whether the provided x and y speeds are relative to the
-   *                      field.
+   * @param speeds[0] Speed of the robot in the x direction (forward).
+   * @param speeds[1] Speed of the robot in the y direction (sideways).
+   * @param rot Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the field.
    */
   public void driveWithInput(Translation2d speed, double rot, boolean fieldRelative) {
     ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0);
@@ -81,41 +80,19 @@ public class SwerveDrive extends SubsystemBase {
 
     double xSpeedMetersPerSecond = speed.getX();
     double ySpeedMetersPerSecond = speed.getY();
-    chassisSpeeds = fieldRelative
-        ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-            -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2)))
-        : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-            -rot * SwerveDriveConstants.ROTATION_SPEED * 2);
+    chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED * 2);
     SwerveModuleState[] states = SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds);
     setModuleStates(states);
   }
 
-  public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
-    Translation2d speed = new Translation2d(leftX, leftY);
-    Translation2d head = new Translation2d(rightX, rightY);
-    driveWithInput(speed, head, fieldRelative);
-  }
-
-  // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
   public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-    ignoreAngles = leftStick.getX() == 0 && leftStick.getY() == 0 && rightStick.getX() == 0 && rightStick.getY() == 0;
-    leftStick = leftStick.times(leftStick.getNorm() * speedAdjust);
-    if (Math.abs(rightStick.getX()) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightStick.getY()) > OIConstants.RIGHT_AXIS_DEADBAND)
-      rotTarget = -(Math.atan2(rightStick.getY(), rightStick.getX()) - Math.PI / 2);
-    double rot = ignoreAngles ? rotTarget - m_gyro.getRotation2d().getRadians() : 0;
-    chassisSpeeds = fieldRelative
-        ? ChassisSpeeds.fromFieldRelativeSpeeds(leftStick.getX(), leftStick.getY(), rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2)))
-        : new ChassisSpeeds(leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2);
-    SwerveModuleState[] states = SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds);
-    // if (ignoreAngles) {
-    //   SwerveModuleState[] lockedStates = new SwerveModuleState[states.length];
-    //   for (int i = 0; i < states.length; i ++) {
-    //     SwerveModuleState state = states[i];
-    //     lockedStates[i]= new SwerveModuleState(0, state.angle);
-    //   }
-    //   setModuleStates(lockedStates);
-    // }
-    setModuleStates(states);
+    if (fieldRelative) {
+      if (rightStick.getNorm() > OIConstants.RIGHT_AXIS_DEADBAND) rotTarget = -Math.atan2(rightStick.getY(), rightStick.getX());
+      double rotDifference = rotTarget - m_gyro.getRotation2d().getRadians();
+      leftStick = leftStick.times(leftStick.getNorm() * speedAdjust);
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(leftStick.getX(), leftStick.getY(), rotDifference * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2) + (Math.PI / 2)));
+    } else chassisSpeeds = new ChassisSpeeds(leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2);
+    setModuleStates(SwerveDriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds));
   }
 
   /**
@@ -124,8 +101,7 @@ public class SwerveDrive extends SubsystemBase {
    * @param desiredStates Array of module states to set.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates,
-        Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
     // int i = 2; {
     for (int i = 0; i < desiredStates.length; i++) {
       SwerveModule module = modules[i];
@@ -199,7 +175,7 @@ public class SwerveDrive extends SubsystemBase {
    * 
    * @return Rotation2d object holding current gyro in radians
    */
-  public Rotation2d getRegGyro() { 
+  public Rotation2d getRegGyro() {
     // * test chassis regression
     // double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
     // * new robot regression
@@ -218,20 +194,16 @@ public class SwerveDrive extends SubsystemBase {
    * Updates the field relative position of the robot.
    */
   public void updateOdometry() {
-    Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2)); //+ (Math.PI/2));
-    Rotation2d actual =  new Rotation2d(m_gyro.getRotation2d().getRadians());
+    Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI * 2)); // + (Math.PI/2));
+    Rotation2d actual = new Rotation2d(m_gyro.getRotation2d().getRadians());
 
     SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
     SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
 
-    m_odometry.update( actual,//m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
-                       modules[0].getState(),
-                       modules[1].getState(),
-                       modules[2].getState(),
-                       modules[3].getState());
+    m_odometry.update(actual, // m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
+        modules[0].getState(), modules[1].getState(), modules[2].getState(), modules[3].getState());
   }
-  
-  
+
   /**
    * Resets pigeon.
    */
@@ -263,11 +235,11 @@ public class SwerveDrive extends SubsystemBase {
     }
   }
 
-  public double getCurrent(){
+  public double getCurrent() {
     return m_frontLeft.getCurrent() + m_frontRight.getCurrent() + m_backRight.getCurrent() + m_backLeft.getCurrent();
   }
 
-  public double getVoltage(){
+  public double getVoltage() {
     return m_frontLeft.getVoltage() + m_frontRight.getVoltage() + m_backRight.getVoltage() + m_backLeft.getVoltage();
   }
-}
+}