From b49abfe3839e94c58886e5a18ef14d893f3aea55 Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Thu, 28 Mar 2024 19:04:02 -0400 Subject: [PATCH] add new drive ratio for black mamba --- src/main/java/frc/robot/drivetrain/Module.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/drivetrain/Module.java b/src/main/java/frc/robot/drivetrain/Module.java index 1af183d..6aa2489 100644 --- a/src/main/java/frc/robot/drivetrain/Module.java +++ b/src/main/java/frc/robot/drivetrain/Module.java @@ -4,17 +4,20 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import frc.robot.Constants; import org.littletonrobotics.junction.Logger; public class Module { // https://www.swervedrivespecialties.com/products/mk4i-swerve-module // However, our first stage is 50:13 instead of 50:14 because we have a different gear. - public static final double kDriveRatio = (50.0 / 13.0) * (16.0 / 28.0) * (45.0 / 15.0); + private static final double kDriveRatioFirstStage = + Constants.kIsViper ? (50.0 / 13.0) : (50.0 / 14.0); + public static final double kDriveRatio = kDriveRatioFirstStage * (16.0 / 28.0) * (45.0 / 15.0); public static final double kSteerRatio = 150.0 / 7.0; private final double kWheelDiameterMeters = Units.inchesToMeters(1.87 * 2); private final double kWheelRadiusMeters = kWheelDiameterMeters / 2.0; - private final double kCouplingGearRatio = 50.0 / 13.0; + private final double kCouplingRatio = kDriveRatioFirstStage; private final ModuleIO m_io; private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); @@ -39,7 +42,7 @@ public void periodic() { for (int i = 0; i < sampleCount; i++) { double driveRotations = Units.radiansToRotations(m_inputs.odometryDrivePositionsRad[i]); Rotation2d steerAngle = m_inputs.odometryTurnPositions[i]; - driveRotations -= steerAngle.getRotations() * kCouplingGearRatio; + driveRotations -= steerAngle.getRotations() * kCouplingRatio; double driveRadians = Units.rotationsToRadians(driveRotations); double positionMeters = driveRadians * kWheelRadiusMeters; m_odometryPositions[i] = new SwerveModulePosition(positionMeters, steerAngle); @@ -58,7 +61,7 @@ public SwerveModuleState setSetpoint(SwerveModuleState state) { if (optimizedState.speedMetersPerSecond != 0.0) { double azimuthVelocityRPS = Units.radiansToRotations(m_inputs.steerVelocityRadPerSec); - double driveRateBackOut = azimuthVelocityRPS *= kCouplingGearRatio; + double driveRateBackOut = azimuthVelocityRPS *= kCouplingRatio; setpointVelocityRPS += driveRateBackOut; }