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

Upgrading to 2025.7.0

Browse Source
This commit is contained in:
thenetworkgrinch
2025-02-22 06:15:56 +00:00
parent 62f8236678
commit 4016ee2190
41 changed files with 2237 additions and 557 deletions
Download Patch File Download Diff File Expand all files Collapse all files

249
swervelib/SwerveModule.java
View File

@@ -1,5 +1,6 @@
package swervelib;
import static edu.wpi.first.units.Units.InchesPerSecond;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecond;
@@ -33,7 +34,7 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
/**
* The Swerve Module class which represents and controls Swerve Modules for the swerve drive.
*/
public class SwerveModule
public class SwerveModule implements AutoCloseable
{
/**
@@ -72,6 +73,29 @@ public class SwerveModule
* An {@link Alert} for if there is no Absolute Encoder on the module.
*/
private final Alert noEncoderWarning;
/**
* An {@link Alert} for if there is no Absolute Encoder on the module.
*/
private final Alert externalSensorIsNull = new Alert("No absolute Encoder found.",
AlertType.kError);
/**
* An {@link Alert} for if the offset is 0 degrees.
*/
private final Alert internalOffsetIsZero = new Alert(
"Absolute encoder offset is 0, this may be a problem.",
AlertType.kWarning);
/**
* An {@link Alert} for if the angle/steer/azimuth motor is incompatible with the absolute encoder.
*/
private final Alert externalFeedbackIncompatible = new Alert(
"Absolute encoder is incompatible, cannot set as an external feedback device.",
AlertType.kError);
/**
* An {@link Alert} for if the absolute encoder cannot set an offset.
*/
private final Alert externalOffsetIncompatible = new Alert(
"Absolute encoder is incompatible, cannot set an offset internally.",
AlertType.kError);
/**
* NT4 Raw Absolute Angle publisher for the absolute encoder.
*/
@@ -108,6 +132,10 @@ public class SwerveModule
* Maximum {@link LinearVelocity} for the drive motor of the swerve module.
*/
private LinearVelocity maxDriveVelocity;
/**
* Maximum velocity for the drive motor of the swerve module.
*/
private double maxDriveVelocityMetersPerSecond;
/**
* Maximum {@link AngularVelocity} for the azimuth/angle motor of the swerve module.
*/
@@ -119,7 +147,7 @@ public class SwerveModule
/**
* Anti-Jitter AKA auto-centering disabled.
*/
private boolean antiJitterEnabled = true;
private boolean antiJitterEnabled = true;
/**
* Last swerve module state applied.
*/
@@ -132,18 +160,22 @@ public class SwerveModule
* Simulated swerve module.
*/
private SwerveModuleSimulation simModule;
/**
* Enables utilization off {@link SwerveModuleState#optimize(Rotation2d)}
*/
private boolean optimizeSwerveModuleState = true;
/**
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
private boolean synchronizeEncoderQueued = false;
/**
* Encoder, Absolute encoder synchronization enabled.
*/
private boolean synchronizeEncoderEnabled = false;
private boolean synchronizeEncoderEnabled = false;
/**
* Encoder synchronization deadband in degrees.
*/
private double synchronizeEncoderDeadband = 3;
private double synchronizeEncoderDeadband = 3;
/**
@@ -196,7 +228,10 @@ public class SwerveModule
absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20);
// Config angle motor/controller
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor);
if (!angleMotor.usingExternalFeedbackSensor())
{
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor);
}
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
angleMotor.configurePIDWrapping(0, 360);
angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
@@ -255,6 +290,14 @@ public class SwerveModule
"swerve/modules/" + configuration.name + "/Angle Setpoint").publish();
}
@Override
public void close()
{
angleMotor.close();
driveMotor.close();
absoluteEncoder.close();
}
/**
* Get the default {@link SimpleMotorFeedforward} for the swerve module drive motor.
*
@@ -269,6 +312,31 @@ public class SwerveModule
configuration.physicalCharacteristics.wheelGripCoefficientOfFriction);
}
/**
* Set utilization of {@link SwerveModuleState#optimize(Rotation2d)} which should be disabled for some debugging.
*
* @param optimizationState Optimization enabled.
*/
public void setModuleStateOptimization(boolean optimizationState)
{
optimizeSwerveModuleState = optimizationState;
if (!optimizeSwerveModuleState)
{
angleMotor.disablePIDWrapping();
angleMotor.burnFlash();
}
}
/**
* Check if the module state optimization used by {@link SwerveModuleState#optimize(Rotation2d)} is enabled.
*
* @return optimization state.
*/
public boolean getModuleStateOptimization()
{
return optimizeSwerveModuleState;
}
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -406,27 +474,20 @@ public class SwerveModule
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
{
desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition()));
// If we are forcing the angle
if (!force && antiJitterEnabled)
{
// Prevents module rotation if speed is less than 1%
SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocity.in(MetersPerSecond), 4));
}
applyStateOptimizations(desiredState);
applyAntiJitter(desiredState, force);
// Cosine compensation.
LinearVelocity nextVelocity = configuration.useCosineCompensator
? getCosineCompensatedVelocity(desiredState)
: MetersPerSecond.of(desiredState.speedMetersPerSecond);
LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond);
desiredState.speedMetersPerSecond = nextVelocity.magnitude();
double nextVelocityMetersPerSecond = configuration.useCosineCompensator
? getCosineCompensatedVelocity(desiredState)
: desiredState.speedMetersPerSecond;
double curVelocityMetersPerSecond = lastState.speedMetersPerSecond;
desiredState.speedMetersPerSecond = nextVelocityMetersPerSecond;
setDesiredState(desiredState,
isOpenLoop,
driveMotorFeedforward.calculateWithVelocities(curVelocity.in(MetersPerSecond),
nextVelocity.in(MetersPerSecond)));
driveMotorFeedforward.calculateWithVelocities(curVelocityMetersPerSecond,
nextVelocityMetersPerSecond));
}
/**
@@ -440,7 +501,6 @@ public class SwerveModule
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
double driveFeedforwardVoltage)
{
if (isOpenLoop)
{
double percentOutput = desiredState.speedMetersPerSecond / maxDriveVelocity.in(MetersPerSecond);
@@ -497,7 +557,7 @@ public class SwerveModule
* @param desiredState Desired {@link SwerveModuleState} to use.
* @return Cosine compensated velocity in meters/second.
*/
private LinearVelocity getCosineCompensatedVelocity(SwerveModuleState desiredState)
private double getCosineCompensatedVelocity(SwerveModuleState desiredState)
{
double cosineScalar = 1.0;
// Taken from the CTRE SwerveModule class.
@@ -515,7 +575,39 @@ public class SwerveModule
cosineScalar = 1;
}
return MetersPerSecond.of(desiredState.speedMetersPerSecond).times(cosineScalar);
return desiredState.speedMetersPerSecond * cosineScalar;
}
/**
* Apply the {@link SwerveModuleState#optimize(Rotation2d)} function if the module state optimization is enabled while
* debugging.
*
* @param desiredState The desired state to apply the optimization to.
*/
public void applyStateOptimizations(SwerveModuleState desiredState)
{
// SwerveModuleState optimization might be desired to be disabled while debugging.
if (optimizeSwerveModuleState)
{
desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition()));
}
}
/**
* Apply anti-jitter to the desired state. This will prevent the module from rotating if the speed requested is too
* low. If force is true, the anti-jitter will not be applied.
*
* @param desiredState The desired state to apply the anti-jitter to.
* @param force Whether to ignore the {@link SwerveModule#antiJitterEnabled} state and apply the anti-jitter
* anyway.
*/
public void applyAntiJitter(SwerveModuleState desiredState, boolean force)
{
if (!force && antiJitterEnabled)
{
// Prevents module rotation if speed is less than 1%
SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocityMetersPerSecond, 4));
}
}
/**
@@ -605,10 +697,13 @@ public class SwerveModule
{
angle = getRelativePosition();
}
angle %= 360;
if (angle < 0.0)
if (optimizeSwerveModuleState)
{
angle += 360;
angle %= 360;
if (angle < 0.0)
{
angle += 360;
}
}
return angle;
@@ -696,9 +791,64 @@ public class SwerveModule
return configuration;
}
/**
* Use external sensors for the feedback of the angle/azimuth/steer controller.
*/
public void useExternalFeedbackSensor()
{
if (absoluteEncoder == null)
{
externalSensorIsNull.set(true);
return;
}
if (angleOffset == 0)
{
internalOffsetIsZero.set(true);
}
if (absoluteEncoder.setAbsoluteEncoderOffset(configuration.angleOffset))
{
angleMotor.setAbsoluteEncoder(absoluteEncoder);
if (angleMotor.usingExternalFeedbackSensor())
{
angleOffset = 0;
} else
{
externalFeedbackIncompatible.set(true);
angleMotor.setAbsoluteEncoder(null);
absoluteEncoder.setAbsoluteEncoderOffset(0);
}
} else
{
externalOffsetIncompatible.set(true);
absoluteEncoder.setAbsoluteEncoderOffset(0);
}
}
/**
* Use external sensors for the feedback of the angle/azimuth/steer controller.
*/
public void useInternalFeedbackSensor()
{
if (absoluteEncoder == null)
{
externalSensorIsNull.set(true);
return;
}
if (angleOffset == 0)
{
internalOffsetIsZero.set(true);
}
angleMotor.setAbsoluteEncoder(null);
absoluteEncoder.setAbsoluteEncoderOffset(0);
angleOffset = configuration.angleOffset;
}
/**
* Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset.
*/
@Deprecated
public void pushOffsetsToEncoders()
{
if (absoluteEncoder != null && angleOffset == configuration.angleOffset)
@@ -759,15 +909,27 @@ public class SwerveModule
* @return {@link LinearVelocity} max velocity of the drive wheel.
*/
public LinearVelocity getMaxVelocity()
{
getMaxDriveVelocityMetersPerSecond();
return maxDriveVelocity;
}
/**
* Get the maximum drive velocity of the module in Meters Per Second.
*
* @return Maximum drive motor velocity in Meters Per Second.
*/
public double getMaxDriveVelocityMetersPerSecond()
{
if (maxDriveVelocity == null)
{
maxDriveVelocity = MetersPerSecond.of(
(RadiansPerSecond.of(driveMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) /
maxDriveVelocity = InchesPerSecond.of(
(driveMotor.getSimMotor().freeSpeedRadPerSec /
configuration.conversionFactors.drive.gearRatio) *
configuration.conversionFactors.drive.diameter);
configuration.conversionFactors.drive.diameter / 2.0);
maxDriveVelocityMetersPerSecond = maxDriveVelocity.in(MetersPerSecond);
}
return maxDriveVelocity;
return maxDriveVelocityMetersPerSecond;
}
/**
@@ -780,7 +942,7 @@ public class SwerveModule
if (maxAngularVelocity == null)
{
maxAngularVelocity = RotationsPerSecond.of(
RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) *
RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) /
configuration.conversionFactors.angle.gearRatio);
}
return maxAngularVelocity;
@@ -795,11 +957,26 @@ public class SwerveModule
{
rawAbsoluteAnglePublisher.set(absoluteEncoder.getAbsolutePosition());
}
rawAnglePublisher.set(angleMotor.getPosition());
rawDriveEncoderPublisher.set(drivePositionCache.getValue());
rawDriveVelocityPublisher.set(driveVelocityCache.getValue());
if (SwerveDriveTelemetry.isSimulation && SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SwerveModulePosition pos = simModule.getPosition();
SwerveModuleState state = simModule.getState();
rawAnglePublisher.set(pos.angle.getDegrees());
rawDriveEncoderPublisher.set(pos.distanceMeters);
rawDriveVelocityPublisher.set(state.speedMetersPerSecond);
// For code coverage
angleMotor.getPosition();
drivePositionCache.getValue();
driveVelocityCache.getValue();
} else
{
rawAnglePublisher.set(angleMotor.getPosition());
rawDriveEncoderPublisher.set(drivePositionCache.getValue());
rawDriveVelocityPublisher.set(driveVelocityCache.getValue());
}
adjAbsoluteAnglePublisher.set(getAbsolutePosition());
absoluteEncoderIssuePublisher.set(getAbsoluteEncoderReadIssue());
}
/**