Skip to content

Commit

Permalink
Upgrading to 2025.1.0.1
Browse files Browse the repository at this point in the history
  • Loading branch information
@thenetworkgrinch
thenetworkgrinch committed Dec 17, 2024
1 parent 4bc6978 commit 8050f43
Show file tree
Hide file tree
Showing 19 changed files with 1,189 additions and 263 deletions.
80 changes: 48 additions & 32 deletions swervelib/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Newtons;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;

Expand All @@ -31,6 +32,8 @@
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Force;
import edu.wpi.first.units.measure.LinearVelocity;
Expand All @@ -55,7 +58,6 @@
import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
import swervelib.encoders.CANCoderSwerve;
import swervelib.imu.IMUVelocity;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveMath;
Expand Down Expand Up @@ -100,18 +102,34 @@ public class SwerveDrive
/**
* Odometry lock to ensure thread safety.
*/
private final Lock odometryLock = new ReentrantLock();
private final Lock odometryLock = new ReentrantLock();
/**
* 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.kWarning);
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.kWarning);
/**
* NT4 Publisher for the IMU reading.
*/
private final DoublePublisher rawIMUPublisher
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/Raw IMU Yaw")
.publish();
/**
* NT4 Publisher for the IMU reading adjusted by offset and inversion.
*/
private final DoublePublisher adjustedIMUPublisher
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/Adjusted IMU Yaw")
.publish();
/**
* Field object.
*/
public Field2d field = new Field2d();
public Field2d field = new Field2d();
/**
* Swerve controller for controlling heading of the robot.
*/
Expand All @@ -120,75 +138,70 @@ public class SwerveDrive
* Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's
* correction.
*/
public boolean chassisVelocityCorrection = true;
public boolean chassisVelocityCorrection = true;
/**
* Correct chassis velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} (auto) using 254's
* correction during auto.
*/
public boolean autonomousChassisVelocityCorrection = false;
public boolean autonomousChassisVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
*/
public boolean angularVelocityCorrection = false;
public boolean angularVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto.
*/
public boolean autonomousAngularVelocityCorrection = false;
public boolean autonomousAngularVelocityCorrection = false;
/**
* Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).
*/
public double angularVelocityCoefficient = 0;
public double angularVelocityCoefficient = 0;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
public boolean headingCorrection = false;
/**
* MapleSim SwerveDrive.
*/
private SwerveDriveSimulation mapleSimDrive;
/**
* Amount of seconds the duration of the timestep the speeds should be applied for.
*/
private double discretizationdtSeconds = 0.02;
private double discretizationdtSeconds = 0.02;
/**
* Deadband for speeds in heading correction.
*/
private double HEADING_CORRECTION_DEADBAND = 0.01;
private double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
private SwerveIMU imu;
/**
* Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}.
*/
private IMUVelocity imuVelocity;
/**
* Simulation of the swerve drive.
*/
private SwerveIMUSimulation simIMU;
private SwerveIMUSimulation simIMU;
/**
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
private int moduleSynchronizationCounter = 0;
private int moduleSynchronizationCounter = 0;
/**
* The last heading set in radians.
*/
private double lastHeadingRadians = 0;
private double lastHeadingRadians = 0;
/**
* The absolute max speed that your robot can reach while translating in meters per second.
*/
private double attainableMaxTranslationalSpeedMetersPerSecond = 0;
private double attainableMaxTranslationalSpeedMetersPerSecond = 0;
/**
* The absolute max speed the robot can reach while rotating radians per second.
*/
private double attainableMaxRotationalVelocityRadiansPerSecond = 0;
private double attainableMaxRotationalVelocityRadiansPerSecond = 0;
/**
* Maximum speed of the robot in meters per second.
*/
private double maxChassisSpeedMPS;
private double maxChassisSpeedMPS;

/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
Expand Down Expand Up @@ -567,7 +580,7 @@ public void drive(
*/
public void drive(ChassisSpeeds robotRelativeVelocity, boolean isOpenLoop, Translation2d centerOfRotationMeters)
{

SwerveDriveTelemetry.startCtrlCycle();
robotRelativeVelocity = movementOptimizations(robotRelativeVelocity,
chassisVelocityCorrection,
angularVelocityCorrection);
Expand Down Expand Up @@ -702,6 +715,7 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds
*/
public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
{
SwerveDriveTelemetry.startCtrlCycle();
double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond);
desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates));
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxModuleSpeedMPS);
Expand All @@ -725,6 +739,7 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo
*/
public void drive(ChassisSpeeds robotRelativeVelocity, SwerveModuleState[] states, Force[] feedforwardForces)
{
SwerveDriveTelemetry.startCtrlCycle();
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
{
SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeVelocity;
Expand Down Expand Up @@ -763,7 +778,7 @@ public void drive(ChassisSpeeds robotRelativeVelocity, SwerveModuleState[] state
*/
public void setChassisSpeeds(ChassisSpeeds robotRelativeSpeeds)
{

SwerveDriveTelemetry.startCtrlCycle();
robotRelativeSpeeds = movementOptimizations(robotRelativeSpeeds,
autonomousChassisVelocityCorrection,
autonomousAngularVelocityCorrection);
Expand Down Expand Up @@ -1108,6 +1123,7 @@ public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward driveFeedforwa
*/
public void updateOdometry()
{
SwerveDriveTelemetry.startOdomCycle();
odometryLock.lock();
invalidateCache();
try
Expand Down Expand Up @@ -1155,8 +1171,8 @@ public void updateOdometry()
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
module.updateTelemetry();
SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees());
SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees());
rawIMUPublisher.set(getYaw().getDegrees());
adjustedIMUPublisher.set(getOdometryHeading().getDegrees());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
Expand All @@ -1183,6 +1199,7 @@ public void updateOdometry()
throw e;
}
odometryLock.unlock();
SwerveDriveTelemetry.endOdomCycle();
}

/**
Expand Down Expand Up @@ -1442,7 +1459,6 @@ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAut
{
if (!SwerveDriveTelemetry.isSimulation)
{
imuVelocity = IMUVelocity.createIMUVelocity(imu);
angularVelocityCorrection = useInTeleop;
autonomousAngularVelocityCorrection = useInAuto;
angularVelocityCoefficient = angularVelocityCoeff;
Expand All @@ -1457,7 +1473,7 @@ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAut
*/
public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds robotRelativeVelocity)
{
var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
var angularVelocity = new Rotation2d(imu.getYawAngularVelocity().in(RadiansPerSecond) * angularVelocityCoefficient);
if (angularVelocity.getRadians() != 0.0)
{
robotRelativeVelocity.toFieldRelativeSpeeds(getOdometryHeading());
Expand Down
Loading

0 comments on commit 8050f43

Please sign in to comment.