diff --git a/frtos/config/ultrasonic_sensor_config.h b/frtos/config/ultrasonic_sensor_config.h
index 3bdcc22..dda705d 100644
--- a/frtos/config/ultrasonic_sensor_config.h
+++ b/frtos/config/ultrasonic_sensor_config.h
@@ -3,9 +3,14 @@
 
 /* ADC Configuration */
 
-#define TRIG_PIN                                ( 2 )
-#define ECHO_PIN                                ( 3 )
+#define TRIG_PIN (2)
+#define ECHO_PIN (3)
 
-#define ULTRASONIC_SENSOR_READ_DELAY            ( 500 )
+#define ULTRASONIC_SENSOR_READ_DELAY (100)
 
-#endif //ULTRASONIC_CONFIG_H
+typedef struct
+{
+    bool obstacle_detected;
+} ultrasonic_t;
+
+#endif // ULTRASONIC_CONFIG_H
diff --git a/frtos/ultrasonic_sensor/ultrasonic_init.h b/frtos/ultrasonic_sensor/ultrasonic_init.h
index d2db86d..f8ee234 100644
--- a/frtos/ultrasonic_sensor/ultrasonic_init.h
+++ b/frtos/ultrasonic_sensor/ultrasonic_init.h
@@ -11,6 +11,8 @@
 #include "pico/stdlib.h"
 #include "ultrasonic_sensor_config.h"
 
+ultrasonic_t ultrasonic_sensor = { .obstacle_detected = false };
+
 void
 init_ultrasonic(void)
 {
diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.c b/frtos/ultrasonic_sensor/ultrasonic_sensor.c
index c5e7f6f..830a48b 100644
--- a/frtos/ultrasonic_sensor/ultrasonic_sensor.c
+++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.c
@@ -8,12 +8,12 @@
 void
 vLaunch(void)
 {   
-    gpio_set_irq_enabled_with_callback(ECHO_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, echo_handler);
+    // gpio_set_irq_enabled_with_callback(ECHO_PIN, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true, echo_handler);
 
-    irq_set_enabled(IO_IRQ_BANK0, true);
+    // irq_set_enabled(IO_IRQ_BANK0, true);
 
     TaskHandle_t disttask;
-    xTaskCreate(distance_task,
+    xTaskCreate(check_obstacle,
                 "TestDistThread",
                 configMINIMAL_STACK_SIZE,
                 NULL,
diff --git a/frtos/ultrasonic_sensor/ultrasonic_sensor.h b/frtos/ultrasonic_sensor/ultrasonic_sensor.h
index d89c337..7d56410 100644
--- a/frtos/ultrasonic_sensor/ultrasonic_sensor.h
+++ b/frtos/ultrasonic_sensor/ultrasonic_sensor.h
@@ -1,8 +1,8 @@
 /**
-* @file ultrasonic_sensor.h
-* @brief Monitor the distance between the car and the wall
-* @author Poon Xiang Yuan
-*/
+ * @file ultrasonic_sensor.h
+ * @brief Monitor the distance between the car and the wall
+ * @author Poon Xiang Yuan
+ */
 
 #ifndef ULTRASONIC_SENSOR_H
 #define ULTRASONIC_SENSOR_H
@@ -12,28 +12,28 @@
 
 // volatile uint32_t start_time;
 // volatile uint32_t end_time;
-volatile bool     echo_rising = false;
+// volatile bool     echo_rising = false;
 
 float
 KalmanFilter(float U)
 {
-   static float R = 10;    // noise convariance can be 10, higher better smooth
-   static float H = 1;     // Measurement Map scalar
-   static float Q = 10;    // initial estimated convariance
-   static float P = 0;     // initial error covariance
-   static float U_hat = 0; // initial estimated state
-   static float K     = 0; // initial Kalman gain
+    static float R = 10;    // noise convariance can be 10, higher better smooth
+    static float H = 1;     // Measurement Map scalar
+    static float Q = 10;    // initial estimated convariance
+    static float P = 0;     // initial error covariance
+    static float U_hat = 0; // initial estimated state
+    static float K     = 0; // initial Kalman gain
 
-   // Predict
-   //
-   K     = P * H / (H * P * H + R);     // Update Kalman gain
-   U_hat = U_hat + K * (U - H * U_hat); // Update estimated state
+    // Predict
+    //
+    K     = P * H / (H * P * H + R);     // Update Kalman gain
+    U_hat = U_hat + K * (U - H * U_hat); // Update estimated state
 
-   // Update error covariance
-   //
-   P = (1 - K * H) * P + Q;
+    // Update error covariance
+    //
+    P = (1 - K * H) * P + Q;
 
-   return U_hat;
+    return U_hat;
 }
 
 // void
@@ -51,10 +51,11 @@ KalmanFilter(float U)
 //     }
 // }
 
-bool obstacle_detected = false;
-
-void check_obstacle() {
-   // Put trigger pin high for 10us
+void
+check_obstacle(void *pvParameters)
+{
+    while (true)
+    { // Put trigger pin high for 10us
         gpio_put(TRIG_PIN, 1);
         sleep_us(10);
         gpio_put(TRIG_PIN, 0);
@@ -75,62 +76,72 @@ void check_obstacle() {
         float    distance
             = (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
 
-        printf("Distance: %.2f cm\n", distance);
+        // printf("Distance: %.2f cm\n", distance);
 
-        obstacle_detected = (distance < 7);
-}
+        // change value of obstacle_detected in ultrasonic_t struct
+        ultrasonic_t *ultrasonic_sensor      = (ultrasonic_t *)pvParameters;
+        ultrasonic_sensor->obstacle_detected = (distance < 7);
 
-void distance_task_two(__unused void *params)
-{
-        for(;;)
-        {
-            // Semaphore to release the task every 100ms
-            if (xSemaphoreTake(distance_semaphore, portMAX_DELAY))
-            {
-                check_obstacle();
-            }
-        }
+        printf("Distance: %.2f cm, Obstacle Detected: %d\n",
+               distance,
+               ultrasonic_sensor->obstacle_detected);
+        vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
+    }
 }
 
 void
-distance_task(__unused void *params)
+check_global(void *pvParameters)
 {
-   while (true)
-   {
-       vTaskDelay(1000);
+    while (true)
+    {
+        ultrasonic_t *ultrasonic_sensor      = (ultrasonic_t *)pvParameters;
 
-       gpio_put(TRIG_PIN, 1);
-       sleep_us(10);
-       gpio_put(TRIG_PIN, 0);
-
-       while (gpio_get(ECHO_PIN) == 0)
-           tight_loop_contents();
-
-       // Measure the pulse width (time taken for the echo to return)
-       uint32_t start_time = time_us_32();
-       while (gpio_get(ECHO_PIN) == 1)
-           tight_loop_contents();
-       uint32_t end_time = time_us_32();
-
-       // Calculate the distance (in centimeters)
-       uint32_t pulse_duration = end_time - start_time;
-       float    distance
-           = (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
-
-       printf("Distance: %.2f cm\n", distance);
-       // printf("Kalman Filtered Distance: %.2f cm\n", KalmanFilter(distance));
-
-       if (distance < 7)
-       {
-           // set_wheel_direction(DIRECTION_MASK);
-           set_wheel_speed_synced(0u);
-           printf("Collision Imminent!\n");
-           vTaskDelay(pdMS_TO_TICKS(3000));
-           spin_to_yaw(350);
-           set_wheel_direction(DIRECTION_FORWARD);
-           set_wheel_speed_synced(90u);
-       }
-       start_time, end_time = 0;
-   }
+        printf("Global Obstacle Detected : %d\n",
+               ultrasonic_sensor->obstacle_detected);
+        vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
+    }
 }
+
+// void
+// distance_task(__unused void *params)
+// {
+//     while (true)
+//     {
+//         vTaskDelay(1000);
+
+//         gpio_put(TRIG_PIN, 1);
+//         sleep_us(10);
+//         gpio_put(TRIG_PIN, 0);
+
+//         while (gpio_get(ECHO_PIN) == 0)
+//             tight_loop_contents();
+
+//         // Measure the pulse width (time taken for the echo to return)
+//         uint32_t start_time = time_us_32();
+//         while (gpio_get(ECHO_PIN) == 1)
+//             tight_loop_contents();
+//         uint32_t end_time = time_us_32();
+
+//         // Calculate the distance (in centimeters)
+//         uint32_t pulse_duration = end_time - start_time;
+//         float    distance
+//             = (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
+
+//         printf("Distance: %.2f cm\n", distance);
+//         // printf("Kalman Filtered Distance: %.2f cm\n",
+//         // KalmanFilter(distance));
+
+//         if (distance < 7)
+//         {
+//             // set_wheel_direction(DIRECTION_MASK);
+//             set_wheel_speed_synced(0u);
+//             printf("Collision Imminent!\n");
+//             vTaskDelay(pdMS_TO_TICKS(3000));
+//             spin_to_yaw(350);
+//             set_wheel_direction(DIRECTION_FORWARD);
+//             set_wheel_speed_synced(90u);
+//         }
+//         start_time, end_time = 0;
+//     }
+// }
 #endif /* ULTRASONIC_SENSOR_H */
\ No newline at end of file