From 1f7e20050da6f13f0bde7875a91c71fa6a7df3d7 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Thu, 18 Jan 2024 11:03:13 -0600 Subject: [PATCH] Fixed pathplanner util function for 3 wheeled module --- .../parser/SwerveDriveConfiguration.java | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) 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