Skip to content

Instantly share code, notes, and snippets.

@FROGbots-4634 FROGbots-4634/IMUtest.java Secret
Created Nov 11, 2017

Embed
What would you like to do?
package org.firstinspires.ftc.teamcode;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
import java.util.Locale;
@TeleOp(name = "IMUtest", group = "Sensor")
//@Disabled
public class IMUtest extends LinearOpMode
{
private BNO055IMU imu;
private Orientation angles;
private Acceleration gravity;
@Override
public void runOpMode()
{
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = true;
parameters.useExternalCrystal = true;
parameters.mode = BNO055IMU.SensorMode.IMU;
parameters.loggingTag = "IMU";
imu = hardwareMap.get(BNO055IMU.class, "imu");
imu.initialize(parameters);
telemetry.setMsTransmissionInterval(100);
waitForStart();
while (opModeIsActive())
{
angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
gravity = imu.getGravity();
sendTelemetry();
}
}
void sendTelemetry()
{
telemetry.addData("Status", imu.getSystemStatus().toString());
telemetry.addData("Calib", imu.getCalibrationStatus().toString());
telemetry.addData("Heading", formatAngle(angles.angleUnit, angles.firstAngle));
telemetry.addData("Roll", formatAngle(angles.angleUnit, angles.secondAngle));
telemetry.addData("Pitch", formatAngle(angles.angleUnit, angles.thirdAngle));
telemetry.addData("Grav", gravity.toString());
telemetry.update();
}
String formatAngle(AngleUnit angleUnit, double angle)
{
return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
}
String formatDegrees(double degrees)
{
return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.