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; + } }