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

Updated swervelib

Browse Source
This commit is contained in:
thenetworkgrinch
2023-02-17 16:10:31 -06:00
parent e309987568
commit d7db0f6902
1 changed files with 31 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
swervelib/math/SwerveMath.java
View File

@@ -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.<br /><b>Requires modules to be named: <br
* />"frontright.json", "frontleft.json", "backright.json", "backleft.json"</b>
* 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. <br /><b>Requires modules to be named:<br /> "frontright.json",
* "frontleft.json", "backright.json", "backleft.json"</b>
* 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;
}
}