diff --git a/src/main/java/com/swervedrivespecialties/swervelib/Mk4SwerveModuleHelper.java b/src/main/java/com/swervedrivespecialties/swervelib/Mk4SwerveModuleHelper.java index 8086b760..7d0a8ab2 100644 --- a/src/main/java/com/swervedrivespecialties/swervelib/Mk4SwerveModuleHelper.java +++ b/src/main/java/com/swervedrivespecialties/swervelib/Mk4SwerveModuleHelper.java @@ -492,7 +492,10 @@ public enum GearRatio { L1(SdsModuleConfigurations.MK4_L1), L2(SdsModuleConfigurations.MK4_L2), L3(SdsModuleConfigurations.MK4_L3), - L4(SdsModuleConfigurations.MK4_L4); + L4(SdsModuleConfigurations.MK4_L4), + L1I(SdsModuleConfigurations.MK4I_L1), + L2I(SdsModuleConfigurations.MK4I_L2), + L3I(SdsModuleConfigurations.MK4I_L3); private final ModuleConfiguration configuration; diff --git a/src/main/java/com/swervedrivespecialties/swervelib/SdsModuleConfigurations.java b/src/main/java/com/swervedrivespecialties/swervelib/SdsModuleConfigurations.java index 21db44f2..b9da9c38 100644 --- a/src/main/java/com/swervedrivespecialties/swervelib/SdsModuleConfigurations.java +++ b/src/main/java/com/swervedrivespecialties/swervelib/SdsModuleConfigurations.java @@ -45,6 +45,28 @@ public final class SdsModuleConfigurations { true ); + public static final ModuleConfiguration MK4I_L1 = new ModuleConfiguration( + 0.10033, + (14.0 / 50.0) * (25.0 / 19.0) * (15.0 / 45.0), + true, + (14.0 / 50.0) * (10.0 / 60.0), + false + ); + public static final ModuleConfiguration MK4I_L2 = new ModuleConfiguration( + 0.10033, + (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0), + true, + (14.0 / 50.0) * (10.0 / 60.0), + false + ); + public static final ModuleConfiguration MK4I_L3 = new ModuleConfiguration( + 0.10033, + (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0), + true, + (14.0 / 50.0) * (10.0 / 60.0), + false + ); + private SdsModuleConfigurations() { } } diff --git a/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java b/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java index 0bf6d2ae..77da81a9 100644 --- a/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java +++ b/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java @@ -122,8 +122,8 @@ public ControllerImplementation create(Falcon500SteerConfiguration steerConfi motor.enableVoltageCompensation(true); } checkCtreError(motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, CAN_TIMEOUT_MS), "Failed to set Falcon 500 feedback sensor"); - motor.setSensorPhase(moduleConfiguration.isSteerInverted()); - motor.setInverted(TalonFXInvertType.CounterClockwise); + motor.setSensorPhase(true); + motor.setInverted(moduleConfiguration.isSteerInverted() ? TalonFXInvertType.CounterClockwise : TalonFXInvertType.Clockwise); motor.setNeutralMode(NeutralMode.Brake); checkCtreError(motor.setSelectedSensorPosition(absoluteEncoder.getAbsoluteAngle() / sensorPositionCoefficient, 0, CAN_TIMEOUT_MS), "Failed to set Falcon 500 encoder position");