From 39f50feda436dcc32f31aa6e317641385892171f Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 16 Dec 2021 21:48:59 -0700 Subject: [PATCH] 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 | 1 + .../java/frc4388/robot/RobotContainer.java | 4 +- src/main/java/frc4388/robot/RobotMap.java | 6 +- .../frc4388/robot/subsystems/SwerveDrive.java | 117 ++++++++++++------ .../robot/subsystems/SwerveModule.java | 95 ++++++++++++++ 5 files changed, 178 insertions(+), 45 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/SwerveModule.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9d3a05b..31b2ccf 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b248748..c697a7f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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(), - getDriverController().getRightYAxis(), getDriverController().getLeftXAxis()), m_robotSwerveDrive)); + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(), + 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)); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5b3ec1a..c3c23c7 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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); - 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); + //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); + //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ebe9d94..1a2abef 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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); - Translation2d m_frontRightLocation = new Translation2d(halfHeight, -halfWidth); - Translation2d m_backLeftLocation = new Translation2d(-halfHeight, halfWidth); - Translation2d m_backRightLocation = new Translation2d(-halfHeight, -halfWidth); - - m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); + modules = new SwerveModule[] { + new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left + new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right + new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left + new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder) // Back Right + }; + //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 */); - driveFromSpeeds(speeds); + /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); + 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_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees()); - m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees()); + m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000); + m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000); + 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(){ - m_leftFrontWheelMotor.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_leftFrontWheelMotor.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_leftFrontWheelMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + /*public void setSwerveGains(){ + m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); + m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, 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_rightFrontWheelMotor.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_rightFrontWheelMotor.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.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); + m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, 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_leftBackWheelMotor.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_leftBackWheelMotor.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.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); + m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, 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_rightBackWheelMotor.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_rightBackWheelMotor.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); - - // PID + m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX); + m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - } + }*/ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..bc68cda --- /dev/null +++ b/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); + } +}