diff --git a/ftc2024-carlike.java b/ftc2024-carlike.java index 91ea86a..7b158d1 100644 --- a/ftc2024-carlike.java +++ b/ftc2024-carlike.java @@ -27,8 +27,6 @@ public class Werobot_FTC2024_carlike extends LinearOpMode { private DcMotor rm; private DcMotor lm; private IMU imu; - private Orientation lastAngles = new Orientation(); - private double globalAngle, power = .30, correction; private double helloexp(double t){ return (Math.exp(5*t)-1)/(Math.exp(5)-1); } @@ -135,69 +133,18 @@ public class Werobot_FTC2024_carlike extends LinearOpMode { telemetry.addData("ltrigg",t); telemetry.addData("t2",t2); telemetry.addData("mode",mode); - // Use gyro to drive in a straight line. - correction = checkDirection(); + // Create an object to receive the IMU angles + YawPitchRollAngles robotOrientation; + robotOrientation = imu.getRobotYawPitchRollAngles(); + + // Now use these simple methods to extract each angle + // (Java type double) from the object you just created: + double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES); + double Roll = robotOrientation.getRoll(AngleUnit.DEGREES); + telemetry.addData("yaw",Yaw); - telemetry.addData("1 imu heading", lastAngles.firstAngle); - telemetry.addData("2 global heading", globalAngle); - telemetry.addData("3 correction", correction); telemetry.update(); } } - private void resetAngle() - { - lastAngles = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); - - globalAngle = 0; - } - - /** - * Get current cumulative angle rotation from last reset. - * @return Angle in degrees. + = left, - = right. - */ - private double getAngle() - { - // We experimentally determined the Z axis is the axis we want to use for heading angle. - // We have to process the angle because the imu works in euler angles so the Z axis is - // 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.getRobotOrientation(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; - } - - /** - * See if we are moving in a straight line and if not return a power correction value. - * @return Power adjustment, + is adjust left - is adjust right. - */ - private double checkDirection() - { - // The gain value determines how sensitive the correction is to direction changes. - // You will have to experiment with your robot to get small smooth direction changes - // to stay on a straight line. - double correction, angle, gain = .10; - - angle = getAngle(); - - if (angle == 0) - correction = 0; // no adjustment. - else - correction = -angle; // reverse sign of angle for correction. - - correction = correction * gain; - - return correction; - } }