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

Updated docs and swervelib

Browse Source
This commit is contained in:
thenetworkgrinch
2023-02-23 21:12:29 -06:00
parent b1781a2238
commit f5c63d5c6f
108 changed files with 279 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
swervelib/SwerveDrive.java
View File

@@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -122,20 +123,23 @@ public class SwerveDrive
SwerveDriveTelemetry.maxSpeed = swerveDriveConfiguration.maxSpeed;
SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity;
SwerveDriveTelemetry.moduleCount = swerveModules.length;
SwerveDriveTelemetry.sizeFrontBack = (SwerveMath.getSwerveModule(swerveModules, true, false).moduleLocation.getX() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getX()) / 2;
SwerveDriveTelemetry.sizeLeftRight = (SwerveMath.getSwerveModule(swerveModules, false, true).moduleLocation.getY() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getY()) / 2;
SwerveDriveTelemetry.sizeFrontBack = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, true,
false).moduleLocation.getX() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getX());
SwerveDriveTelemetry.sizeLeftRight = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, false,
true).moduleLocation.getY() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getY());
SwerveDriveTelemetry.wheelLocations = new double[SwerveDriveTelemetry.moduleCount * 2];
for (SwerveModule module : swerveModules)
{
SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = module.configuration.moduleLocation.getX() / 2;
SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] =
module.configuration.moduleLocation.getY() / 2;
SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = Units.metersToInches(
module.configuration.moduleLocation.getX());
SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] = Units.metersToInches(
module.configuration.moduleLocation.getY());
}
SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
@@ -215,6 +219,10 @@ public class SwerveDrive
*/
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
{
SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
}