Skip to content

Ratio Coupling #48

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ShmayaR wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Ratio Coupling #48

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5271c41
added coupling rations
ShmayaR Feb 14, 2026
277fabc
fixed logic
ShmayaR Feb 14, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 11 additions & 6 deletions src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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() {
Expand All @@ -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);
}
Comment on lines 119 to 124
Copy link

@coderabbitai coderabbitai bot Feb 14, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🟠 Major

🧩 Analysis chain

🏁 Script executed:

#!/bin/bash
# First, let's locate and examine the SwerveModule.java file
find . -name "SwerveModule.java" -type f

Repository: Programming-TRIGON/RobotCode2026

Length of output: 157

🏁 Script executed:

#!/bin/bash
# Read the relevant section of the file (around lines 117-136)
sed -n '110,145p' ./src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java

Repository: Programming-TRIGON/RobotCode2026

Length of output: 1939

🏁 Script executed:

#!/bin/bash
# Search for calls to getDriveWheelPositionRadians and getOdometryPosition
rg -n --type=java 'getDriveWheelPositionRadians|getOdometryPosition' --max-count=30

Repository: Programming-TRIGON/RobotCode2026

Length of output: 690

🏁 Script executed:

#!/bin/bash
# Search for where latestOdometryDrivePositions is populated
rg -n --type=java 'latestOdometryDrivePositions\s*=' -B2 -A2

Repository: Programming-TRIGON/RobotCode2026

Length of output: 714

🏁 Script executed:

#!/bin/bash
# Look at the full SwerveModule class to understand the context better
wc -l ./src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java

Repository: Programming-TRIGON/RobotCode2026

Length of output: 161

🏁 Script executed:

#!/bin/bash
# Look at the context around where latestOdometryDrivePositions and latestOdometrySteerPositions are populated
sed -n '75,95p' ./src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java

Repository: Programming-TRIGON/RobotCode2026

Length of output: 896

🏁 Script executed:

#!/bin/bash
# Check the whole update method and surrounding context
sed -n '70,100p' ./src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java

Repository: Programming-TRIGON/RobotCode2026

Length of output: 1272

🏁 Script executed:

#!/bin/bash
# Search for any comments or documentation about coupling correction in odometry
rg -n --type=java 'coupling|COUPLED_RATIO' -B2 -A2

Repository: Programming-TRIGON/RobotCode2026

Length of output: 2406

🏁 Script executed:

#!/bin/bash
# Check how driveWheelRotationsToMeters is implemented to understand the conversion
rg -n --type=java 'driveWheelRotationsToMeters' -A5

Repository: Programming-TRIGON/RobotCode2026

Length of output: 2309

🏁 Script executed:

#!/bin/bash
# Look at the broader context of where these methods are called to understand intent
sed -n '145,160p' ./src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java

Repository: Programming-TRIGON/RobotCode2026

Length of output: 824

🏁 Script executed:

#!/bin/bash
# Check around line 326 in Swerve.java to see how getOdometryPosition is used
sed -n '320,335p' ./src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java

Repository: Programming-TRIGON/RobotCode2026

Length of output: 1022

Apply coupling correction in getOdometryPosition for consistency with getDriveWheelPositionRadians.

getDriveWheelPositionRadians compensates for steering coupling, but getOdometryPosition calls driveWheelRotationsToMeters on the raw odometry drive position without any correction. This inconsistency means pose estimates from odometry will diverge from the coupling-corrected position used elsewhere.

Apply the same correction in getOdometryPosition using the odometry steer angle snapshot (latestOdometrySteerPositions[odometryUpdateIndex]) to maintain temporal consistency:

final double drivePositionWithCouplingCorrected = latestOdometryDrivePositions[odometryUpdateIndex] - 
    (Rotation2d.fromRotations(latestOdometrySteerPositions[odometryUpdateIndex]).getRotations() * SwerveModuleConstants.COUPLED_RATIO);
return new SwerveModulePosition(
    driveWheelRotationsToMeters(drivePositionWithCouplingCorrected),
    Rotation2d.fromRotations(latestOdometrySteerPositions[odometryUpdateIndex])
);


/**
Expand Down Expand Up @@ -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;
Expand All @@ -154,7 +160,6 @@ private void setTargetDriveVelocity(double targetVelocityMetersPerSecond) {

private void setTargetClosedLoopDriveVelocity(double targetVelocityMetersPerSecond) {
final double targetDriveVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond);

driveMotor.setControl(driveVelocityRequest.withVelocity(targetDriveVelocityRotationsPerSecond));
}
Comment on lines 161 to 164
Copy link

@coderabbitai coderabbitai bot Feb 14, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🔴 Critical

velocityToSet is computed but never used—the motor still receives the uncorrected velocity.

Line 163 passes targetDriveVelocityRotationsPerSecond to the motor instead of velocityToSet. The coupling compensation has no effect.

Additionally, the formula itself looks incorrect. Coupling compensation for velocity should involve the steer velocity, not the drive velocity. The typical pattern is:

velocityToSet = targetDriveVelocityRotationsPerSecond + steerVelocityRotationsPerSecond * COUPLED_RATIO

This adds a feed-forward term to counteract the parasitic drive caused by steering motion. Right now driveRateBackOut = targetDriveVelocityRotationsPerSecond * COUPLED_RATIO scales the drive velocity by the coupling ratio, which doesn't model the physical coupling.

Proposed fix (verify the coupling direction sign for your module) 
 private void setTargetClosedLoopDriveVelocity(double targetVelocityMetersPerSecond) {
     final double targetDriveVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond);
-    final double driveRateBackOut = targetDriveVelocityRotationsPerSecond * SwerveModuleConstants.COUPLED_RATIO;
-    final double velocityToSet = driveRateBackOut;
+    final double steerVelocityRotationsPerSecond = steerMotor.getSignal(TalonFXSignal.VELOCITY);
+    final double couplingCompensation = steerVelocityRotationsPerSecond * SwerveModuleConstants.COUPLED_RATIO;
+    final double velocityToSet = targetDriveVelocityRotationsPerSecond + couplingCompensation;

-    driveMotor.setControl(driveVelocityRequest.withVelocity(targetDriveVelocityRotationsPerSecond));
+    driveMotor.setControl(driveVelocityRequest.withVelocity(velocityToSet));
 }
📝 Committable suggestion

‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.

Suggested change
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));
}
private void setTargetClosedLoopDriveVelocity(double targetVelocityMetersPerSecond) {
final double targetDriveVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond);
final double steerVelocityRotationsPerSecond = steerMotor.getSignal(TalonFXSignal.VELOCITY);
final double couplingCompensation = steerVelocityRotationsPerSecond * SwerveModuleConstants.COUPLED_RATIO;
final double velocityToSet = targetDriveVelocityRotationsPerSecond + couplingCompensation;
driveMotor.setControl(driveVelocityRequest.withVelocity(velocityToSet));
}


Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,9 @@
import frc.trigon.robot.constants.AutonomousConstants;

public class SwerveModuleConstants {
static final double COUPLED_RATIO = 3;
Copy link

@coderabbitai coderabbitai bot Feb 14, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🧹 Nitpick | 🔵 Trivial

Add a brief comment explaining what COUPLED_RATIO represents physically.

The other gear ratios have clarifying comments (e.g., R1: 7.03, R2: 6.03, R3: 5.27). A similar note here—e.g., the bevel gear teeth ratio or the mechanical source of this value—would help future readers understand why it's 3 and when it should change.

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;

Expand Down
Loading