diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h
index b97c2c7..f22a356 100644
--- a/frtos/config/motor_config.h
+++ b/frtos/config/motor_config.h
@@ -20,6 +20,8 @@
 
 #define DIRECTION_FORWARD   (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD)
 #define DIRECTION_BACKWARD  (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD)
+#define DIRECTION_LEFT      (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_FORWARD)
+#define DIRECTION_RIGHT     (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD)
 
 #define DIRECTION_MASK  (DIRECTION_FORWARD | DIRECTION_BACKWARD)
 
diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h
index 9464cb8..57f6463 100644
--- a/frtos/motor/motor_direction.h
+++ b/frtos/motor/motor_direction.h
@@ -4,8 +4,12 @@
  * @author Richie
  */
 
+#ifndef MOTOR_DIRECTION_H
+#define MOTOR_DIRECTION_H
+
 #include "motor_init.h"
 #include "magnetometer_init.h"
+#include "magnetometer_direction.h"
 
 /*!
  * @brief Set the direction of the wheels; can use bitwise OR to set both
@@ -124,36 +128,44 @@ turn_right_90()
 //    g_motor_left.speed.distance_cm  = initial;
 //}
 
-//void
-//spin_to_yaw(float target_yaw)
-//{
-//    float initial_yaw = g_direction.yaw;
-//
-//    // if it will to turn more than 180 degrees, turn the other way
-//    if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f)
-//        || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f)))
-//    {
-//        set_wheel_direction(DIRECTION_LEFT);
-//    }
-//    else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f)
-//             || ((target_yaw < initial_yaw)
-//                 && (initial_yaw - target_yaw < 180.f)))
-//    {
-//        set_wheel_direction(DIRECTION_RIGHT);
-//    }
-//
-//    set_wheel_speed_synced(90u);
-//
-//    g_use_pid = false;
-//
-//    for (;;)
-//    {
-//        if (initial_yaw == target_yaw)
+void
+spin_to_yaw(float target_yaw)
+{
+    float initial_yaw = g_direction.yaw;
+
+    // if it will to turn more than 180 degrees, turn the other way
+    if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f)
+        || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f)))
+    {
+        set_wheel_direction(DIRECTION_LEFT);
+    }
+    else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f)
+             || ((target_yaw < initial_yaw)
+                 && (initial_yaw - target_yaw < 180.f)))
+    {
+        set_wheel_direction(DIRECTION_RIGHT);
+    }
+
+    set_wheel_speed_synced(50u);
+
+    g_use_pid = false;
+
+    for (;;)
+    {
+//        if (xSemaphoreTake(g_direction_sem, portMAX_DELAY) == pdTRUE)
 //        {
-//            set_wheel_speed_synced(0u);
-//            break;
+//            updateDirection();
 //        }
-//        vTaskDelay(pdMS_TO_TICKS(5));
-//    }
-//    g_use_pid = true;
-//}
\ No newline at end of file
+        updateDirection();
+        print_orientation_data();
+        if (initial_yaw == target_yaw)
+        {
+            set_wheel_speed_synced(0u);
+            break;
+        }
+        vTaskDelay(pdMS_TO_TICKS(5));
+    }
+    g_use_pid = true;
+}
+
+#endif /* MOTOR_DIRECTION_H */
\ No newline at end of file
diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h
index cc7ce33..706b27f 100644
--- a/frtos/motor/motor_pid.h
+++ b/frtos/motor/motor_pid.h
@@ -5,6 +5,9 @@
  * @author Richie
  */
 
+#ifndef MOTOR_PID_H
+#define MOTOR_PID_H
+
 // #include "magnetometer_init.h"
 
 /*!
@@ -113,4 +116,6 @@ repeating_i_handler(__unused struct repeating_timer *t)
     set_wheel_speed((uint32_t)temp, &g_motor_right);
 
     return true;
-}
\ No newline at end of file
+}
+
+#endif /* MOTOR_PID_H */
\ No newline at end of file
diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h
index 6fc1c58..be0072e 100644
--- a/frtos/motor/motor_speed.h
+++ b/frtos/motor/motor_speed.h
@@ -3,6 +3,8 @@
  * @brief monitor and update the speed of the wheels
  * @author Richie
  */
+#ifndef MOTOR_SPEED_H
+#define MOTOR_SPEED_H
 
 #include "motor_init.h"
 
@@ -84,21 +86,21 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor)
                        motor->pwm.level);
 }
 
-///*!
-// * @brief Set the speed of the wheels
-// * @param pwm_level The pwm_level of the wheels, from 0 to 99
-// */
-//void
-//set_wheel_speed_synced(uint32_t pwm_level)
-//{
-//    if (pwm_level > MAX_PWM_LEVEL)
-//    {
-//        pwm_level = MAX_PWM_LEVEL;
-//    }
-//
-//    set_wheel_speed(pwm_level, &g_motor_left);
-//    set_wheel_speed(pwm_level, &g_motor_right);
-//}
+/*!
+ * @brief Set the speed of the wheels
+ * @param pwm_level The pwm_level of the wheels, from 0 to 99
+ */
+void
+set_wheel_speed_synced(uint32_t pwm_level)
+{
+    if (pwm_level > MAX_PWM_LEVEL)
+    {
+        pwm_level = MAX_PWM_LEVEL;
+    }
+
+    set_wheel_speed(pwm_level, &g_motor_left);
+    set_wheel_speed(pwm_level, &g_motor_right);
+}
 
 ///*!
 // * @brief Set the distance to travel before stopping, must be called after
@@ -122,3 +124,5 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor)
 //    vTaskDelay(pdMS_TO_TICKS(1000));
 //    g_motor_right.speed.distance_cm = g_motor_left.speed.distance_cm;
 //}
+
+#endif /* MOTOR_SPEED_H */