diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java index 5cbee566..a2f0280e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java @@ -8,14 +8,14 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; -import frc.trigon.robot.constants.RobotConstants; -import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimatorConstants; -import frc.trigon.robot.subsystems.swerve.SwerveConstants; import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderEncoder; import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderSignal; import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; import frc.trigon.lib.utilities.Conversions; +import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveConstants; public class SwerveModule { private final TalonFXMotor @@ -102,7 +102,9 @@ public void setTargetSteerAngle(Rotation2d angle) { } public SwerveModuleState getCurrentState() { - return new SwerveModuleState(driveWheelRotationsToMeters(driveMotor.getSignal(TalonFXSignal.VELOCITY)), getCurrentSteerAngle()); + final Rotation2d currentSteerAngle = getCurrentSteerAngle(); + final double currentDriveVelocity = driveMotor.getSignal(TalonFXSignal.VELOCITY) - (currentSteerAngle.getRotations() * SwerveModuleConstants.COUPLED_RATIO); + return new SwerveModuleState(driveWheelRotationsToMeters(currentDriveVelocity), currentSteerAngle); } public SwerveModuleState getTargetState() { @@ -115,7 +117,10 @@ public SwerveModuleState getTargetState() { * @return the position of the drive wheel in meters */ public double getDriveWheelPositionRadians() { - return edu.wpi.first.math.util.Units.rotationsToRadians(driveMotor.getSignal(TalonFXSignal.POSITION)); + final double driveWheelPositionRotations = driveMotor.getSignal(TalonFXSignal.POSITION); + final double wheelAngleRotations = getCurrentSteerAngle().getRotations(); + final double driveWheelPositionWithCouplingRatioRotations = driveWheelPositionRotations - (wheelAngleRotations * SwerveModuleConstants.COUPLED_RATIO); + return edu.wpi.first.math.util.Units.rotationsToRadians(driveWheelPositionWithCouplingRatioRotations); } /** @@ -144,6 +149,7 @@ private boolean willOptimize(SwerveModuleState state) { * @param targetVelocityMetersPerSecond the target drive velocity, in meters per second */ private void setTargetDriveVelocity(double targetVelocityMetersPerSecond) { + targetVelocityMetersPerSecond -= steerMotor.getSignal(TalonFXSignal.VELOCITY) * SwerveModuleConstants.COUPLED_RATIO; if (shouldDriveMotorUseClosedLoop) { setTargetClosedLoopDriveVelocity(targetVelocityMetersPerSecond); return; @@ -154,7 +160,6 @@ private void setTargetDriveVelocity(double targetVelocityMetersPerSecond) { private void setTargetClosedLoopDriveVelocity(double targetVelocityMetersPerSecond) { final double targetDriveVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond); - driveMotor.setControl(driveVelocityRequest.withVelocity(targetDriveVelocityRotationsPerSecond)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index 5a0b67c1..dae0c79d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -14,6 +14,7 @@ import frc.trigon.robot.constants.AutonomousConstants; public class SwerveModuleConstants { + static final double COUPLED_RATIO = 3; private static final double DRIVE_MOTOR_GEAR_RATIO = 6.03,//R1: 7.03, R2: 6.03, R3: 5.27 STEER_MOTOR_GEAR_RATIO = 287.0 / 11.0;