Ultrasonic Sensor + Car init obs struct

frtos/car/car_init.h

@@ -14,30 +14,10 @@
 
 #include "line_sensor_config.h"
 
-typedef enum {
-    ERROR = 0,
-    RIGHT = 1,
-    LEFT = 2,
-    FORWARD = 3
-} direction_t;
-
-typedef enum {
-    NORTH = 0,
-    EAST = 1,
-    SOUTH = 2,
-    WEST = 3,
-} orientation_t;
-
-typedef struct {
-    u_int8_t x;                     // Current x coordinate
-    u_int8_t y;                     // Current y coordinate
-    direction_t current_direction;  // Current direction (forward, left, right)
-    orientation_t orientation;      // Current orientation (N, E, S, W)
-} car_state_t;
-
-/* Common Car State Structure (TODO: TBC)*/
-
-static car_state_t g_car_state;
+struct s_obs_struct {
+    bool line_detected;
+    bool ultrasonic_detected;
+};
 
 
 

frtos/ultrasonic_sensor/ultrasonic_sensor.h

@@ -51,6 +51,47 @@ KalmanFilter(float U)
 //     }
 // }
 
+bool obstacle_detected = false;
+
+void check_obstacle() {
+   // Put trigger pin high for 10us
+        gpio_put(TRIG_PIN, 1);
+        sleep_us(10);
+        gpio_put(TRIG_PIN, 0);
+
+        // Wait for echo pin to go high
+        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);
+
+        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();
+            }
+        }
+}
+
 void
 distance_task(__unused void *params)
 {