diff --git a/ftc2024-holonom.java b/ftc2024-holonom.java
index 2f10e0e..82f6a06 100644
--- a/ftc2024-holonom.java
+++ b/ftc2024-holonom.java
@@ -18,6 +18,7 @@ public class Werobot_FTC2024 extends LinearOpMode {
         private DcMotor frm;
         private DcMotor brm;
         private DcMotor blm;
+	private IMU imu;
         @Override
         public void runOpMode() throws InterruptedException {
                 float x;
@@ -37,11 +38,29 @@ public class Werobot_FTC2024 extends LinearOpMode {
                 frm = hardwareMap.get(DcMotor.class, "frm");
                 blm = hardwareMap.get(DcMotor.class, "blm");
                 brm = hardwareMap.get(DcMotor.class, "brm");
+		imu = hardwareMap.get(IMU.class, "imu");
+		imu.initialize(
+			new IMU.Parameters(
+				new RevHubOrientationOnRobot(
+					RevHubOrientationOnRobot.LogoFacingDirection.UP,
+					RevHubOrientationOnRobot.UsbFacingDirection.FORWARD
+				)
+			)
+	        );
+		imu.resetYaw();
                 waitForStart();
                 
                 
                 while (opModeIsActive()) {
                         telemetry.addData("Status", "Running");
+			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.RADIAN);
+			double Pitch = robotOrientation.getPitch(AngleUnit.RADIAN);
+			double Roll  = robotOrientation.getRoll(AngleUnit.RADIAN);
                         double[] motors_values = new double[4];
                         telemetry.addData("left_stick_x", gamepad1.left_stick_x);
                         telemetry.addData("left_stick_y", gamepad1.left_stick_y);
@@ -61,6 +80,7 @@ public class Werobot_FTC2024 extends LinearOpMode {
                                 double joystick_angle = Math.atan2(joystick_vector[0],joystick_vector[1]);
                                 double cur_motor_angle = Math.atan2(cur_motor[0],cur_motor[1]);
                                 double diff_angle = joystick_angle - cur_motor_angle;
+				diff_angle+=Yaw;
                                 motors_values[i] =  (cur_motor_norm*joystick_norm*Math.cos(diff_angle));
                                 motors_values[i] = (motors_values[i]+gamepad1.right_stick_x)/(Math.abs(gamepad1.right_stick_x)+1);
                                 motors_values[i]= motors_values[i]/Math.sqrt(2);