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

Update to 2024.4.8

Browse Source
This commit is contained in:
thenetworkgrinch
2024-02-02 18:55:29 -06:00
parent 2dcfaac857
commit c478a800b2
118 changed files with 2009 additions and 607 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
swervelib/imu/Pigeon2Swerve.java
View File

@@ -16,6 +16,10 @@ import java.util.Optional;
public class Pigeon2Swerve extends SwerveIMU
{
/**
* Wait time for status frames to show up.
*/
private final double STATUS_TIMEOUT_SECONDS = 0.02;
/**
* Pigeon2 IMU device.
*/
@@ -23,11 +27,11 @@ public class Pigeon2Swerve extends SwerveIMU
/**
* Offset for the Pigeon 2.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* Generate the SwerveIMU for pigeon.
@@ -106,10 +110,10 @@ public class Pigeon2Swerve extends SwerveIMU
StatusSignal<Double> x = imu.getQuatX();
StatusSignal<Double> y = imu.getQuatY();
StatusSignal<Double> z = imu.getQuatZ();
Rotation3d reading = new Rotation3d(new Quaternion(w.refresh().getValue(),
x.refresh().getValue(),
y.refresh().getValue(),
z.refresh().getValue()));
Rotation3d reading = new Rotation3d(new Quaternion(w.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
x.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
y.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
z.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()));
return invertedIMU ? reading.unaryMinus() : reading;
}
@@ -138,9 +142,9 @@ public class Pigeon2Swerve extends SwerveIMU
StatusSignal<Double> yAcc = imu.getAccelerationY();
StatusSignal<Double> zAcc = imu.getAccelerationZ();
return Optional.of(new Translation3d(xAcc.refresh().getValue(),
yAcc.refresh().getValue(),
zAcc.refresh().getValue()).times(9.81 / 16384.0));
return Optional.of(new Translation3d(xAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
yAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
zAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()).times(9.81 / 16384.0));
}
/**