Churro

.idea/.gitignore

@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml

.idea/gradle.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="GradleSettings">
+    <option name="linkedExternalProjectsSettings">
+      <GradleProjectSettings>
+        <option name="distributionType" value="DEFAULT_WRAPPED" />
+        <option name="externalProjectPath" value="$PROJECT_DIR$" />
+        <option name="gradleJvm" value="temurin-11" />
+        <option name="modules">
+          <set>
+            <option value="$PROJECT_DIR$" />
+          </set>
+        </option>
+      </GradleProjectSettings>
+    </option>
+  </component>
+</project>

.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ExternalStorageConfigurationManager" enabled="true" />
+</project>

.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="" vcs="Git" />
+  </component>
+</project>

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

@@ -18,6 +18,52 @@ import frc4388.utility.LEDPatterns;
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+  public static final class SwerveDriveConstants {
+    public static final double ROTATION_SPEED = 0.1;
+    public static final double WHEEL_SPEED = 0.1;
+    public static final double WIDTH = 22;
+    public static final double HEIGHT = 22;
+    public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
+    public static final double MAX_SPEED_FEET_PER_SEC = 16;
+    public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
+    public static final int LEFT_FRONT_STEER_CAN_ID = 2;
+    public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
+    public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
+    public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
+    public static final int LEFT_BACK_STEER_CAN_ID = 6;
+    public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
+    public static final int RIGHT_BACK_STEER_CAN_ID = 8;
+    public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
+    public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
+    public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
+    public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
+    public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
+    // ofsets are in degrees
+    //ofsets are in degrees
+        // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;    
+        // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;   
+        // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;     
+        // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
+        public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
+        public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
+        public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
+        public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
+
+    // swerve PID constants
+    public static final int SWERVE_SLOT_IDX = 0;
+    public static final int SWERVE_PID_LOOP_IDX = 1;
+    public static final int SWERVE_TIMEOUT_MS = 30;
+    public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
+
+    // swerve configuration
+    public static final double NEUTRAL_DEADBAND = 0.04;
+    public static final double OPEN_LOOP_RAMP_RATE = 0.2;
+    public static final int REMOTE_0 = 0;
+
+    // misc
+    public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+  }
+
     public static final class DriveConstants {
         public static final int DRIVE_LEFT_FRONT_CAN_ID = 2;
         public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4;

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

@@ -0,0 +1,315 @@
+// 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.sensors.WPI_Pigeon2;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+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.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.OIConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.Gains;
+
+public class SwerveDrive extends SubsystemBase {
+
+  private SwerveModule m_leftFront;
+  private SwerveModule m_leftBack;
+  private SwerveModule m_rightFront;
+  private SwerveModule m_rightBack;
+
+  double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
+  double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
+
+  public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
+
+  Translation2d m_frontLeftLocation = 
+      new Translation2d(
+          Units.inchesToMeters(halfHeight), 
+          Units.inchesToMeters(halfWidth));
+  Translation2d m_frontRightLocation =
+       new Translation2d(
+          Units.inchesToMeters(halfHeight), 
+          Units.inchesToMeters(-halfWidth));
+  Translation2d m_backLeftLocation = 
+      new Translation2d(
+          Units.inchesToMeters(-halfHeight), 
+          Units.inchesToMeters(halfWidth));
+  Translation2d m_backRightLocation = 
+      new Translation2d(
+          Units.inchesToMeters(-halfHeight), 
+          Units.inchesToMeters(-halfWidth));
+
+  public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
+      m_backLeftLocation, m_backRightLocation);
+
+  public SwerveModule[] modules;
+  public WPI_Pigeon2 m_gyro;
+
+  public SwerveDriveOdometry m_odometry;
+  // public SwerveDriveOdometry m_odometry;
+
+  public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+  public boolean ignoreAngles;
+  public Rotation2d rotTarget = new Rotation2d();
+  private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+
+  private final Field2d m_field = new Field2d();
+
+  public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
+      WPI_Pigeon2 gyro) {
+
+    m_leftFront = leftFront;
+    m_leftBack = leftBack;
+    m_rightFront = rightFront;
+    m_rightBack = rightBack;
+    m_gyro = gyro;
+
+    modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
+    
+    // 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
+    
+    m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
+
+    m_gyro.reset();
+    SmartDashboard.putData("Field", m_field);
+  }
+
+  public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
+    Translation2d speed = new Translation2d(speedX, speedY);
+    driveWithInput(speed, rot, fieldRelative);
+  }
+
+  /**
+   * 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.
+   */
+  public void driveWithInput(Translation2d speed, double rot, boolean fieldRelative) {
+    ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0);
+
+    double mag = speed.getNorm();
+    speed = speed.times(mag * speedAdjust);
+
+    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);
+    SwerveModuleState[] states = m_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 = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0,1));
+    double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
+    if (ignoreAngles) {
+      rot = 0;
+    }
+    double xSpeedMetersPerSecond = leftStick.getX();
+    double ySpeedMetersPerSecond = leftStick.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(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2);
+    SwerveModuleState[] states = m_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);
+    // SmartDashboard.putNumber("rot", rot);
+    // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());
+  }
+
+  /**
+   * 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_SEC));
+    // int i = 2; {
+    for (int i = 0; i < desiredStates.length; i++) {
+      SwerveModule module = modules[i];
+      SwerveModuleState state = desiredStates[i];
+      module.setDesiredState(state, ignoreAngles);
+    }
+    // modules[0].setDesiredState(desiredStates[0], false);
+  }
+
+  public void setModuleRotationsToAngle(double angle) {
+    for (int i = 0; i < modules.length; i++) {
+      SwerveModule module = modules[i];
+      module.rotateToAngle(angle);
+    }
+  }
+
+  @Override
+  public void periodic() {
+    updateOdometry();
+    updateSmartDash();
+
+    // SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
+    // SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
+    // SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
+    // SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
+
+    m_field.setRobotPose(getOdometry());
+    super.periodic();
+  }
+
+  private void updateSmartDash() {
+    // odometry
+    SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
+    SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
+    SmartDashboard.putNumber("Odometry: Theta", getOdometry().getRotation().getDegrees());
+
+    // chassis speeds
+    // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
+    // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
+    // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
+    // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Gets the current chassis speeds in m/s and rad/s.
+   * @return Current chassis speeds (vx, vy, ω)
+   */
+  public ChassisSpeeds getChassisSpeeds() {
+    return chassisSpeeds;
+  }
+
+  /**
+   * Gets the current pose of the robot.
+   * 
+   * @return Robot's current pose.
+   */
+  public Pose2d getOdometry() {
+    // return m_odometry.getPoseMeters();
+    return m_odometry.getPoseMeters();
+    // return m_poseEstimator.getEstimatedPosition();
+  }
+
+  public Pose2d getAutoOdo() {
+    Pose2d workingPose = getOdometry();
+    return new Pose2d(-workingPose.getX(), workingPose.getY(), workingPose.getRotation());
+  }
+
+  /**
+   * Gets the current gyro using regression formula.
+   * 
+   * @return Rotation2d object holding current gyro in radians
+   */
+  public Rotation2d getRegGyro() { 
+    // * test chassis regression
+    // double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
+    // * new robot regression
+    double regCur = 0.2507023948 + m_gyro.getRotation2d().getDegrees() * 0.999034743;
+    return new Rotation2d(Math.toRadians(regCur));
+  }
+
+  /**
+   * Resets the odometry of the robot to the given pose.
+   */
+  public void resetOdometry(Pose2d pose) {
+    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
+  }
+
+  /**
+   * 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());
+
+    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());
+  }
+  
+  
+  /**
+   * Resets pigeon.
+   */
+  public void resetGyro() {
+    m_gyro.reset();
+    rotTarget = new Rotation2d(0);
+  }
+
+  /**
+   * Stop all four swerve modules.
+   */
+  public void stopModules() {
+    modules[0].stop();
+    modules[1].stop();
+    modules[2].stop();
+    modules[3].stop();
+  }
+
+  /**
+   * Switches speed modes.
+   * 
+   * @param shift True if fast mode, false if slow mode.
+   */
+  public void highSpeed(boolean shift) {
+    if (shift) {
+      speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+    } else {
+      speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+    }
+  }
+
+  public double getCurrent(){
+    return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent();
+  }
+
+  public double getVoltage(){
+    return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage();
+  }
+}

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

@@ -0,0 +1,181 @@
+// 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.TalonFXFeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+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.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.Gains;
+
+public class SwerveModule extends SubsystemBase {
+    public WPI_TalonFX angleMotor;
+    public WPI_TalonFX driveMotor;
+    private CANCoder canCoder;
+    public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
+
+    private static double kEncoderTicksPerRotation = 4096;
+    private SwerveModuleState state;
+    private double canCoderFeedbackCoefficient;
+
+    public long m_currentTime;
+    public long m_lastTime;
+    public double m_deltaTime;
+
+    public double m_currentPos;
+    public double m_lastPos;
+
+    public SwerveModuleState lastState = new SwerveModuleState();
+    public SwerveModuleState currentState;
+
+    /** Creates a new SwerveModule. */
+    public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
+        this.driveMotor = driveMotor;
+        this.angleMotor = angleMotor;
+        this.canCoder = canCoder;
+        canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
+
+        TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
+
+        angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP;
+        angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI;
+        angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD;
+
+        // Use the CANCoder as the remote sensor for the primary TalonFX PID
+        angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
+        angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
+        angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
+        angleMotor.configAllSettings(angleTalonFXConfiguration);
+        // angleMotor.setInverted(true);
+        // TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
+        // driveTalonFXConfiguration.slot0.kP = 0.05;
+        // driveTalonFXConfiguration.slot0.kI = 0.0;
+        // driveTalonFXConfiguration.slot0.kD = 0.0;
+        // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor =
+        // FeedbackDevice.IntegratedSensor;
+        driveMotor.configFactoryDefault();
+        driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
+        driveMotor.configNominalOutputForward(0, 30);
+        driveMotor.configNominalOutputReverse(0, 30);
+        driveMotor.configPeakOutputForward(1, 30);
+        driveMotor.configPeakOutputReverse(-1, 30);
+        driveMotor.configAllowableClosedloopError(0, 0, 30);
+        // driveMotor.setInverted(true);
+        driveMotor.config_kP(0, 0, 30);
+        driveMotor.config_kI(0, 0, 30);
+        driveMotor.config_kD(0, 0, 30);
+        
+        // driveMotor.configAllSettings(driveTalonFXConfiguration);
+
+        CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
+        canCoderConfiguration.sensorCoefficient = 0.087890625;
+        canCoderConfiguration.magnetOffsetDegrees = offset;
+        canCoderConfiguration.sensorDirection = true;
+        canCoder.configAllSettings(canCoderConfiguration);
+
+        m_currentTime = System.currentTimeMillis();
+        m_lastTime = System.currentTimeMillis();
+
+        m_lastPos = driveMotor.getSelectedSensorPosition();
+    }
+
+    private Rotation2d getAngle() {
+        // ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
+        return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
+    }
+
+    /**
+     * Set the speed + rotation of the swerve module from a SwerveModuleState object
+     *
+     * @param desiredState - A SwerveModuleState representing the desired new state
+     *                     of the module
+     */
+    public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
+        Rotation2d currentRotation = getAngle();
+        // currentRotation.getDegrees());
+        state = SwerveModuleState.optimize(desiredState, currentRotation);
+
+        // Find the difference between our current rotational position + our new
+        // rotational position
+        Rotation2d rotationDelta = state.angle.minus(currentRotation);
+
+        // Find the new absolute position of the module based on the difference in
+        // rotation
+        double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
+        // Convert the CANCoder from it's position reading back to ticks
+        double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
+        double desiredTicks = currentTicks + deltaTicks;
+
+        if (!ignoreAngle) {
+            angleMotor.set(TalonFXControlMode.Position, desiredTicks);
+        }
+
+        // Please work
+        double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
+        double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
+        // double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
+        driveMotor.set(normFtPerSec);// - angleMotor.get());
+        // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio
+        // between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
+    }
+
+    /**
+     * Get current module state.
+     *
+     * @return The current state of the module in m/s.
+     */
+    public SwerveModuleState getState() {
+        // return state;
+        return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK
+                * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
+    }
+
+    /**
+     * Stop the drive and steer motors of current module.
+     */
+    public void stop() {
+        driveMotor.set(0);
+        angleMotor.set(0);
+    }
+
+    public void rotateToAngle(double angle) {
+        this.angleMotor.set(TalonFXControlMode.Position, angle);
+    }
+
+    @Override
+    public void periodic() {
+        currentState = this.getState();
+
+        Rotation2d currentRotation = getAngle();
+        SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
+        SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
+                ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
+
+        lastState = currentState;
+    }
+
+    public void reset() {
+        canCoder.setPositionToAbsolute();
+        // canCoder.configSensorInitializationStrategy(initializationStrategy)
+    }
+    public double getCurrent(){
+        return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
+    }
+
+    public double getVoltage(){
+        return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
+    }
+}