Initial commit

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

@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* 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 java.util.logging.Level;
+
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.hardware.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;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
+import frc4388.utility.Status.ReportLevel;
+
+/**
+ * Add your docs here.
+ */
+public class DiffDrive extends Subsystem {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+
+  private RobotTime m_robotTime = RobotTime.getInstance();
+
+  private TalonFX m_leftFrontMotor;
+  private TalonFX m_rightFrontMotor;
+  private TalonFX m_leftBackMotor;
+  private TalonFX m_rightBackMotor;
+  private DifferentialDrive m_driveTrain;
+  private RobotGyro m_gyro;
+
+  /**
+   * Add your docs here.
+   */
+  public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
+      TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
+    
+        super();
+
+    m_leftFrontMotor = leftFrontMotor;
+    m_rightFrontMotor = rightFrontMotor;
+    m_leftBackMotor = leftBackMotor;
+    m_rightBackMotor = rightBackMotor;
+    m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
+    m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
+    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);
+  }
+
+
+  @Override
+  public String getSubsystemName() {
+      return "Diff Drive";
+  }
+
+  @Override
+  public void queryStatus() {
+      // TODO: Add Stuff
+  }
+
+  @Override
+  public Status diagnosticStatus() {
+      Log("Diagnostic info for this has not been inplemented!"); //TODO
+      return new Status();
+  }
+}

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

@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* 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 edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.LEDPatterns;
+
+/**
+ * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
+ * Driver
+ */
+public class LED extends SubsystemBase {
+
+  static AddressableLED m_led;
+  static AddressableLEDBuffer m_ledBuffer;
+  static LED m_self;
+  /**
+   * Add your docs here.
+   */
+
+  public LED(){
+    if(m_self != null)
+      return;
+    m_led = new AddressableLED(9);
+    m_ledBuffer = new AddressableLEDBuffer(10);
+    m_led.setLength(m_ledBuffer.getLength());
+    m_led.setData(m_ledBuffer);
+    m_led.start();
+    System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
+  }
+  public static LED getInstance() {
+    if(m_self == null)
+      m_self = new LED();
+    return m_self;
+  }
+  @Override
+  public void periodic(){
+    //gamermode();
+    //SmartDashboard.putNumber("LED", m_currentPattern.getValue());
+    return;
+  }
+  static int firstcolor = 0;
+  static void gamermode() {
+      for(int i = 0; i < m_ledBuffer.getLength(); i++) {
+          final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
+          setLEDHSV(i, hue, 255, 128);
+      }
+      firstcolor +=3;
+      firstcolor %= 180;
+  }
+  /**
+   * Add your docs here.
+   */
+  public static void updateLED (){
+    gamermode();
+   // m_LEDController.set(m_currentPattern.getValue());
+  }
+
+  /**
+   * Add your docs here.
+   */
+  public static void setLEDRGB(int lednum, int r, int g, int b){
+    m_ledBuffer.setRGB(lednum, r, g, b);
+    //m_currentPattern = pattern;
+   // m_LEDController.set(m_currentPattern.getValue());
+  }
+  public static void setLEDHSV(int lednum, int hue, int sat, int val){
+    m_ledBuffer.setRGB(lednum, hue, sat, val);
+    //m_currentPattern = pattern;
+   // m_LEDController.set(m_currentPattern.getValue());
+  }
+  /**
+   * Add your docs here.
+   * @return
+   */
+  public AddressableLEDBuffer getBuffer() {
+    return m_ledBuffer;
+  }
+}

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

@@ -0,0 +1,364 @@
+// 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 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.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 SwerveModule leftFront;
+  private SwerveModule rightFront;
+  private SwerveModule leftBack;
+  private SwerveModule rightBack;
+
+  private SwerveModule[] modules;
+
+  private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  
+  private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
+  
+  private RobotGyro gyro;
+  
+  private int gear_index;
+  private boolean stopped = false;
+
+  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+  public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
+  public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+  
+  public double rotTarget = 0.0;
+  public Rotation2d orientRotTarget = new Rotation2d();
+  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+
+  /** Creates a new SwerveDrive. */
+  public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
+    super();
+    this.leftFront = leftFront;
+    this.rightFront = rightFront;
+    this.leftBack = leftBack;
+    this.rightBack = rightBack;
+    
+    this.gyro = gyro;
+    reset_index();
+    this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
+  }
+
+  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) {
+
+    double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
+    SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
+
+    if (fieldRelative) {
+
+      double rot = 0;
+      
+      // ! drift correction
+      if (rightStick.getNorm() > 0.05) {
+        rotTarget = gyro.getAngle();
+        rot_correction = 0;
+        // rot = rightStick.getX();
+      //  SmartDashboard.putBoolean("drift correction", false);
+        stopped = false;
+      } else if(leftStick.getNorm() > 0.05) {
+        if (!stopped) {
+          stopModules();
+          stopped = true;
+        }
+
+     //   SmartDashboard.putBoolean("drift correction", true);
+        
+        // rot = ((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() * speedAdjust);
+      // 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.
+      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);
+    }
+    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+  }
+
+  public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+    if (fieldRelative) {
+
+      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
+      }
+    
+    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);
+    }
+  }
+
+  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);
+
+    if (Math.abs(angle - getGyroAngle()) < 5.0) {
+      return true;
+    }
+
+    return false;
+  }
+
+  public double getGyroAngle() {
+    return -gyro.getAngle();
+  }
+
+  public void add180() {
+    gyro.reset(gyro.getAngle()+180);
+    rotTarget = gyro.getAngle();
+
+  }
+
+  public void resetGyro() {
+    gyro.reset();
+    rotTarget = gyro.getAngle();
+  }
+
+  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;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run\
+    SmartDashboard.putNumber("Gyro", getGyroAngle());
+    SmartDashboard.putNumber("RotTartget", rotTarget);
+  }
+
+  private void reset_index() {
+    gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?)
+  }
+
+  public void shiftDown() {
+    if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
+    int i = gear_index - 1;
+    if (i == -1) i = 0;
+    setPercentOutput(SwerveDriveConstants.GEARS[i]);
+    gear_index = i;
+  }
+
+  public void shiftUp() {
+    if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
+    int i = gear_index + 1;
+    if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
+    setPercentOutput(SwerveDriveConstants.GEARS[i]);
+    gear_index = i;
+  }
+
+  public void setPercentOutput(double speed) {
+    speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed;
+    gear_index = -1;
+  }
+
+  public void setToSlow() {
+    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
+    for(int i=0;i<5;i++){
+      Log("SLOW");
+    }
+  }
+
+  public void setToFast() {
+    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
+    for(int i=0;i<5;i++){
+      Log("FAST");
+    }
+  }
+
+  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;
+    }
+  }
+
+  public void shiftUpRot() {
+    rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
+  }
+
+  public void shiftDownRot() {
+    rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
+  }
+
+  @Override
+  public String getSubsystemName() {
+    return "Swerve Drive Controller";
+  }
+
+  @Override
+  public void queryStatus() {
+
+    SmartDashboard.putNumber("[" + getSubsystemName() + "] Gyro angle", this.gyro.getAngle());
+    SmartDashboard.putNumber("[" + getSubsystemName() + "] Shift State", this.speedAdjust);
+
+    // this.leftFront.queryStatus();
+    // this.leftBack.queryStatus();
+    // this.rightFront.queryStatus();
+    // this.rightBack.queryStatus();
+  
+    //TODO: Add more status things
+  }
+
+  @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());
+    
+    return status;
+  }
+}

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

@@ -0,0 +1,280 @@
+// 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.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;
+    }
+
+    @Override
+    public void queryStatus() {
+        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;
+    // }
+}