Skip to content

Commit

Permalink
add coupling compensation to getPosition
Browse files Browse the repository at this point in the history
  • Loading branch information
Tsundoiii committed Feb 19, 2024
1 parent 72ed297 commit a771f29
Show file tree
Hide file tree
Showing 2 changed files with 5 additions and 39 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@ public static class SwerveConstants {
public DCMotor STEER_GEARBOX_REPR;
public double DRIVE_MOI;
public double STEER_MOI;

public double COUPLING_RATIO;
}

public double getDriveMotorPosition();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,9 @@ public Rotation2d getWheelAngle() {
@Override
@Log
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(getDriveMotorPosition(), getWheelAngle());
return new SwerveModulePosition(
getDriveMotorPosition() - getWheelAngle().getRotations() * SWERVE_CONSTANTS.COUPLING_RATIO,
getWheelAngle());
}

@Override
Expand Down Expand Up @@ -260,42 +262,4 @@ public void setState(SwerveModuleState state) {
public void setStateClosedLoop(SwerveModuleState state) {
setStateInternal(state, false);
}

/**
* Find the coupling ratio (ratio of drive motor rotations to azimuth
* rotations) for module.
*
* @param volts Voltage to send to angle motors to spin.
* @param automatic Attempt to automatically spin the modules.
* @return Average coupling ratio.
*/
public double findCouplingRatio(double volts, boolean automatic) {
System.out.println("Stopping the Swerve Drive.");
driveMotor.setVoltage(0);
steerMotor.setVoltage(0);
Timer.delay(1);

System.out.println("Fetching the current absolute encoder and drive encoder position.");
Timer.delay(1);
Rotation2d startingAzimuthPosition = Rotation2d
.fromRotations(steerCanCoder.getAbsolutePosition().getValue());
double startingDriveMotorPosition = getDriveMotorPosition();

if (automatic) {
System.out.println("Rotating the module 360 degrees");
while (!Rotation2d.fromRotations(steerCanCoder.getAbsolutePosition().getValue())
.equals(startingAzimuthPosition)) {
steerMotor.setVoltage(volts);
}

steerMotor.setVoltage(0);
} else {
DriverStation.reportWarning(
"Spin the module 360 degrees now, you have 1 minute.\n",
false);
Timer.delay(60);
}

return getDriveMotorPosition() - startingDriveMotorPosition;
}
}

0 comments on commit a771f29

Please sign in to comment.