diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java
index 7bdc16b..79533df 100644
--- a/swervelib/math/SwerveMath.java
+++ b/swervelib/math/SwerveMath.java
@@ -7,9 +7,9 @@ import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.SwerveController;
+import swervelib.SwerveModule;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.parser.SwerveModuleConfiguration;
-import swervelib.parser.SwerveParser;
/**
* Mathematical functions which pertain to swerve drive.
@@ -127,8 +127,7 @@ public class SwerveMath
/**
* Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm position from
- * NetworkTables and is passed the direction in question.
Requires modules to be named:
"frontright.json", "frontleft.json", "backright.json", "backleft.json"
+ * NetworkTables and is passed the direction in question.
*
* @param angle The direction in which to calculate max acceleration, as a Rotation2d. Note that this
* is robot-relative.
@@ -161,25 +160,24 @@ public class SwerveMath
double angDeg = angle.getDegrees();
if (angDeg <= 45 && angDeg >= -45)
{
- SwerveModuleConfiguration conf = SwerveParser.getModuleConfigurationByName("frontleft", config).configuration;
+ SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true);
projectedWheelbaseEdge = new Translation2d(conf.moduleLocation.getX(),
conf.moduleLocation.getX() * angle.getTan());
} else if (135 >= angDeg && angDeg > 45)
{
- SwerveModuleConfiguration conf = SwerveParser.getModuleConfigurationByName("frontleft", config).configuration;
-
+ SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true);
projectedWheelbaseEdge = new Translation2d(
conf.moduleLocation.getY() / angle.getTan(),
conf.moduleLocation.getY());
} else if (-135 <= angDeg && angDeg < -45)
{
- SwerveModuleConfiguration conf = SwerveParser.getModuleConfigurationByName("frontright", config).configuration;
+ SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false);
projectedWheelbaseEdge = new Translation2d(
conf.moduleLocation.getY() / angle.getTan(),
conf.moduleLocation.getY());
} else
{
- SwerveModuleConfiguration conf = SwerveParser.getModuleConfigurationByName("backleft", config).configuration;
+ SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true);
projectedWheelbaseEdge = new Translation2d(
conf.moduleLocation.getX(),
conf.moduleLocation.getX() * angle.getTan());
@@ -195,8 +193,7 @@ public class SwerveMath
/**
* Limits a commanded velocity to prevent exceeding the maximum acceleration given by
* {@link SwerveMath#calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)}. Note that
- * this takes and returns field-relative velocities.
Requires modules to be named:
"frontright.json",
- * "frontleft.json", "backright.json", "backleft.json"
+ * this takes and returns field-relative velocities.
*
* @param commandedVelocity The desired velocity
* @param fieldVelocity The velocity of the robot within a field relative state.
@@ -245,4 +242,28 @@ public class SwerveMath
return commandedVelocity;
}
}
+
+ /**
+ * Get the fruthest module from center based on the module locations.
+ *
+ * @param modules Swerve module list.
+ * @param front True = furthest front, False = furthest back.
+ * @param left True = furthest left, False = furthest right.
+ * @return Module location which is the furthest from center and abides by parameters.
+ */
+ public static SwerveModuleConfiguration getSwerveModule(SwerveModule[] modules, boolean front, boolean left)
+ {
+ Translation2d target = modules[0].configuration.moduleLocation, current, temp;
+ SwerveModuleConfiguration configuration = modules[0].configuration;
+ for (SwerveModule module : modules)
+ {
+ current = module.configuration.moduleLocation;
+ temp = front ? (target.getY() >= current.getY() ? current : target)
+ : (target.getY() <= current.getY() ? current : target);
+ target = left ? (target.getX() >= temp.getX() ? temp : target)
+ : (target.getX() <= temp.getX() ? temp : target);
+ configuration = current.equals(target) ? module.configuration : configuration;
+ }
+ return configuration;
+ }
}