diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java index 06ec164..4cebfb8 100644 --- a/swervelib/parser/SwerveDriveConfiguration.java +++ b/swervelib/parser/SwerveDriveConfiguration.java @@ -83,28 +83,20 @@ public class SwerveDriveConfiguration } /** - * Assume the first module is the furthest. Usually front-left. + * Calculate the Drive Base Radius * * @return Drive base radius from center of robot to the farthest wheel in meters. */ public double getDriveBaseRadiusMeters() { - //Find Center of Robot by adding all module offsets together. Should be zero, but incase it isn't - Translation2d centerOfModules = moduleLocationsMeters[0].plus(moduleLocationsMeters[1]) - .plus(moduleLocationsMeters[2]) - .plus(moduleLocationsMeters[3]); - - //Find Largest Radius by checking the distance to the center point - double largestRadius = centerOfModules.getDistance(moduleLocationsMeters[0]); - for (int i = 1; i < moduleLocationsMeters.length; i++) - { - if (largestRadius < centerOfModules.getDistance(moduleLocationsMeters[i])) - { - largestRadius = centerOfModules.getDistance(moduleLocationsMeters[i]); - } - } + Translation2d centerOfModules = moduleLocationsMeters[0]; + //Calculate the Center by adding all module offsets together. + for(int i=1; i