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

Update to 2024.4.8

Browse Source
This commit is contained in:
thenetworkgrinch
2024-02-02 18:55:29 -06:00
parent 2dcfaac857
commit c478a800b2
118 changed files with 2009 additions and 607 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
swervelib/SwerveModule.java
View File

@@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
import swervelib.motors.SwerveMotor;
import swervelib.parser.Cache;
import swervelib.parser.SwerveModuleConfiguration;
import swervelib.simulation.SwerveModuleSimulation;
import swervelib.telemetry.Alert;
@@ -25,6 +26,18 @@ public class SwerveModule
* Swerve module configuration options.
*/
public final SwerveModuleConfiguration configuration;
/**
* Absolute encoder position cache.
*/
public final Cache<Double> absolutePositionCache;
/**
* Drive motor position cache.
*/
public final Cache<Double> drivePositionCache;
/**
* Drive motor velocity cache.
*/
public final Cache<Double> driveVelocityCache;
/**
* Swerve Motors.
*/
@@ -70,6 +83,7 @@ public class SwerveModule
*/
private boolean synchronizeEncoderQueued = false;
/**
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
*
@@ -114,6 +128,9 @@ public class SwerveModule
absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
}
// Setup the cache for the absolute encoder position.
absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 15);
// Config angle motor/controller
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle);
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
@@ -136,11 +153,20 @@ public class SwerveModule
driveMotor.burnFlash();
angleMotor.burnFlash();
drivePositionCache = new Cache<>(driveMotor::getPosition, 15);
driveVelocityCache = new Cache<>(driveMotor::getVelocity, 15);
if (SwerveDriveTelemetry.isSimulation)
{
simModule = new SwerveModuleSimulation();
}
// Force a cache update on init.
driveVelocityCache.update();
drivePositionCache.update();
absolutePositionCache.update();
// Save the current state.
lastState = getState();
noEncoderWarning = new Alert("Motors",
@@ -198,8 +224,9 @@ public class SwerveModule
{
desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
// Cosine compensation.
double velocity = configuration.useCosineCompensator ? getCosineCompensatedVelocity(desiredState)
: desiredState.speedMetersPerSecond;
double velocity = configuration.useCosineCompensator
? getCosineCompensatedVelocity(desiredState)
: desiredState.speedMetersPerSecond;
if (isOpenLoop)
{
@@ -298,7 +325,7 @@ public class SwerveModule
Rotation2d azimuth;
if (!SwerveDriveTelemetry.isSimulation)
{
velocity = driveMotor.getVelocity();
velocity = driveVelocityCache.getValue();
azimuth = Rotation2d.fromDegrees(getAbsolutePosition());
} else
{
@@ -318,7 +345,7 @@ public class SwerveModule
Rotation2d azimuth;
if (!SwerveDriveTelemetry.isSimulation)
{
position = driveMotor.getPosition();
position = drivePositionCache.getValue();
azimuth = Rotation2d.fromDegrees(getAbsolutePosition());
} else
{
@@ -333,6 +360,16 @@ public class SwerveModule
* @return Absolute encoder angle in degrees in the range [0, 360).
*/
public double getAbsolutePosition()
{
return absolutePositionCache.getValue();
}
/**
* Get the absolute position. Falls back to relative position on reading failure.
*
* @return Absolute encoder angle in degrees in the range [0, 360).
*/
public double getRawAbsolutePosition()
{
double angle;
if (absoluteEncoder != null)