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

Upgrading to 2025.1.0.1

Browse Source
This commit is contained in:
thenetworkgrinch
2024-12-17 18:49:55 +00:00
parent 4bc6978a20
commit 8050f43fa5
19 changed files with 1189 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
swervelib/imu/Pigeon2Swerve.java
View File

@@ -8,7 +8,9 @@ import com.ctre.phoenix6.configs.Pigeon2Configurator;
import com.ctre.phoenix6.hardware.Pigeon2;
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.LinearAcceleration;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
import java.util.function.Supplier;
@@ -22,23 +24,28 @@ public class Pigeon2Swerve extends SwerveIMU
/**
* Wait time for status frames to show up.
*/
public static double STATUS_TIMEOUT_SECONDS = 0.04;
public static double STATUS_TIMEOUT_SECONDS = 0.04;
/**
* {@link Pigeon2} IMU device.
*/
private final Pigeon2 imu;
private final Pigeon2 imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the {@link Pigeon2}.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* {@link Pigeon2} configurator.
*/
private Pigeon2Configurator cfg;
private Pigeon2Configurator cfg;
/**
* X Acceleration supplier
@@ -158,14 +165,10 @@ public class Pigeon2Swerve extends SwerveIMU
return Optional.empty();
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return Rotation rate in DegreesPerSecond.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(DegreesPerSecond);
return yawVel.mut_replace(imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue());
}
/**