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

62
swervelib/SwerveDrive.java
View File

@@ -100,6 +100,10 @@ public class SwerveDrive
* correction.
*/
public boolean chassisVelocityCorrection = true;
/**
* Whether heading correction PID is currently active.
*/
private boolean correctionEnabled = false;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
@@ -116,6 +120,10 @@ public class SwerveDrive
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
private int moduleSynchronizationCounter = 0;
/**
* Deadband for speeds in heading correction.
*/
private final double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* The last heading set in radians.
*/
@@ -396,27 +404,28 @@ public class SwerveDrive
// https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
if (chassisVelocityCorrection)
{
double dtConstant = 0.009;
Pose2d robotPoseVel = new Pose2d(velocity.vxMetersPerSecond * dtConstant,
velocity.vyMetersPerSecond * dtConstant,
Rotation2d.fromRadians(velocity.omegaRadiansPerSecond * dtConstant));
Twist2d twistVel = SwerveMath.PoseLog(robotPoseVel);
velocity = new ChassisSpeeds(twistVel.dx / dtConstant, twistVel.dy / dtConstant,
twistVel.dtheta / dtConstant);
velocity = ChassisSpeeds.discretize(velocity, 0.02);
}
// Heading Angular Velocity Deadband, might make a configuration option later.
// Originally made by Team 1466 Webb Robotics.
// Modified by Team 7525 Pioneers and BoiledBurntBagel of 6036
if (headingCorrection)
{
if (Math.abs(velocity.omegaRadiansPerSecond) < 0.01)
if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND
&& (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
|| Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
{
if (!correctionEnabled)
{
lastHeadingRadians = getYaw().getRadians();
correctionEnabled = true;
}
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians());
} else
{
lastHeadingRadians = getYaw().getRadians();
correctionEnabled = false;
}
}
@@ -488,15 +497,6 @@ public class SwerveDrive
SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) +
1] = module.lastState.speedMetersPerSecond;
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber(
"Module[" + module.configuration.name + "] Speed Setpoint: ",
module.lastState.speedMetersPerSecond);
SmartDashboard.putNumber(
"Module[" + module.configuration.name + "] Angle Setpoint: ",
module.lastState.angle.getDegrees());
}
}
}
@@ -897,13 +897,8 @@ public class SwerveDrive
sumVelocity += Math.abs(moduleState.speedMetersPerSecond);
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber(
"Module[" + module.configuration.name + "] Relative Encoder", module.getRelativePosition());
SmartDashboard.putNumber(
"Module[" + module.configuration.name + "] Absolute Encoder", module.getAbsolutePosition());
SmartDashboard.putNumber(
"Module[" + module.configuration.name + "] Absolute Encoder Read Issue",
module.getAbsoluteEncoderReadIssue() ? 1 : 0);
module.updateTelemetry();
SmartDashboard.putNumber("Adjusted IMU Yaw", getYaw().getDegrees());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
@@ -1051,7 +1046,7 @@ public class SwerveDrive
* Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in
* autonomous.
*/
public void resetEncoders()
public void resetDriveEncoders()
{
for (SwerveModule module : swerveModules)
{
@@ -1059,19 +1054,6 @@ public class SwerveDrive
}
}
/**
* Configure whether the {@link SwerveModuleState} will be optimized to prevent spinning more than 90deg.
*
* @param optimizationEnabled Whether the module optimization is enabled.
*/
public void setModuleStateOptimization(boolean optimizationEnabled)
{
for (SwerveModule module : swerveModules)
{
module.moduleStateOptimization = optimizationEnabled;
}
}
/**
* Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the
* internal offsets to prevent double offsetting.