This code allows us to see where our robot is drifting and recorrect it with some loops.
// Gyro Sensor Calibration
BNO055IMU imu;
BNO055IMU.Parameters parameters;
Orientation lastAngles = new Orientation();
double globalAngle;
// Gyro Sensor Functions
public void imuinit() {
imu = hardwareMap.get(BNO055IMU.class, "imu");
parameters = new BNO055IMU.Parameters();
parameters.mode = BNO055IMU.SensorMode.IMU;
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = false;
imu.initialize(parameters);
telemetry.addData("Gyro Mode", "calibrating...");
telemetry.update();
while (!isStopRequested() && !imu.isGyroCalibrated()) {
sleep(50);
idle();
}
telemetry.addData("Gyro Mode", "ready");
telemetry.addData("imu calibration status", imu.getCalibrationStatus().toString());
telemetry.update();
}
private void resetAngle() {
lastAngles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
globalAngle = 0;
}
private double getAngle() {
Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
double deltaAngle = angles.firstAngle - lastAngles.firstAngle;
if (deltaAngle < -180)
deltaAngle += 360;
else if (deltaAngle > 180)
deltaAngle -= 360;
globalAngle += deltaAngle;
lastAngles = angles;
return globalAngle;
}