From 5271c4190faaa906190ab5affde5c36cd73edd8c Mon Sep 17 00:00:00 2001 From: NoamB Date: Sat, 14 Feb 2026 19:59:52 +0200 Subject: [PATCH 1/2] added coupling rations --- .../swerve/swervemodule/SwerveModule.java | 13 +++++++++---- .../swerve/swervemodule/SwerveModuleConstants.java | 3 ++- 2 files changed, 11 insertions(+), 5 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 731c224b..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,8 +14,9 @@ import frc.trigon.robot.constants.AutonomousConstants; public class SwerveModuleConstants { + static final double COUPLED_RATIO = 3; private static final double - DRIVE_MOTOR_GEAR_RATIO = 7.03,//R1: 7.03, R2: 6.03, R3: 5.27 + DRIVE_MOTOR_GEAR_RATIO = 6.03,//R1: 7.03, R2: 6.03, R3: 5.27 STEER_MOTOR_GEAR_RATIO = 287.0 / 11.0; static final boolean ENABLE_FOC = true; From 277fabc4849e3e84e0fad9d1cb712788cf425dd6 Mon Sep 17 00:00:00 2001 From: NoamB Date: Sat, 14 Feb 2026 20:15:56 +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)); }