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

Browse Source
This commit is contained in:
thenetworkgrinch
2024-01-22 15:11:18 -06:00
parent 7c76cffc78
commit a20a6b83af
120 changed files with 923 additions and 603 deletions
Download Patch File Download Diff File Expand all files Collapse all files

147
swervelib/SwerveDrive.java
View File

@@ -1,7 +1,6 @@
package swervelib;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.filter.SlewRateLimiter;
@@ -72,9 +71,12 @@ public class SwerveDrive
*/
private final Lock odometryLock = new ReentrantLock();
/**
* Deadband for speeds in heading correction.
* Alert to recommend Tuner X if the configuration is compatible.
*/
private double HEADING_CORRECTION_DEADBAND = 0.01;
private final Alert tunerXRecommendation = new Alert("Swerve Drive",
"Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
"https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
AlertType.WARNING);
/**
* Field object.
*/
@@ -83,26 +85,6 @@ public class SwerveDrive
* Swerve controller for controlling heading of the robot.
*/
public SwerveController swerveController;
/**
* Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of
* rotation)
*/
public Matrix<N3, N1> stateStdDevs = VecBuilder.fill(0.1,
0.1,
0.1);
/**
* The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more
* points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)}
* with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get
* vision accurate to inches instead of feet.
*/
public Matrix<N3, N1> visionMeasurementStdDevs = VecBuilder.fill(0.9,
0.9,
0.9);
/**
* Invert odometry readings of drive motor positions, used as a patch for debugging currently.
*/
public boolean invertOdometry = false;
/**
* Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's
* correction.
@@ -112,6 +94,10 @@ public class SwerveDrive
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
/**
* Deadband for speeds in heading correction.
*/
private double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* Whether heading correction PID is currently active.
*/
@@ -144,13 +130,6 @@ public class SwerveDrive
* Maximum speed of the robot in meters per second.
*/
private double maxSpeedMPS;
/**
* Alert to recommend Tuner X if the configuration is compatible.
*/
private final Alert tunerXRecommendation = new Alert("Swerve Drive",
"Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
"https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
AlertType.WARNING);
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -194,9 +173,8 @@ public class SwerveDrive
kinematics,
getYaw(),
getModulePositions(),
new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)),
stateStdDevs,
visionMeasurementStdDevs); // x,y,heading in radians; Vision measurement std dev, higher=less weight
new Pose2d(new Translation2d(0, 0),
Rotation2d.fromDegrees(0))); // x,y,heading in radians; Vision measurement std dev, higher=less weight
zeroGyro();
@@ -308,6 +286,16 @@ public class SwerveDrive
}
}
/**
* Fetch the latest odometry heading, should be trusted over {@link SwerveDrive#getYaw()}.
*
* @return {@link Rotation2d} of the robot heading.
*/
public Rotation2d getOdometryHeading()
{
return swerveDrivePoseEstimator.getEstimatedPosition().getRotation();
}
/**
* Set the heading correction capabilities of YAGSL.
*
@@ -337,7 +325,7 @@ public class SwerveDrive
*/
public void driveFieldOriented(ChassisSpeeds velocity)
{
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw());
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
drive(fieldOrientedVelocity);
}
@@ -349,7 +337,7 @@ public class SwerveDrive
*/
public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters)
{
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw());
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
drive(fieldOrientedVelocity, centerOfRotationMeters);
}
@@ -401,7 +389,7 @@ public class SwerveDrive
ChassisSpeeds velocity =
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
translation.getX(), translation.getY(), rotation, getYaw())
translation.getX(), translation.getY(), rotation, getOdometryHeading())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
drive(velocity, isOpenLoop, centerOfRotationMeters);
@@ -430,7 +418,7 @@ public class SwerveDrive
ChassisSpeeds velocity =
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
translation.getX(), translation.getY(), rotation, getYaw())
translation.getX(), translation.getY(), rotation, getOdometryHeading())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
drive(velocity, isOpenLoop, new Translation2d());
@@ -466,11 +454,11 @@ public class SwerveDrive
{
if (!correctionEnabled)
{
lastHeadingRadians = getYaw().getRadians();
lastHeadingRadians = getOdometryHeading().getRadians();
correctionEnabled = true;
}
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians());
swerveController.headingCalculate(lastHeadingRadians, getOdometryHeading().getRadians());
} else
{
correctionEnabled = false;
@@ -623,7 +611,7 @@ public class SwerveDrive
// angle
// given as the robot angle reverses the direction of rotation, and the conversion is reversed.
return ChassisSpeeds.fromFieldRelativeSpeeds(
kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus());
kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus());
}
/**
@@ -646,7 +634,7 @@ public class SwerveDrive
public void resetOdometry(Pose2d pose)
{
odometryLock.lock();
swerveDrivePoseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose);
swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
odometryLock.unlock();
kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation()));
}
@@ -680,8 +668,7 @@ public class SwerveDrive
}
/**
* Gets the current module positions (azimuth and wheel position (meters)). Inverts the distance from each module if
* {@link #invertOdometry} is true.
* Gets the current module positions (azimuth and wheel position (meters)).
*
* @return A list of SwerveModulePositions containg the current module positions
*/
@@ -692,10 +679,6 @@ public class SwerveDrive
for (SwerveModule module : swerveModules)
{
positions[module.moduleNumber] = module.getPosition();
if (invertOdometry)
{
positions[module.moduleNumber].distanceMeters *= -1;
}
}
return positions;
}
@@ -729,9 +712,7 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
return swerveDriveConfiguration.invertedIMU
? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getZ())
: Rotation2d.fromRadians(imu.getRotation3d().getZ());
return Rotation2d.fromRadians(imu.getRotation3d().getZ());
} else
{
return simIMU.getYaw();
@@ -748,9 +729,7 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
return swerveDriveConfiguration.invertedIMU
? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getY())
: Rotation2d.fromRadians(imu.getRotation3d().getY());
return Rotation2d.fromRadians(imu.getRotation3d().getY());
} else
{
return simIMU.getPitch();
@@ -767,9 +746,7 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
return swerveDriveConfiguration.invertedIMU
? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getX())
: Rotation2d.fromRadians(imu.getRotation3d().getX());
return Rotation2d.fromRadians(imu.getRotation3d().getX());
} else
{
return simIMU.getRoll();
@@ -786,9 +763,7 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
return swerveDriveConfiguration.invertedIMU
? imu.getRotation3d().unaryMinus()
: imu.getRotation3d();
return imu.getRotation3d();
} else
{
return simIMU.getGyroRotation3d();
@@ -953,7 +928,7 @@ public class SwerveDrive
SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
SwerveDriveTelemetry.robotRotation = getYaw().getDegrees();
SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees();
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
@@ -969,7 +944,8 @@ public class SwerveDrive
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
module.updateTelemetry();
SmartDashboard.putNumber("Adjusted IMU Yaw", getYaw().getDegrees());
SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees());
SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
@@ -1027,28 +1003,6 @@ 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)}.
*
* @param robotPose Robot {@link Pose2d} as measured by vision.
* @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
* {@link Timer#getFPGATimestamp()} or similar sources.
*/
public void addVisionMeasurement(Pose2d robotPose, double timestamp)
{
odometryLock.lock();
swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp);
Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(),
robotPose.getRotation());
odometryLock.unlock();
setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
resetOdometry(newOdometry);
}
/**
* Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
* the given timestamp of the vision measurement.
@@ -1066,8 +1020,31 @@ public class SwerveDrive
public void addVisionMeasurement(Pose2d robotPose, double timestamp,
Matrix<N3, N1> visionMeasurementStdDevs)
{
this.visionMeasurementStdDevs = visionMeasurementStdDevs;
addVisionMeasurement(robotPose, timestamp);
odometryLock.lock();
swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp, visionMeasurementStdDevs);
odometryLock.unlock();
}
/**
* 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)}.
*
* @param robotPose Robot {@link Pose2d} as measured by vision.
* @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
* {@link Timer#getFPGATimestamp()} or similar sources.
*/
public void addVisionMeasurement(Pose2d robotPose, double timestamp)
{
odometryLock.lock();
swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp);
// Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(),
// robotPose.getRotation());
odometryLock.unlock();
// setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
// resetOdometry(newOdometry);
}