Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed pathplanner util function for 3 wheeled module

Browse Source
This commit is contained in:
thenetworkgrinch
2024-01-18 11:03:13 -06:00
parent 8528c50c04
commit 1f7e20050d
1 changed files with 8 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
swervelib/parser/SwerveDriveConfiguration.java
View File

@@ -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<moduleLocationsMeters.length; i++){
centerOfModules = centerOfModules.plus(moduleLocationsMeters[i]);
}
//Return Largest Radius
return largestRadius;
return centerOfModules.getDistance(moduleLocationsMeters[0]);
}
}