Merge pull request #24 from Team4388/swerve-drive

Swerve Drive --> Master

config/LeftBackCANCoder.json

@@ -0,0 +1,47 @@
+{
+  "Configs": [
+    {
+      "Name": "Integrated Sensor",
+      "Type": "IntegratedSensor",
+      "Description": "Configures how the sensor is processed.",
+      "Ordinal": 0,
+      "Values": {
+        "Absolute Sensor Range": 0,
+        "Magnet Offset (deg)": -240.0293,
+        "Sensor Direction": false,
+        "Sensor Initialization Strategy": 0
+      }
+    },
+    {
+      "Name": "Unit Coefficient And String",
+      "Type": "UnitCoeffGroup",
+      "Description": "Describes the units to report in API and Self-Test.",
+      "Ordinal": 0,
+      "Values": {
+        "Sensor Coefficient": 0.087890625,
+        "Sensor Time Base": 1,
+        "Unit String": "deg"
+      }
+    },
+    {
+      "Name": "Custom Params",
+      "Type": "CustomParams",
+      "Description": "These are arbitrary values not used by the firmware.",
+      "Ordinal": 0,
+      "Values": {
+        "Custom Param 0": 0,
+        "Custom Param 1": 0
+      }
+    },
+    {
+      "Name": "Advanced Sensor and Meas",
+      "Type": "AdvancedSensorMeas",
+      "Description": "Group of configs relating to advanced sensor features and filters",
+      "Ordinal": 0,
+      "Values": {
+        "Velocity Period": 100,
+        "Velocity Window": 64
+      }
+    }
+  ]
+}

config/LeftFrontCANCoder.json

@@ -0,0 +1,47 @@
+{
+  "Configs": [
+    {
+      "Name": "Integrated Sensor",
+      "Type": "IntegratedSensor",
+      "Description": "Configures how the sensor is processed.",
+      "Ordinal": 0,
+      "Values": {
+        "Absolute Sensor Range": 0,
+        "Magnet Offset (deg)": -181.230469,
+        "Sensor Direction": false,
+        "Sensor Initialization Strategy": 0
+      }
+    },
+    {
+      "Name": "Unit Coefficient And String",
+      "Type": "UnitCoeffGroup",
+      "Description": "Describes the units to report in API and Self-Test.",
+      "Ordinal": 0,
+      "Values": {
+        "Sensor Coefficient": 0.087890625,
+        "Sensor Time Base": 1,
+        "Unit String": "deg"
+      }
+    },
+    {
+      "Name": "Custom Params",
+      "Type": "CustomParams",
+      "Description": "These are arbitrary values not used by the firmware.",
+      "Ordinal": 0,
+      "Values": {
+        "Custom Param 0": 0,
+        "Custom Param 1": 0
+      }
+    },
+    {
+      "Name": "Advanced Sensor and Meas",
+      "Type": "AdvancedSensorMeas",
+      "Description": "Group of configs relating to advanced sensor features and filters",
+      "Ordinal": 0,
+      "Values": {
+        "Velocity Period": 100,
+        "Velocity Window": 64
+      }
+    }
+  ]
+}

config/RightBackCANCoder.json

@@ -0,0 +1,47 @@
+{
+  "Configs": [
+    {
+      "Name": "Integrated Sensor",
+      "Type": "IntegratedSensor",
+      "Description": "Configures how the sensor is processed.",
+      "Ordinal": 0,
+      "Values": {
+        "Absolute Sensor Range": 0,
+        "Magnet Offset (deg)": -40.86914,
+        "Sensor Direction": false,
+        "Sensor Initialization Strategy": 0
+      }
+    },
+    {
+      "Name": "Unit Coefficient And String",
+      "Type": "UnitCoeffGroup",
+      "Description": "Describes the units to report in API and Self-Test.",
+      "Ordinal": 0,
+      "Values": {
+        "Sensor Coefficient": 0.087890625,
+        "Sensor Time Base": 1,
+        "Unit String": "deg"
+      }
+    },
+    {
+      "Name": "Custom Params",
+      "Type": "CustomParams",
+      "Description": "These are arbitrary values not used by the firmware.",
+      "Ordinal": 0,
+      "Values": {
+        "Custom Param 0": 0,
+        "Custom Param 1": 0
+      }
+    },
+    {
+      "Name": "Advanced Sensor and Meas",
+      "Type": "AdvancedSensorMeas",
+      "Description": "Group of configs relating to advanced sensor features and filters",
+      "Ordinal": 0,
+      "Values": {
+        "Velocity Period": 100,
+        "Velocity Window": 64
+      }
+    }
+  ]
+}

config/RightFrontCANCoder.json

@@ -0,0 +1,47 @@
+{
+  "Configs": [
+    {
+      "Name": "Integrated Sensor",
+      "Type": "IntegratedSensor",
+      "Description": "Configures how the sensor is processed.",
+      "Ordinal": 0,
+      "Values": {
+        "Absolute Sensor Range": 0,
+        "Magnet Offset (deg)": -270.615234,
+        "Sensor Direction": false,
+        "Sensor Initialization Strategy": 0
+      }
+    },
+    {
+      "Name": "Unit Coefficient And String",
+      "Type": "UnitCoeffGroup",
+      "Description": "Describes the units to report in API and Self-Test.",
+      "Ordinal": 0,
+      "Values": {
+        "Sensor Coefficient": 0.087890625,
+        "Sensor Time Base": 1,
+        "Unit String": "deg"
+      }
+    },
+    {
+      "Name": "Custom Params",
+      "Type": "CustomParams",
+      "Description": "These are arbitrary values not used by the firmware.",
+      "Ordinal": 0,
+      "Values": {
+        "Custom Param 0": 0,
+        "Custom Param 1": 0
+      }
+    },
+    {
+      "Name": "Advanced Sensor and Meas",
+      "Type": "AdvancedSensorMeas",
+      "Description": "Group of configs relating to advanced sensor features and filters",
+      "Ordinal": 0,
+      "Values": {
+        "Velocity Period": 100,
+        "Velocity Window": 64
+      }
+    }
+  ]
+}

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

@@ -7,6 +7,10 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
 
 /**
@@ -18,24 +22,120 @@ import frc4388.utility.LEDPatterns;
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    public static final class DriveConstants {
+  public static final class SwerveDriveConstants {
-        public static final int DRIVE_PIGEON_ID = 6;
 
-        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+    public static final double ROTATION_SPEED = 2.0;
-    }
-    public static final class ArmConstants {
-        public static final int MIN_ARM_LEN = 0;
-        public static final int MAX_ARM_LEN = 1;
-        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
-    }
-    public static final class LEDConstants {
-        public static final int LED_SPARK_ID = 0;
 
-        public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
+    public static final class IDs {
+      public static final int LEFT_FRONT_WHEEL_ID = 2;
+      public static final int LEFT_FRONT_STEER_ID = 3;
+      public static final int LEFT_FRONT_ENCODER_ID = 10;
+
+      public static final int RIGHT_FRONT_WHEEL_ID = 4;
+      public static final int RIGHT_FRONT_STEER_ID = 5;
+      public static final int RIGHT_FRONT_ENCODER_ID = 11;
+
+      public static final int LEFT_BACK_WHEEL_ID = 6;
+      public static final int LEFT_BACK_STEER_ID = 7;
+      public static final int LEFT_BACK_ENCODER_ID = 12;
+      
+      public static final int RIGHT_BACK_WHEEL_ID = 8;
+      public static final int RIGHT_BACK_STEER_ID = 9;
+      public static final int RIGHT_BACK_ENCODER_ID = 13;
     }
 
-    public static final class OIConstants {
+    public static final class PIDConstants {
-        public static final int XBOX_DRIVER_ID = 0;
+      public static final int SWERVE_SLOT_IDX = 0;
-        public static final int XBOX_OPERATOR_ID = 1;
+      public static final int SWERVE_PID_LOOP_IDX = 1;
+      public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
     }
+
+    public static final class AutoConstants {
+      public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
+      public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
+      public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0, 
+        new TrapezoidProfile.Constraints(0.0, 0.0)
+      );
+
+      public static final double PATH_MAX_VEL = -1; // TODO: find the actual value
+      public static final double PATH_MAX_ACC = -1; // TODO: find the actual value
+    }
+
+    public static final class Conversions {
+      public static final int CANCODER_TICKS_PER_ROTATION = 4096;
+
+      public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value
+      public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value
+
+      public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value
+      public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value
+
+      public static final double TICKS_PER_MOTOR_REV = 2048;
+      public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value
+      public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
+
+      public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
+      public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
+      public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
+      public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
+
+      public static final double TICK_TIME_TO_SECONDS = 10;
+      public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
+    }
+
+    public static final class Configurations {
+      public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
+      public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
+      public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
+
+      // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2022 SwerveDrive values
+      // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2022 SwerveDrive values
+      // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2022 SwerveDrive values
+      // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2022 SwerveDrive values
+      
+      // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2023 translated values (don't work)
+      // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2023 translated values (don't work)
+      // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2023 translated values (don't work)
+      // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2023 translated values (don't work)
+      
+      // public static final double LEFT_FRONT_ENCODER_OFFSET = 0.0 + 90.0; // * 2023 experimentally-derived values (mostly work) 
+      // public static final double RIGHT_FRONT_ENCODER_OFFSET = 0.0; // * 2023 experimentally-derived values (mostly work) 
+      // public static final double LEFT_BACK_ENCODER_OFFSET = 0.0 + 24.0; // * 2023 experimentally-derived values (mostly work) 
+      // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0 + 45.0 + 180.0; // * 2023 experimentally-derived values (mostly work)  
+      
+    }
+
+    public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value
+
+    // dimensions
+    public static final double WIDTH = 18.5; // TODO: find the actual value
+    public static final double HEIGHT = 18.5; // TODO: find the actual value
+    public static final double HALF_WIDTH = WIDTH / 2.d;
+    public static final double HALF_HEIGHT = HEIGHT / 2.d;
+
+    // misc
+    public static final int TIMEOUT_MS = 30;
+    public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+  }
+
+  public static final class GyroConstants {
+    public static final int ID = 14; // TODO: find the actual ID
+  }
+  
+  public static final class ArmConstants {
+      public static final int MIN_ARM_LEN = 0;
+      public static final int MAX_ARM_LEN = 1;
+      public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+  }
+  
+  public static final class LEDConstants {
+    // public static final int LED_SPARK_ID = 0;
+
+    public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
+  }
+
+  public static final class OIConstants {
+    public static final int XBOX_DRIVER_ID = 0;
+    public static final int XBOX_OPERATOR_ID = 1;
+  }
 }

src/main/java/frc4388/robot/Robot.java

@@ -25,6 +25,7 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
+
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
@@ -75,13 +76,6 @@ public class Robot extends TimedRobot {
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
-    /*
-     * String autoSelected = SmartDashboard.getString("Auto Selector",
-     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
-     * = new MyAutoCommand(); break; case "Default Auto": default:
-     * autonomousCommand = new ExampleCommand(); break; }
-     */
-
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
@@ -113,7 +107,6 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    
   }
 
   /**

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

@@ -10,11 +10,11 @@ package frc4388.robot;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
-import frc4388.robot.subsystems.LED;
+import frc4388.robot.commands.AutoBalance;
-import frc4388.utility.LEDPatterns;
+import frc4388.robot.commands.DriveWithInput;
+import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.controller.IHandController;
 import frc4388.utility.controller.XboxController;
 
@@ -27,10 +27,12 @@ import frc4388.utility.controller.XboxController;
  */
 public class RobotContainer {
     /* RobotMap */
-    private final RobotMap m_robotMap = new RobotMap();
+    public final RobotMap m_robotMap = new RobotMap();
 
     /* Subsystems */
-    private final LED m_robotLED = new LED(m_robotMap.LEDController);
+    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
+    // private final LED m_robotLED = new LED(m_robotMap.LEDController);
+    
 
     /* Controllers */
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
@@ -43,11 +45,19 @@ public class RobotContainer {
         configureButtonBindings();
 
         /* Default Commands */
-        // drives the robot with a two-axis input from the driver controller
+
-        // continually sends updates to the Blinkin LED controller to keep the lights on
+        m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, 
-        m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+                                                                () -> getDriverController().getLeftXAxis(), 
+                                                                () -> getDriverController().getLeftYAxis(), 
+                                                                () -> getDriverController().getRightXAxis(),
+                                                                false));
+        
+        // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+
+    
     }
 
+
     /**
      * Use this method to define your button->command mappings. Buttons can be
      * created by instantiating a {@link GenericHID} or one of its subclasses
@@ -56,13 +66,19 @@ public class RobotContainer {
      */
     private void configureButtonBindings() {
         /* Driver Buttons */
-        // test command to spin the robot while pressing A on the driver controller
+        
+        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotMap.gyro.reset()));
+            // .onFalse()
 
         /* Operator Buttons */
-        // activates "Lit Mode"
+
-        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+        new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-                .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
+            .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
-                .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+
+        // interrupt button
+        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand());
     }
 
     /**
@@ -71,7 +87,7 @@ public class RobotContainer {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        // no auto
+
         return new InstantCommand();
     }
 

src/main/java/frc4388/robot/RobotMap.java

@@ -7,14 +7,12 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.InvertType;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import frc4388.robot.Constants.SwerveDriveConstants;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import frc4388.robot.subsystems.SwerveModule;
-import frc4388.robot.Constants.LEDConstants;
 import frc4388.utility.RobotGyro;
 
 /**
@@ -22,19 +20,103 @@ import frc4388.utility.RobotGyro;
  * testing and modularization.
  */
 public class RobotMap {
+  private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
+  public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
-    public RobotMap() {
-        configureLEDMotorControllers();
-        configureDriveMotorControllers();
-    }
 
-    /* LED Subsystem */
+  public SwerveModule leftFront;
-    public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+  public SwerveModule rightFront;
+  public SwerveModule leftBack;
+  public SwerveModule rightBack;
 
-    void configureLEDMotorControllers() {
 
-    }
+  public RobotMap() {
+    configureLEDMotorControllers();
+    configureDriveMotors();
+  }
 
-    void configureDriveMotorControllers() {
+  /* LED Subsystem */
-    }
+  // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+
+  void configureLEDMotorControllers() {
+        
+  }
+
+    // swerve drive subsystem
+  public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
+  public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
+  public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
+
+  public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
+  public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
+  public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
+    
+  public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
+  public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
+  public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
+
+  public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
+  public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
+  public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
+
+
+  void configureDriveMotors() {
+    // config factory default
+    leftFrontWheel.configFactoryDefault();
+    leftFrontSteer.configFactoryDefault();
+    
+    rightFrontWheel.configFactoryDefault();
+    rightFrontSteer.configFactoryDefault();
+    
+    leftBackWheel.configFactoryDefault();
+    leftBackSteer.configFactoryDefault();
+    
+    rightBackWheel.configFactoryDefault();
+    rightBackSteer.configFactoryDefault();
+
+    // config open loop ramp
+    leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    
+    rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    
+    leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    
+    rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+    // config closed loop ramp
+    leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    
+    rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    
+    leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    
+    rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+    // config neutral deadband
+    leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+    leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+    rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+    rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+    leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+    leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+    rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+    rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+    // initialize SwerveModules
+    this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
+    this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
+    this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
+    this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
+  }
 }

src/main/java/frc4388/robot/commands/AutoBalance.java

@@ -0,0 +1,51 @@
+// 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.commands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.RobotGyro;
+
+public class AutoBalance extends PelvicInflammatoryDisease {
+	RobotGyro gyro;
+	SwerveDrive drive;
+
+	/** Creates a new AutoBalanceTF2. */
+	public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
+		super(1.0, 0, 0, 0);
+
+		this.gyro = gyro;
+		this.drive = drive;
+
+		addRequirements(drive);
+	}
+
+	@Override
+	public double getError() {
+		var pitch = gyro.getPitch();
+		SmartDashboard.putNumber("pitch", pitch);
+		return pitch;
+	}
+
+	@Override
+	public void runWithOutput(double output) {
+		double out2 = MathUtil.clamp(output / 40, -.5, .5);
+		if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
+		drive.drive(out2, 0, 0, false);
+	}
+
+	@Override
+	public void initialize() {
+		super.initialize();
+		// this.gyro.reset();
+	}
+
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}

src/main/java/frc4388/robot/commands/DriveWithInput.java

@@ -0,0 +1,75 @@
+// 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.commands;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class DriveWithInput extends CommandBase {
+
+  private final SwerveDrive swerve;
+
+  private final Supplier<Double> xSpeed;
+  private final Supplier<Double> ySpeed;
+  private final Supplier<Double> rot;
+  private final boolean fieldRelative;
+
+  private final SlewRateLimiter xLimiter, yLimiter, rotLimiter;
+
+  /** Creates a new DriveWithInput. */
+  public DriveWithInput(SwerveDrive swerve, Supplier<Double> xSpeed, Supplier<Double> ySpeed, Supplier<Double> rot, boolean fieldRelative) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.swerve = swerve;
+    this.xSpeed = xSpeed;
+    this.ySpeed = ySpeed;
+    this.rot = rot;
+    this.fieldRelative = fieldRelative;
+
+    this.xLimiter = new SlewRateLimiter(3);
+    this.yLimiter = new SlewRateLimiter(3);
+    this.rotLimiter = new SlewRateLimiter(3);
+
+    addRequirements(swerve);
+  }
+
+  // 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() {
+    double x = xSpeed.get();
+    double y = ySpeed.get();
+    double r = rot.get();
+
+    x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(5*Math.PI));
+  
+    swerve.drive(x, y, r, fieldRelative);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    System.out.println("----------------------------------------------------------------");
+    System.out.println("DriveWithInput ended");
+    System.out.println("Interrupted: " + interrupted);
+    System.out.println("----------------------------------------------------------------");
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java

@@ -0,0 +1,58 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.utility.Gains;
+
+public abstract class PelvicInflammatoryDisease extends CommandBase {
+	protected Gains gains;
+	private double output = 0;
+
+	/** Creates a new PelvicInflammatoryDisease. */
+	public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) {
+		gains = new Gains(kp, ki, kd, kf, 0);
+	}
+
+	public PelvicInflammatoryDisease(Gains gains) {
+		this.gains = gains;
+	}
+
+	/** produces the error from the setpoint */
+	public abstract double getError();
+	/** figure it out bitch */
+	public abstract void runWithOutput(double output);
+
+	// Called when the command is initially scheduled.
+	@Override 
+	public void initialize() {
+		output = 0;
+	}
+
+	private double prevError, cumError = 0;
+	
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override 
+	public void execute() {
+		double error = getError();
+		cumError += error * .02; // 20 ms
+		double delta = error - prevError;
+
+		output = error * gains.kP;
+		output += cumError * gains.kI;
+		output += delta * gains.kD;
+		output += gains.kF;
+
+		runWithOutput(output);
+	}
+
+	// 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/subsystems/DiffDrive.java

@@ -1,85 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.subsystems;
-
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import frc4388.robot.Constants.DriveConstants;
-import frc4388.utility.RobotGyro;
-import frc4388.utility.RobotTime;
-
-/**
- * Add your docs here.
- */
-public class DiffDrive extends SubsystemBase {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-  private RobotTime m_robotTime = RobotTime.getInstance();
-
-  private WPI_TalonFX m_leftFrontMotor;
-  private WPI_TalonFX m_rightFrontMotor;
-  private WPI_TalonFX m_leftBackMotor;
-  private WPI_TalonFX m_rightBackMotor;
-  private DifferentialDrive m_driveTrain;
-  private RobotGyro m_gyro;
-
-  /**
-   * Add your docs here.
-   */
-  public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
-      WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
-
-    m_leftFrontMotor = leftFrontMotor;
-    m_rightFrontMotor = rightFrontMotor;
-    m_leftBackMotor = leftBackMotor;
-    m_rightBackMotor = rightBackMotor;
-    m_driveTrain = driveTrain;
-    m_gyro = gyro;
-  }
-
-  @Override
-  public void periodic() {
-    m_gyro.updatePigeonDeltas();
-
-    if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
-      updateSmartDashboard();
-    }
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(move, steer);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void tankDriveWithInput(double leftMove, double rightMove) {
-    m_leftFrontMotor.set(leftMove);
-    m_rightFrontMotor.set(rightMove);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  private void updateSmartDashboard() {
-
-    // Examples of the functionality of RobotGyro
-    SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
-    SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
-    SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
-    // SmartDashboard.putData(m_gyro);
-  }
-}

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

@@ -0,0 +1,152 @@
+// 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.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.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+
+public class SwerveDrive extends SubsystemBase {
+  
+  public SwerveModule leftFront;
+  public SwerveModule rightFront;
+  public SwerveModule leftBack;
+  public 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 SwerveDriveOdometry odometry = new SwerveDriveOdometry(
+  //   kinematics, 
+  //   gyro.getRotation2d(),
+  //   new SwerveModulePosition[] {
+  //     leftFront.getPosition(),
+  //     rightFront.getPosition(),
+  //     leftBack.getPosition(),
+  //     rightBack.getPosition()
+  //   }
+  // );
+
+  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
+
+  /** 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[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
+  }
+
+  // WPILib swerve drive example
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    // SwerveModuleState[] states = kinematics.toSwerveModuleStates(
+    //   fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) 
+    //                 : new ChassisSpeeds(xSpeed, ySpeed, rot)
+    // );
+    SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
+    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) {
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(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);
+    }
+  }
+
+  /**
+   * Updates the odometry of the SwerveDrive.
+   */
+  // public void updateOdometry() {
+  //   odometry.update(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     }
+  //   );
+  // }
+
+  /**
+   * Gets the odometry of the SwerveDrive.
+   * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
+   */
+  // public Pose2d getOdometry() {
+  //   return odometry.getPoseMeters();
+  // }
+
+  /**
+   * Sets the odometry of the SwerveDrive.
+   * @param pose Pose to set the odometry to.
+   */
+  // public void setOdometry(Pose2d pose) {
+  //   odometry.resetPosition(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     },
+  //     pose
+  //   );
+  // }
+
+  /**
+   * Resets the odometry of the SwerveDrive to 0.
+   * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions.
+   */
+  // public void resetOdometry() {
+  //   odometry.resetPosition(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     },
+  //     new Pose2d()
+  //   );
+  // }
+
+  public SwerveDriveKinematics getKinematics() {
+    return this.kinematics;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    // updateOdometry();
+  }
+
+  /**
+   * Shifts gear from high to low, or vice versa.
+   * @param shift true to shift to high, false to shift to low
+   */
+  public void highSpeed(boolean shift) {
+    this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+  }
+
+}

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

@@ -0,0 +1,146 @@
+// 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 com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+
+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.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.Gains;
+
+public class SwerveModule extends SubsystemBase {
+    public WPI_TalonFX driveMotor;
+    public WPI_TalonFX angleMotor;
+    private CANCoder encoder;
+
+    public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
+  
+    /** Creates a new SwerveModule. */
+    public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
+        this.driveMotor = driveMotor;
+        this.angleMotor = angleMotor;
+        this.encoder = encoder;
+
+        TalonFXConfiguration angleConfig = new TalonFXConfiguration();
+        angleConfig.slot0.kP = swerveGains.kP;
+        angleConfig.slot0.kI = swerveGains.kI;
+        angleConfig.slot0.kD = swerveGains.kD;
+
+        // use the CANcoder as the remote sensor for the primary TalonFX PID
+        angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
+        angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
+        angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
+        angleMotor.configAllSettings(angleConfig);
+
+        encoder.configMagnetOffset(offset);
+    }
+
+    /**
+     * 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.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());
+    }
+
+    public void stop() {
+        driveMotor.set(0);
+        angleMotor.set(0);
+    }
+
+    public void rotateToAngle(double angle) {
+        angleMotor.set(TalonFXControlMode.Position, angle);
+    }
+
+    /**
+     * Get state of swerve module
+     * @return speed in m/s and angle in degrees
+     */
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(
+            Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, 
+            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 desiredState) {
+        Rotation2d currentRotation = this.getAngle();
+
+        SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
+
+        // calculate the difference between our current rotational position and our new rotational position
+        Rotation2d rotationDelta = state.angle.minus(currentRotation);
+
+        // calculate the new absolute position of the SwerveModule based on the difference in rotation
+        double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
+
+        // convert the CANCoder from its position reading to ticks
+        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
+        angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+
+        double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
+
+        driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
+        // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
+    }
+
+    public void reset(double position) {
+        encoder.setPositionToAbsolute();
+    }
+
+    public double getCurrent() {
+        return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
+    }
+
+    public double getVoltage() {
+        return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
+    }
+}

src/main/java/frc4388/utility/Gains.java

@@ -0,0 +1,77 @@
+// 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.utility;
+
+/** Add your docs here. */
+public class Gains {
+    public double kP;
+    public double kI;
+    public double kD;
+    public double kF;
+    public int kIZone;
+    public double kPeakOutput;
+    public double kMaxOutput;
+    public double kMinOutput;
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = kPeakOutput;
+        this.kMaxOutput = kPeakOutput;
+        this.kMinOutput = -kPeakOutput;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = 1.0;
+        this.kMaxOutput = 1.0;
+        this.kMinOutput = -1.0;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
+     * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kMaxOutput = kMaxOutput;
+        this.kMinOutput = kMinOutput;
+        this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
+    }    
+}

src/main/java/frc4388/utility/RobotGyro.java

@@ -7,8 +7,7 @@
 
 package frc4388.utility;
 
-import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
-import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
 import com.kauailabs.navx.frc.AHRS;
 
 // import edu.wpi.first.wpilibj.GyroBase;
@@ -21,18 +20,21 @@ import edu.wpi.first.math.MathUtil;
 public class RobotGyro implements Gyro {
     private RobotTime m_robotTime = RobotTime.getInstance();
 
-    private PigeonIMU m_pigeon = null;
+    private WPI_Pigeon2 m_pigeon = null;
     private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
     private double m_lastPigeonAngle;
     private double m_deltaPigeonAngle;
 
+    private double pitchZero = 0;
+    private double rollZero = 0;
+
     /**
      * Creates a Gyro based on a pigeon
      * @param gyro the gyroscope to use for Gyro
      */
-    public RobotGyro(PigeonIMU gyro) {
+    public RobotGyro(WPI_Pigeon2 gyro) {
         m_pigeon = gyro;
         m_isGyroAPigeon = true;
     }
@@ -46,6 +48,16 @@ public class RobotGyro implements Gyro {
         m_isGyroAPigeon = false;
     }
 
+    /**
+     * Resets yaw, pitch, and roll.
+     */
+    public void resetZeroValues() {
+        if (!m_isGyroAPigeon) return;
+
+        pitchZero = m_pigeon.getPitch();
+        rollZero =  m_pigeon.getRoll();
+    }
+
     /**
      * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
      * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
@@ -75,7 +87,7 @@ public class RobotGyro implements Gyro {
     @Override
     public void calibrate() {
         if (m_isGyroAPigeon) {
-            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+            m_pigeon.calibrate();
         } else {
             m_navX.calibrate();
         }
@@ -83,6 +95,8 @@ public class RobotGyro implements Gyro {
 
     @Override
     public void reset() {
+        resetZeroValues();
+
         if (m_isGyroAPigeon) {
             m_pigeon.setYaw(0);
         } else {
@@ -99,9 +113,10 @@ public class RobotGyro implements Gyro {
 	 *					Roll is within [-90,+90] degrees.
      */
     private double[] getPigeonAngles() {
-        double[] angles = new double[3];
+        double[] ypr = new double[3];
-        m_pigeon.getYawPitchRoll(angles);
+        m_pigeon.getYawPitchRoll(ypr);
-        return angles;
+
+        return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
     }
 
     @Override
@@ -113,6 +128,10 @@ public class RobotGyro implements Gyro {
         }
     }
 
+    public double getYaw() {
+       return this.getAngle();
+    }
+
     /**
      * Gets an absolute heading of the robot
      * @return heading from -180 to 180 degrees
@@ -166,7 +185,7 @@ public class RobotGyro implements Gyro {
         }
     }
 
-    public PigeonIMU getPigeon(){
+    public WPI_Pigeon2 getPigeon(){
         return m_pigeon;
     }