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

Added some helper functions and cleared up some optimizations

Browse Source
This commit is contained in:
thenetworkgrinch
2023-03-20 19:20:26 -05:00
parent 9887cf3c26
commit 670154f061
111 changed files with 445 additions and 280 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
swervelib/SwerveDrive.java
View File

@@ -4,6 +4,7 @@ import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
@@ -222,13 +223,16 @@ public class SwerveDrive
// Heading Angular Velocity Deadband, might make a configuration option later.
// Originally made by Team 1466 Webb Robotics.
if (Math.abs(rotation) < 0.01 && headingCorrection)
if (headingCorrection)
{
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians());
} else
{
lastHeadingRadians = getYaw().getRadians();
if (Math.abs(rotation) < 0.01)
{
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians());
} else
{
lastHeadingRadians = getYaw().getRadians();
}
}
// Display commanded speed for testing
@@ -706,6 +710,31 @@ public class SwerveDrive
imu.setOffset(imu.getRawRotation3d().minus(gyro));
}
/**
* Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} which can be used to
* generate {@link ChassisSpeeds} for the robot to orient it correctly given axis or angles, and apply
* {@link edu.wpi.first.math.filter.SlewRateLimiter} to given inputs. Important functions to look at are
* {@link SwerveController#getTargetSpeeds(double, double, double, double)},
* {@link SwerveController#addSlewRateLimiters(SlewRateLimiter, SlewRateLimiter, SlewRateLimiter)},
* {@link SwerveController#getRawTargetSpeeds(double, double, double)}.
*
* @return {@link SwerveController} for the {@link SwerveDrive}.
*/
public SwerveController getSwerveController()
{
return swerveController;
}
/**
* Get the {@link SwerveModule}s associated with the {@link SwerveDrive}.
*
* @return {@link SwerveModule} array specified by configurations.
*/
public SwerveModule[] getModules()
{
return swerveDriveConfiguration.modules;
}
/**
* Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in
* autonomous.