Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-07-01 07:21:39 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to 2024.4.8.2

Browse Source
This commit is contained in:
thenetworkgrinch
2024-02-07 12:25:00 -06:00
parent 2a78abb56b
commit 78349d6f2d
111 changed files with 249 additions and 287 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
swervelib/imu/Pigeon2Swerve.java
View File

@@ -1,5 +1,6 @@
package swervelib.imu;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.configs.Pigeon2Configurator;
@@ -19,7 +20,7 @@ public class Pigeon2Swerve extends SwerveIMU
/**
* Wait time for status frames to show up.
*/
private final double STATUS_TIMEOUT_SECONDS = 0.02;
public static double STATUS_TIMEOUT_SECONDS = 0.04;
/**
* Pigeon2 IMU device.
*/
@@ -110,10 +111,26 @@ 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.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
x.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
y.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
z.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()));
if(w.getStatus() != StatusCode.OK)
{
w = w.waitForUpdate(STATUS_TIMEOUT_SECONDS);
}
if(x.getStatus() != StatusCode.OK)
{
x = x.waitForUpdate(STATUS_TIMEOUT_SECONDS);
}
if(y.getStatus() != StatusCode.OK)
{
y = y.waitForUpdate(STATUS_TIMEOUT_SECONDS);
}
if(z.getStatus() != StatusCode.OK)
{
z = z.waitForUpdate(STATUS_TIMEOUT_SECONDS);
}
Rotation3d reading = new Rotation3d(new Quaternion(w.getValue(),
x.getValue(),
y.getValue(),
z.getValue()));
return invertedIMU ? reading.unaryMinus() : reading;
}