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 IMU

Browse Source
This commit is contained in:
thenetworkgrinch
2023-03-08 23:34:33 -06:00
parent ca03b2122f
commit ec958aecaa
117 changed files with 678 additions and 635 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
swervelib/imu/Pigeon2Swerve.java
View File

@@ -18,6 +18,10 @@ public class Pigeon2Swerve extends SwerveIMU
* Pigeon2 IMU device.
*/
WPI_Pigeon2 imu;
/**
* Offset for the Pigeon 2.
*/
private Rotation3d offset = new Rotation3d();
/**
* Generate the SwerveIMU for pigeon.
@@ -62,25 +66,26 @@ public class Pigeon2Swerve extends SwerveIMU
}
/**
* Set the yaw in degrees.
* Set the gyro offset.
*
* @param yaw Angle in degrees.
* @param offset gyro offset as a {@link Rotation3d}.
*/
@Override
public void setYaw(double yaw)
public void setOffset(Rotation3d offset)
{
imu.setYaw(yaw);
offset = getRotation3d();
}
/**
* Fetch the yaw/pitch/roll from the IMU, inverts them all if SwerveIMU is inverted.
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
* @param yprArray Array which will be filled with {yaw, pitch, roll} in degrees.
* @return {@link Rotation3d} from the IMU.
*/
@Override
public void getYawPitchRoll(double[] yprArray)
public Rotation3d getRawRotation3d()
{
imu.getYawPitchRoll(yprArray);
double[] wxyz = new double[4];
imu.get6dQuaternion(wxyz);
return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
}
/**
@@ -91,9 +96,7 @@ public class Pigeon2Swerve extends SwerveIMU
@Override
public Rotation3d getRotation3d()
{
double[] wxyz = new double[4];
imu.get6dQuaternion(wxyz);
return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
return getRawRotation3d().minus(offset);
}
/**