From 0064a62555bc348071198a07c824f3bf890afdaa Mon Sep 17 00:00:00 2001 From: GZod01 Date: Sun, 7 Jan 2024 16:37:00 +0100 Subject: [PATCH] feat:ftc2024-carlike.java --- ftc2024-carlike.java | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/ftc2024-carlike.java b/ftc2024-carlike.java index 7cb418d..91ea86a 100644 --- a/ftc2024-carlike.java +++ b/ftc2024-carlike.java @@ -57,18 +57,18 @@ public class Werobot_FTC2024_carlike extends LinearOpMode { ) ); rm.setDirection(DcMotorSimple.Direction.REVERSE); - telemetry.addData("Mode", "calibrating..."); - telemetry.update(); + //telemetry.addData("Mode", "calibrating..."); + //telemetry.update(); // make sure the imu gyro is calibrated before continuing. - while (!isStopRequested() && !imu.isGyroCalibrated()) - { - sleep(50); - idle(); - } + //while (!isStopRequested() && !imu.isGyroCalibrated()) + //{ + // sleep(50); + // idle(); + //} telemetry.addData("Mode", "waiting for start"); - telemetry.addData("imu calib status", imu.getCalibrationStatus().toString()); + //telemetry.addData("imu calib status", imu.getCalibrationStatus().toString()); telemetry.update(); waitForStart(); @@ -146,7 +146,7 @@ public class Werobot_FTC2024_carlike extends LinearOpMode { } private void resetAngle() { - lastAngles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + lastAngles = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); globalAngle = 0; } @@ -162,7 +162,7 @@ public class Werobot_FTC2024_carlike extends LinearOpMode { // returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes // 180 degrees. We detect this transition and track the total cumulative angle of rotation. - Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + Orientation angles = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); double deltaAngle = angles.firstAngle - lastAngles.firstAngle;