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 to 2024.4.7

Browse Source
This commit is contained in:
thenetworkgrinch
2024-01-26 12:45:09 -06:00
parent 053ae733cd
commit 2dcfaac857
110 changed files with 213 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
swervelib/SwerveModule.java
View File

@@ -56,7 +56,7 @@ public class SwerveModule
/**
* Last swerve module state applied.
*/
public SwerveModuleState lastState;
private SwerveModuleState lastState;
/**
* Angle offset from the absolute encoder.
*/
@@ -197,6 +197,9 @@ public class SwerveModule
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
{
desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
// Cosine compensation.
double velocity = configuration.useCosineCompensator ? getCosineCompensatedVelocity(desiredState)
: desiredState.speedMetersPerSecond;
if (isOpenLoop)
{
@@ -204,32 +207,13 @@ public class SwerveModule
driveMotor.set(percentOutput);
} else
{
double cosineScalar = 1.0;
if (configuration.useCosineCompensator)
{
// Taken from the CTRE SwerveModule class.
// https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
/* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
/* To reduce the "skew" that occurs when changing direction */
double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition();
/* If error is close to 0 rotations, we're already there, so apply full power */
/* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError));
/* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
if (cosineScalar < 0.0)
{
cosineScalar = 0.0;
}
}
double velocity = desiredState.speedMetersPerSecond * (cosineScalar);
driveMotor.setReference(velocity, feedforward.calculate(velocity));
}
// If we are forcing the angle
if (!force)
{
// Prevents module rotation if speed is less than 1%
// Prevents module rotation if speed is less than 1%
SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4));
}
@@ -253,6 +237,12 @@ public class SwerveModule
simModule.updateStateAndPosition(desiredState);
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity;
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond);
@@ -260,6 +250,32 @@ public class SwerveModule
}
}
/**
* Get the cosine compensated velocity to set the swerve module to.
*
* @param desiredState Desired {@link SwerveModuleState} to use.
* @return Cosine compensated velocity in meters/second.
*/
private double getCosineCompensatedVelocity(SwerveModuleState desiredState)
{
double cosineScalar = 1.0;
// Taken from the CTRE SwerveModule class.
// https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
/* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
/* To reduce the "skew" that occurs when changing direction */
double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition();
/* If error is close to 0 rotations, we're already there, so apply full power */
/* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError));
/* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
if (cosineScalar < 0.0)
{
cosineScalar = 0.0;
}
return desiredState.speedMetersPerSecond * (cosineScalar);
}
/**
* Set the angle for the module.
*