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

Browse Source
This commit is contained in:
thenetworkgrinch
2024-01-15 14:37:13 -06:00
parent 3fd8493ccb
commit a71d7a0b4e
185 changed files with 6910 additions and 10987 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
swervelib/SwerveModule.java
View File

@@ -4,6 +4,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.encoders.SwerveAbsoluteEncoder;
@@ -48,15 +49,10 @@ public class SwerveModule
* Last swerve module state applied.
*/
public SwerveModuleState lastState;
/**
* Enable {@link SwerveModuleState} optimizations so the angle is reversed and speed is reversed to ensure the module
* never turns more than 90deg.
*/
public boolean moduleStateOptimization = true;
/**
* Angle offset from the absolute encoder.
*/
private double angleOffset;
private double angleOffset;
/**
* Simulated swerve module.
*/
@@ -114,7 +110,7 @@ public class SwerveModule
// Config angle motor/controller
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle);
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
angleMotor.configurePIDWrapping(-180, 180);
angleMotor.configurePIDWrapping(0, 180);
angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
angleMotor.setMotorBrake(false);
@@ -178,11 +174,7 @@ public class SwerveModule
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
{
if (moduleStateOptimization)
{
desiredState = SwerveModuleState.optimize(desiredState,
Rotation2d.fromDegrees(getAbsolutePosition()));
}
desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
if (isOpenLoop)
{
@@ -190,40 +182,44 @@ public class SwerveModule
driveMotor.set(percentOutput);
} else
{
if (desiredState.speedMetersPerSecond != lastState.speedMetersPerSecond)
// 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 */
double 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)
{
double velocity = desiredState.speedMetersPerSecond;
driveMotor.setReference(velocity, feedforward.calculate(velocity));
cosineScalar = 0.0;
}
double velocity = desiredState.speedMetersPerSecond * (cosineScalar);
driveMotor.setReference(velocity, feedforward.calculate(velocity));
}
/* // Not necessary anymore.
// 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));
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint:", desiredState.speedMetersPerSecond);
SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint:", desiredState.angle.getDegrees());
}
*/
// Prevent module rotation if angle is the same as the previous angle.
if (desiredState.angle != lastState.angle || synchronizeEncoderQueued)
// Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
if (absoluteEncoder != null && synchronizeEncoderQueued)
{
// Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
if (absoluteEncoder != null && synchronizeEncoderQueued)
{
double absoluteEncoderPosition = getAbsolutePosition();
angleMotor.setPosition(absoluteEncoderPosition);
angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
synchronizeEncoderQueued = false;
} else
{
angleMotor.setReference(desiredState.angle.getDegrees(), 0);
}
double absoluteEncoderPosition = getAbsolutePosition();
angleMotor.setPosition(absoluteEncoderPosition);
angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
synchronizeEncoderQueued = false;
} else
{
angleMotor.setReference(desiredState.angle.getDegrees(), 0);
}
lastState = desiredState;
@@ -232,6 +228,12 @@ public class SwerveModule
{
simModule.updateStateAndPosition(desiredState);
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond);
SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees());
}
}
/**
@@ -282,10 +284,6 @@ public class SwerveModule
{
return simModule.getPosition();
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber("Module[" + configuration.name + "] Angle", azimuth.getDegrees());
}
return new SwerveModulePosition(position, azimuth);
}
@@ -434,4 +432,20 @@ public class SwerveModule
return absoluteEncoder.readingError;
}
}
/**
* Update data sent to {@link SmartDashboard}.
*/
public void updateTelemetry()
{
if (absoluteEncoder != null)
{
SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Absolute Encoder",
absoluteEncoder.getAbsolutePosition());
}
SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Motor Encoder", angleMotor.getPosition());
SmartDashboard.putNumber("Module[" + configuration.name + "] Adjusted Absolute Encoder", getAbsolutePosition());
SmartDashboard.putNumber("Module[" + configuration.name + "] Absolute Encoder Read Issue",
getAbsoluteEncoderReadIssue() ? 1 : 0);
}
}