diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h
index 4aeb752..8da5c41 100644
--- a/frtos/config/magnetometer_config.h
+++ b/frtos/config/magnetometer_config.h
@@ -5,7 +5,7 @@
 #define I2C_SDA  (8)
 #define I2C_SCL  (9)
 
-#define DIRECTION_READ_DELAY (100)
+#define DIRECTION_READ_DELAY (200)
 
 #define NUM_READINGS                            ( 10 ) // Number of readings to
                                                         // take before
diff --git a/frtos/magnetometer/magnetometer_direction.h b/frtos/magnetometer/magnetometer_direction.h
index 0c20347..bf5e1dc 100644
--- a/frtos/magnetometer/magnetometer_direction.h
+++ b/frtos/magnetometer/magnetometer_direction.h
@@ -388,7 +388,7 @@ magnetometer_tasks_init(car_struct_t *car_struct)
     xTaskCreate(monitor_direction_task,
                 "Direction Task",
                 configMINIMAL_STACK_SIZE,
-                car_struct,
+                (void *) car_struct->p_direction,
                 PRIO,
                 &h_direction_task);
 }
diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h
index dc8da76..ad79f18 100644
--- a/frtos/magnetometer/magnetometer_init.h
+++ b/frtos/magnetometer/magnetometer_init.h
@@ -299,6 +299,7 @@ magnetometer_init(car_struct_t *p_car_struct)
     p_car_struct->p_direction->roll_angle  = LEFT;
     p_car_struct->p_direction->pitch_angle = UP;
 
+    printf("Magnetometer Initialising\n");
     i2c_init(I2C_PORT, 400 * 1000);
 
     gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c
index 8627969..2ec2c4a 100644
--- a/frtos/rtos_car.c
+++ b/frtos/rtos_car.c
@@ -8,8 +8,7 @@ bool
 check_collision(void *params)
 {
     car_struct_t *car_struct = (car_struct_t *)params;
-    return !(car_struct->obs->line_detected) ||
-           car_struct->obs->ultrasonic_detected;
+    return car_struct->obs->line_detected || car_struct->obs->ultrasonic_detected;
 }
 
 void
@@ -18,19 +17,20 @@ motor_control_task(void *params)
     car_struct_t *car_struct = (car_struct_t *)params;
     for (;;)
     {
+        printf("Collision: %d\n", check_collision(car_struct));
         if (check_collision(car_struct))
         {
             spin_left(90, car_struct);
 
-            if (check_collision(car_struct))
-            {
-                spin_right(180, car_struct);
-
-                if (check_collision(car_struct))
-                {
-                    spin_right(90, car_struct);
-                }
-            }
+//            if (check_collision(car_struct))
+//            {
+//                spin_right(180, car_struct);
+//
+//                if (check_collision(car_struct))
+//                {
+//                    spin_right(90, car_struct);
+//                }
+//            }
         }
         else
         {
@@ -52,10 +52,13 @@ main(void)
     motor_t     motor_left;
     motor_pid_t pid;
 
+    direction_t direction;
+
     car_struct_t car_struct = { .p_right_motor = &motor_right,
                                 .p_left_motor  = &motor_left,
                                 .p_pid         = &pid,
-                                .obs = &obs};
+                                .obs = &obs,
+                               .p_direction = &direction};
 
     // ultra
     ultrasonic_init(&car_struct);
@@ -72,6 +75,11 @@ main(void)
     motor_tasks_init(&car_struct, &h_wheel_sensor_isr_handler);
     printf("Motor initialized!\n");
 
+    // Magnetometer
+    magnetometer_init(&car_struct);
+//    magnetometer_tasks_init(&car_struct);
+    printf("Magnetometer initialized!\n");
+
     // control task
     TaskHandle_t h_motor_turning_task_handle = NULL;
     xTaskCreate(motor_control_task,