Full Rework

- New SwerveModule subsystem
- Seperates everything out
- Untested
- VERY FUNKY
- Will need an hour or two to clean up and making usable beyond a basic test

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

@@ -35,6 +35,7 @@ public final class Constants {
         public static final double WHEEL_SPEED = 0.1;
         public static final double WIDTH = 22;
         public static final double HEIGHT = 22;
+        public static final double MAX_SPEED_FEET_PER_SEC = 5;
         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;

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

@@ -66,8 +66,8 @@ public class RobotContainer {
 
         // drives the swerve drive with a two-axis input from the driver controller
         m_robotSwerveDrive.setDefaultCommand(
-                new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getRightXAxis(),
+                new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(),
-                        getDriverController().getRightYAxis(), getDriverController().getLeftXAxis()), m_robotSwerveDrive));
+                        getDriverController().getLeftYAxis(), getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
 
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));

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

@@ -131,9 +131,9 @@ public class RobotMap {
 
         // config cancoder as remote encoder for swerve steer motors
         leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     }
 
 }

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

@@ -16,9 +16,11 @@ import edu.wpi.first.wpilibj.geometry.Translation2d;
 import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
+import frc4388.utility.RobotGyro;
 
 public class SwerveDrive extends SubsystemBase
 {
@@ -35,14 +37,39 @@ public class SwerveDrive extends SubsystemBase
     private CANCoder m_rightFrontEncoder;
     private CANCoder m_leftBackEncoder;
     private CANCoder m_rightBackEncoder;    
+    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));
+        //setSwerveGains();
+        
+    private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+    public SwerveModule[] modules;
+    public RobotGyro gyro; //TODO Add Gyro Lol
+
+
     public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
     WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
     CANCoder rightFrontEncoder,
     CANCoder leftBackEncoder,
     CANCoder rightBackEncoder)
-    
     {
         m_leftFrontSteerMotor = leftFrontSteerMotor;
         m_leftFrontWheelMotor = leftFrontWheelMotor;
@@ -56,22 +83,32 @@ public class SwerveDrive extends SubsystemBase
         m_rightFrontEncoder = rightFrontEncoder;
         m_leftBackEncoder = leftBackEncoder;
         m_rightBackEncoder = rightBackEncoder; 
-        double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
-        double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
 
-        Translation2d m_frontLeftLocation = new Translation2d(halfHeight, halfWidth);
+        modules = new SwerveModule[] {
-        Translation2d m_frontRightLocation = new Translation2d(halfHeight, -halfWidth);
+            new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left
-        Translation2d m_backLeftLocation = new Translation2d(-halfHeight, halfWidth);
+            new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right
-        Translation2d m_backRightLocation = new Translation2d(-halfHeight, -halfWidth);
+            new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left
-        
+            new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder)  // Back Right
-        m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+        };
+        //gyro.reset(); 
     }
 
 
-    public void driveWithInput(double strafeX, double strafeY, double rotate)
+    public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative)
     {
-        var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED /*in rad/s */);
+        /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
-        driveFromSpeeds(speeds);
+        driveFromSpeeds(speeds);*/
+        SwerveModuleState[] states =
+            kinematics.toSwerveModuleStates(
+                fieldRelative
+                  ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d())
+                  : new ChassisSpeeds(xSpeed, ySpeed, rot));
+         SwerveDriveKinematics.normalizeWheelSpeeds(states, SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
+         for (int i = 0; i < states.length; i++) {
+            SwerveModule module = modules[i];
+            SwerveModuleState state = states[i];
+            module.setDesiredState(state);
+    }
     }
     //Converts a ChassisSpeed to SwerveModuleStates (targets)
     public void driveFromSpeeds(ChassisSpeeds speeds)
@@ -96,47 +133,47 @@ public class SwerveDrive extends SubsystemBase
     //Sets steering motors to PID values
     public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
     {
-        //Set the Wheel motor speeds
+        /*//Set the Wheel motor speeds
         m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
         m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
         m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
         m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
 
         //PID
-        m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees());
+        m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
-        m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees());
+        m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
-        m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees());
+        m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
         m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
+
+        System.out.println("Target: " + leftFront.angle.getDegrees());*/
     }    
     
-    public void setSwerveGains(){
+    /*public void setSwerveGains(){
-        m_leftFrontWheelMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
+        m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
-        m_leftFrontWheelMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_leftFrontWheelMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_leftFrontWheelMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_leftFrontWheelMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 
-        m_rightFrontWheelMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
+        m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
-        m_rightFrontWheelMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_rightFrontWheelMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_rightFrontWheelMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_rightFrontWheelMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 
-        m_leftBackWheelMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
+        m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
-        m_leftBackWheelMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_leftBackWheelMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_leftBackWheelMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_leftBackWheelMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 
-        m_rightBackWheelMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
+        m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
-        m_rightBackWheelMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_rightBackWheelMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_rightBackWheelMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-        m_rightBackWheelMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+        m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
-        // PID
         
-    }
+    }*/
 
 
 

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

@@ -0,0 +1,95 @@
+// 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.wpilibj2.command.SubsystemBase;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.util.Units;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.Gains;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+public class SwerveModule extends SubsystemBase {
+    private WPI_TalonFX driveMotor;
+    private WPI_TalonFX angleMotor;
+    private CANCoder canCoder;
+    public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
+
+    private static double kEncoderTicksPerRotation = 4096;
+
+
+  /** Creates a new SwerveModule. */
+  public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder) {
+    this.driveMotor = driveMotor;
+    this.angleMotor = angleMotor;
+    this.canCoder = canCoder;
+
+    TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
+
+    angleTalonFXConfiguration.slot0.kP = m_swerveGains.m_kP;
+    angleTalonFXConfiguration.slot0.kI = m_swerveGains.m_kI;
+    angleTalonFXConfiguration.slot0.kD = m_swerveGains.m_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);
+
+    /*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
+
+    driveTalonFXConfiguration.slot0.kP = kDriveP;
+    driveTalonFXConfiguration.slot0.kI = kDriveI;
+    driveTalonFXConfiguration.slot0.kD = kDriveD;
+    driveTalonFXConfiguration.slot0.kF = kDriveF;
+
+    driveMotor.configAllSettings(driveTalonFXConfiguration);*/
+
+    CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
+    canCoder.configAllSettings(canCoderConfiguration);
+  }
+
+
+  public Rotation2d getAngle() {
+    // Note: This assumes the CANCoders are setup with the default feedback coefficient
+    // and the sesnor 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) {
+    Rotation2d currentRotation = getAngle();
+    SwerveModuleState 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() / canCoder.configGetFeedbackCoefficient();
+    double desiredTicks = currentTicks + deltaTicks;
+    angleMotor.set(TalonFXControlMode.Position, desiredTicks);
+
+
+    double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
+    driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
+  }
+}