Updated to 2024.4.6.1

This commit is contained in:
thenetworkgrinch
2024-01-22 15:11:18 -06:00
parent 7c76cffc78
commit a20a6b83af
120 changed files with 923 additions and 603 deletions

View File

@@ -19,7 +19,11 @@ public class AnalogGyroSwerve extends SwerveIMU
/**
* Offset for the analog gyro.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1.
@@ -67,6 +71,16 @@ public class AnalogGyroSwerve extends SwerveIMU
this.offset = offset;
}
/**
* Set the gyro to invert its default direction
*
* @param invertIMU invert gyro direction
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
}
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
@@ -74,7 +88,8 @@ public class AnalogGyroSwerve extends SwerveIMU
*/
public Rotation3d getRawRotation3d()
{
return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle()));
Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle()));
return invertedIMU ? reading.unaryMinus() : reading;
}
/**