diff --git a/src/main/cpp/cowlib/SwerveModule.cpp b/src/main/cpp/cowlib/SwerveModule.cpp index c06b39d..5171130 100644 --- a/src/main/cpp/cowlib/SwerveModule.cpp +++ b/src/main/cpp/cowlib/SwerveModule.cpp @@ -8,6 +8,7 @@ #include "frc/kinematics/SwerveModuleState.h" #include "rev/CANSparkMax.h" #include "units/angle.h" +#include "units/base.h" #include "units/voltage.h" #include "cowlib/SwerveModule.h" @@ -29,6 +30,15 @@ SwerveModule::SwerveModule(int driveMotorId, int steerMotorId, int encoderId, this->driveMotor.SetSmartCurrentLimit(SwerveConstants::currentLimit); this->steerMotor.SetSmartCurrentLimit(SwerveConstants::currentLimit); + + auto speedEncoder = this->driveMotor.GetEncoder(); + double driveReduction = 1.0 / 6.75; + double wheelDiameter = 0.1016; + double rotationsToDistance = + driveReduction * wheelDiameter * units::constants::detail::PI_VAL; + + speedEncoder.SetPositionConversionFactor(rotationsToDistance); + speedEncoder.SetVelocityConversionFactor(rotationsToDistance / 60); } void SwerveModule::drive(double speedMetersPerSecond, double angle) {