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.6.3

Browse Source
This commit is contained in:
thenetworkgrinch
2024-01-26 11:29:15 -06:00
parent bd949c2fa5
commit 75ec1aee24
121 changed files with 1751 additions and 552 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
swervelib/SwerveDrive.java
View File

@@ -98,10 +98,6 @@ public class SwerveDrive
* Deadband for speeds in heading correction.
*/
private double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* Whether heading correction PID is currently active.
*/
private boolean correctionEnabled = false;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
@@ -177,6 +173,7 @@ public class SwerveDrive
Rotation2d.fromDegrees(0))); // x,y,heading in radians; Vision measurement std dev, higher=less weight
zeroGyro();
setMaximumSpeed(maxSpeedMPS);
// Initialize Telemetry
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
@@ -452,16 +449,11 @@ public class SwerveDrive
&& (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
|| Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
{
if (!correctionEnabled)
{
lastHeadingRadians = getOdometryHeading().getRadians();
correctionEnabled = true;
}
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getOdometryHeading().getRadians());
swerveController.headingCalculate(getOdometryHeading().getRadians(), lastHeadingRadians);
} else
{
correctionEnabled = false;
lastHeadingRadians = getOdometryHeading().getRadians();
}
}
@@ -686,6 +678,33 @@ public class SwerveDrive
return positions;
}
/**
* Getter for the {@link SwerveIMU}.
*
* @return generated {@link SwerveIMU}
*/
public SwerveIMU getGyro()
{
return swerveDriveConfiguration.imu;
}
/**
* Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d}
* subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}.
*
* @param gyro expected gyroscope angle as {@link Rotation3d}.
*/
public void setGyro(Rotation3d gyro)
{
if (SwerveDriveTelemetry.isSimulation)
{
setGyroOffset(simIMU.getGyroRotation3d().minus(gyro));
} else
{
setGyroOffset(imu.getRawRotation3d().minus(gyro));
}
}
/**
* Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
*/
@@ -820,6 +839,7 @@ public class SwerveDrive
swerveDriveConfiguration.physicalCharacteristics.optimalVoltage = optimalVoltage;
for (SwerveModule module : swerveModules)
{
module.maxSpeed = maximumSpeed;
if (updateModuleFeedforward)
{
module.feedforward = SwerveMath.createDriveFeedforward(optimalVoltage,
@@ -1030,9 +1050,7 @@ public class SwerveDrive
/**
* Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
* the given timestamp of the vision measurement. <br /> <b>IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET
* AFTER USING THIS FUNCTION!</b> <br /> To update your gyroscope readings directly use
* {@link SwerveDrive#setGyroOffset(Rotation3d)}.
* the given timestamp of the vision measurement.
*
* @param robotPose Robot {@link Pose2d} as measured by vision.
* @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
@@ -1050,24 +1068,6 @@ public class SwerveDrive
// resetOdometry(newOdometry);
}
/**
* Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d}
* subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}.
*
* @param gyro expected gyroscope angle as {@link Rotation3d}.
*/
public void setGyro(Rotation3d gyro)
{
if (SwerveDriveTelemetry.isSimulation)
{
setGyroOffset(simIMU.getGyroRotation3d().minus(gyro));
} else
{
setGyroOffset(imu.getRawRotation3d().minus(gyro));
}
}
/**
* Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} which can be used to
* generate {@link ChassisSpeeds} for the robot to orient it correctly given axis or angles, and apply