Skip to content

Commit

Permalink
Removed IMUVelocity in favor of native functions. Added `MutAngular…
Browse files Browse the repository at this point in the history 
…Velocity` to the return type of SwerveIMU.getYawAngularVelocity. Renamed SwerveIMU.getRate to SwerveIMU.getYawAngularVelocity. Formatted code.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
  • Loading branch information
@thenetworkgrinch
thenetworkgrinch committed Dec 13, 2024
1 parent 6eec94b commit 0a3ff7a
Show file tree
Hide file tree
Showing 13 changed files with 247 additions and 421 deletions.
82 changes: 38 additions & 44 deletions src/main/java/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 Down Expand Up @@ -57,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 @@ -102,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 @@ -122,91 +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;
/**
* 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();
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 @@ -1464,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 @@ -1479,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
25 changes: 15 additions & 10 deletions src/main/java/swervelib/imu/ADIS16448Swerve.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,11 @@
package swervelib.imu;

import static edu.wpi.first.units.Units.DegreesPerSecond;

import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.ADIS16448_IMU;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
Expand All @@ -15,15 +19,19 @@ public class ADIS16448Swerve extends SwerveIMU
/**
* {@link ADIS16448_IMU} device to read the current headings from.
*/
private final ADIS16448_IMU imu;
private final ADIS16448_IMU imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the ADIS16448.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;

/**
* Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the SmartDashboard.
Expand Down Expand Up @@ -110,14 +118,11 @@ public Optional<Translation3d> getAccel()
return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()));
}

/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();

return yawVel.mut_setMagnitude(imu.getRate());
}

/**
Expand Down
24 changes: 14 additions & 10 deletions src/main/java/swervelib/imu/ADIS16470Swerve.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,11 @@
package swervelib.imu;

import static edu.wpi.first.units.Units.DegreesPerSecond;

import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
Expand All @@ -16,15 +20,19 @@ public class ADIS16470Swerve extends SwerveIMU
/**
* {@link ADIS16470_IMU} device to read the current headings from.
*/
private final ADIS16470_IMU imu;
private final ADIS16470_IMU imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the ADIS16470.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;

/**
* Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the SmartDashboard.
Expand Down Expand Up @@ -110,14 +118,10 @@ public Optional<Translation3d> getAccel()
return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()));
}

/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();
return yawVel.mut_setMagnitude(imu.getRate());
}

/**
Expand Down
24 changes: 14 additions & 10 deletions src/main/java/swervelib/imu/ADXRS450Swerve.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,11 @@
package swervelib.imu;

import static edu.wpi.first.units.Units.DegreesPerSecond;

import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
Expand All @@ -15,15 +19,19 @@ public class ADXRS450Swerve extends SwerveIMU
/**
* {@link ADXRS450_Gyro} device to read the current headings from.
*/
private final ADXRS450_Gyro imu;
private final ADXRS450_Gyro imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the ADXRS450.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;

/**
* Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the SmartDashboard.
Expand Down Expand Up @@ -108,14 +116,10 @@ public Optional<Translation3d> getAccel()
return Optional.empty();
}

/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();
return yawVel.mut_setMagnitude(imu.getRate());
}

/**
Expand Down
24 changes: 14 additions & 10 deletions src/main/java/swervelib/imu/AnalogGyroSwerve.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,11 @@
package swervelib.imu;

import static edu.wpi.first.units.Units.DegreesPerSecond;

import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
Expand All @@ -15,15 +19,19 @@ public class AnalogGyroSwerve extends SwerveIMU
/**
* Gyroscope object.
*/
private final AnalogGyro imu;
private final AnalogGyro imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the analog gyro.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;

/**
* Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1.
Expand Down Expand Up @@ -115,14 +123,10 @@ public Optional<Translation3d> getAccel()
return Optional.empty();
}

/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();
return yawVel.mut_setMagnitude(imu.getRate());
}

/**
Expand Down
Loading

0 comments on commit 0a3ff7a

Please sign in to comment.