From 7144a7f8444f39a4b2a47196bdcc99ad5e52e9ca Mon Sep 17 00:00:00 2001 From: NoamB Date: Sat, 14 Feb 2026 19:46:28 +0200 Subject: [PATCH 1/2] added ratio coupling to swerve --- .../swerve/swervemodule/SwerveModule.java | 13 +++++++++---- .../swerve/swervemodule/SwerveModuleConstants.java | 1 + 2 files changed, 10 insertions(+), 4 deletions(-) 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..67dd82f2 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 @@ -115,7 +115,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); } /** @@ -154,6 +157,8 @@ private void setTargetDriveVelocity(double targetVelocityMetersPerSecond) { private void setTargetClosedLoopDriveVelocity(double targetVelocityMetersPerSecond) { final double targetDriveVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond); + final double driveRateBackOut = targetDriveVelocityRotationsPerSecond * SwerveModuleConstants.COUPLED_RATIO; + final double velocityToSet = driveRateBackOut; 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; From 4b0c996cc54dc7c2dfc9b4db36a00c2f3309a6ad Mon Sep 17 00:00:00 2001 From: NoamB Date: Sat, 14 Feb 2026 20:15:46 +0200 Subject: [PATCH 2/2] fixed logic --- .../subsystems/swerve/swervemodule/SwerveModule.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 67dd82f2..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 @@ -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() { @@ -147,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; @@ -157,9 +160,6 @@ private void setTargetDriveVelocity(double targetVelocityMetersPerSecond) { private void setTargetClosedLoopDriveVelocity(double targetVelocityMetersPerSecond) { final double targetDriveVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond); - final double driveRateBackOut = targetDriveVelocityRotationsPerSecond * SwerveModuleConstants.COUPLED_RATIO; - final double velocityToSet = driveRateBackOut; - driveMotor.setControl(driveVelocityRequest.withVelocity(targetDriveVelocityRotationsPerSecond)); }