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

Updated to include gyro and accelerometer, and Rotation3d offset.

Browse Source
This commit is contained in:
thenetworkgrinch
2023-03-06 20:45:54 -06:00
parent 9b78c6363f
commit 5fd00878d2
120 changed files with 994 additions and 312 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
swervelib/imu/ADIS16448Swerve.java
View File

@@ -1,7 +1,10 @@
package swervelib.imu;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.ADIS16448_IMU;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
/**
* IMU Swerve class for the {@link ADIS16448_IMU} device.
@@ -70,6 +73,31 @@ public class ADIS16448Swerve extends SwerveIMU
yprArray[2] = imu.getYComplementaryAngle() % 360;
}
/**
* Fetch the {@link Rotation3d} from the IMU. Robot relative.
*
* @return {@link Rotation3d} from the IMU.
*/
public Rotation3d getRotation3d()
{
return new Rotation3d(
imu.getYComplementaryAngle(), imu.getXComplementaryAngle(), imu.getAngle())
.minus(new Rotation3d(0, 0, Math.toRadians(yawOffset)));
}
/**
* Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
* empty.
*
* @return {@link Translation3d} of the acceleration.
*/
@Override
public Optional<Translation3d> getAccel()
{
return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()));
}
/**
* Get the instantiated IMU object.
*