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

Upgrading to 2025.1.0

Browse Source
This commit is contained in:
thenetworkgrinch
2024-12-09 23:26:04 +00:00
parent 9fe6551d88
commit 4bc6978a20
35 changed files with 1902 additions and 1122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
swervelib/imu/Pigeon2Swerve.java
View File

@@ -1,13 +1,17 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.DegreesPerSecond;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
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.LinearAcceleration;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
import java.util.function.Supplier;
/**
* SwerveIMU interface for the {@link Pigeon2}
@@ -36,6 +40,19 @@ public class Pigeon2Swerve extends SwerveIMU
*/
private Pigeon2Configurator cfg;
/**
* X Acceleration supplier
*/
private Supplier<StatusSignal<LinearAcceleration>> xAcc;
/**
* Y Accelleration supplier.
*/
private Supplier<StatusSignal<LinearAcceleration>> yAcc;
/**
* Z Acceleration supplier.
*/
private Supplier<StatusSignal<LinearAcceleration>> zAcc;
/**
* Generate the SwerveIMU for {@link Pigeon2}.
*
@@ -46,6 +63,9 @@ public class Pigeon2Swerve extends SwerveIMU
{
imu = new Pigeon2(canid, canbus);
this.cfg = imu.getConfigurator();
xAcc = imu::getAccelerationX;
yAcc = imu::getAccelerationY;
zAcc = imu::getAccelerationZ;
SmartDashboard.putData(imu);
}
@@ -123,6 +143,7 @@ public class Pigeon2Swerve extends SwerveIMU
return getRawRotation3d().minus(offset);
}
/**
* Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
* empty.
@@ -132,24 +153,19 @@ public class Pigeon2Swerve extends SwerveIMU
@Override
public Optional<Translation3d> getAccel()
{
// TODO: Switch to suppliers.
StatusSignal<Double> xAcc = imu.getAccelerationX();
StatusSignal<Double> yAcc = imu.getAccelerationY();
StatusSignal<Double> zAcc = imu.getAccelerationZ();
// TODO: Implement later.
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));
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}.
* @return Rotation rate in DegreesPerSecond.
*/
public double getRate()
{
return imu.getRate();
return imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(DegreesPerSecond);
}
/**