Integrate Line and Ultraman

frtos/car/car.h

@@ -7,14 +7,5 @@
 
 #include "car_init.h"
 
-static car_state_t initialize_car_state() {
-    g_car_state.x = MAP_SIZE >> 1;
-    g_car_state.y = MAP_SIZE >> 1;
-    g_car_state.current_direction = FORWARD;
-    g_car_state.orientation = NORTH;
-
-    return g_car_state;
-}
-
 
 #endif /* CAR_H */

frtos/car/car_init.h

@@ -12,13 +12,5 @@
 #include "FreeRTOS.h"
 #include "task.h"
 
-#include "line_sensor_config.h"
-
-struct s_obs_struct {
-    bool line_detected;
-    bool ultrasonic_detected;
-};
-
-
 
 #endif /* CAR_INIT_H */

frtos/config/line_sensor_config.h

@@ -8,16 +8,4 @@
 #define LEFT_SENSOR_PIN                         ( 26 )
 #define RIGHT_SENSOR_PIN                        ( 27 )
 
-typedef struct s_obs_struct {
-    uint8_t line_detected;
-    bool ultrasonic_detected;
-} obs_t;
-
-
-typedef struct
-{
-    obs_t      *obs;
-
-} line_car_struct_t;
-
 #endif //CONFIG_H

frtos/line_sensor/CMakeLists.txt

@@ -12,6 +12,8 @@ target_link_libraries(
 
 target_include_directories(line_sensor_test
         PRIVATE ../config
+        ../car
+        ../ultrasonic_sensor
 )
 
 pico_enable_stdio_usb(line_sensor_test 1)

frtos/line_sensor/line_sensor_init.h

@@ -18,6 +18,7 @@
 #include "semphr.h"
 
 #include "line_sensor_config.h"
+#include "car_config.h"
 
 // Semaphore
 SemaphoreHandle_t g_left_sensor_sem = NULL;
@@ -28,10 +29,10 @@ SemaphoreHandle_t g_left_sensor_sem = NULL;
  * This function will setup the Line Sensor by initializing it as an input
  */
 static inline void
-line_sensor_init(line_car_struct_t *p_car) {
+line_sensor_init() {
-    obs_t obs = {1, 0};
+//    obs_t obs = {0, 0};
-
+//
-    p_car->obs = &obs;
+//    p_car->obs = &obs;
 
     g_left_sensor_sem = xSemaphoreCreateBinary();
 
@@ -65,12 +66,13 @@ monitor_left_sensor_task(void *pvParameters) {
 
     for (;;)
     {
-        if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
+//        if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
-        {
+//        {
             // Set the flag to notify the task
             p_obs->line_detected = gpio_get(LEFT_SENSOR_PIN);
             printf("Left Sensor: %d\n", p_obs->line_detected);
-        }
+            vTaskDelay(pdMS_TO_TICKS(LINE_SENSOR_READ_DELAY));
+//        }
     }
 }
 

frtos/line_sensor/line_sensor_test.c

@@ -1,17 +1,13 @@
 
 #include "line_sensor_init.h"
+#include "ultrasonic_sensor.h"
+#include "car_config.h"
 
-#define READ_LEFT_SENSOR_PRIO  (tskIDLE_PRIORITY + 2UL)
+#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
 
 void
-launch(line_car_struct_t *car_struct)
+launch(car_struct_t *car_struct)
 {
-    struct repeating_timer g_left_sensor_timer;
-    add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
-                           h_left_sensor_timer_handler,
-                           NULL,
-                           &g_left_sensor_timer);
-
     TaskHandle_t h_monitor_left_sensor_task = NULL;
     xTaskCreate(monitor_left_sensor_task,
                 "read_left_sensor_task",
@@ -20,7 +16,13 @@ launch(line_car_struct_t *car_struct)
                 READ_LEFT_SENSOR_PRIO,
                 &h_monitor_left_sensor_task);
 
-    vTaskStartScheduler();
+    TaskHandle_t h_monitor_ultrasonic_task = NULL;
+    xTaskCreate(check_obstacle,
+                "read_ultrasonic_task",
+                configMINIMAL_STACK_SIZE,
+                (void *)car_struct->obs,
+                READ_LEFT_SENSOR_PRIO,
+                &h_monitor_ultrasonic_task);
 }
 
 int
@@ -30,15 +32,21 @@ main(void)
 
     obs_t obs = { 0, 0 };
 
-    line_car_struct_t car_struct = { .obs = &obs };
+    car_struct_t car_struct = { .obs = &obs };
 
     sleep_ms(2000);
 
     printf("Test started!\n");
 
-    line_sensor_init(&car_struct);
+    line_sensor_init();
+    printf("Line sensor initialized!\n");
+
+    init_ultrasonic();
+    printf("Ultrasonic sensor initialized!\n");
 
     launch(&car_struct);
 
+    vTaskStartScheduler();
+
     return (0);
 }

frtos/magnetometer/magnetometer_direction.h

@@ -340,7 +340,7 @@ void updateDirection() {
     // Temperature in degrees Celsius
 //    printf("Temperature: %d\n", temperature[0]);
 
-    print_orientation_data();
+//    print_orientation_data();
 
 //    printf("Direction: ");
 

frtos/ultrasonic_sensor/ultrasonic_sensor.h

@@ -8,7 +8,8 @@
 #define ULTRASONIC_SENSOR_H
 
 #include "ultrasonic_init.h"
-#include "motor_speed.h"
+//#include "motor_speed.h"
+#include "car_config.h"
 
 // volatile uint32_t start_time;
 // volatile uint32_t end_time;
@@ -56,51 +57,53 @@ check_obstacle(void *pvParameters)
 {
     while (true)
     { // Put trigger pin high for 10us
-        gpio_put(TRIG_PIN, 1);
+            gpio_put(TRIG_PIN, 1);
-        sleep_us(10);
+            sleep_us(10);
-        gpio_put(TRIG_PIN, 0);
+            gpio_put(TRIG_PIN, 0);
 
-        // Wait for echo pin to go high
+            // Wait for echo pin to go high
-        while (gpio_get(ECHO_PIN) == 0)
+            while (gpio_get(ECHO_PIN) == 0)
-            tight_loop_contents();
+                tight_loop_contents();
 
-        // Measure the pulse width (time taken for the echo to return)
+            // Measure the pulse width (time taken for the echo to return)
-        uint32_t start_time = time_us_32();
+            uint32_t start_time = time_us_32();
-        while (gpio_get(ECHO_PIN) == 1)
+            while (gpio_get(ECHO_PIN) == 1)
-            tight_loop_contents();
+                tight_loop_contents();
 
-        uint32_t end_time = time_us_32();
+            uint32_t end_time = time_us_32();
 
-        // Calculate the distance (in centimeters)
+            // Calculate the distance (in centimeters)
-        uint32_t pulse_duration = end_time - start_time;
+            uint32_t pulse_duration = end_time - start_time;
-        float    distance
+            float    distance
-            = (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
+                = (pulse_duration * 0.034 / 2); // Speed of sound in cm/us
 
-        // printf("Distance: %.2f cm\n", distance);
+            // printf("Distance: %.2f cm\n", distance);
 
-        // change value of obstacle_detected in ultrasonic_t struct
+            // change value of obstacle_detected in ultrasonic_t struct
-        ultrasonic_t *ultrasonic_sensor      = (ultrasonic_t *)pvParameters;
+            obs_t *ultrasonic_sensor               = (obs_t *)pvParameters;
-        ultrasonic_sensor->obstacle_detected = (distance < 7);
+            ultrasonic_sensor->ultrasonic_detected = (distance < 7);
 
-        printf("Distance: %.2f cm, Obstacle Detected: %d\n",
+            printf("Distance: %.2f cm, Obstacle Detected: %d\n",
-               distance,
+                   distance,
-               ultrasonic_sensor->obstacle_detected);
+                   ultrasonic_sensor->ultrasonic_detected);
-        vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
+                    vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
     }
 }
 
-void
-check_global(void *pvParameters)
-{
-    while (true)
-    {
-        ultrasonic_t *ultrasonic_sensor      = (ultrasonic_t *)pvParameters;
 
-        printf("Global Obstacle Detected : %d\n",
+
-               ultrasonic_sensor->obstacle_detected);
+//void
-        vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
+//check_global(void *pvParameters)
-    }
+//{
-}
+//    while (true)
+//    {
+//        ultrasonic_t *ultrasonic_sensor      = (ultrasonic_t *)pvParameters;
+//
+//        printf("Global Obstacle Detected : %d\n",
+//               ultrasonic_sensor->obstacle_detected);
+//        vTaskDelay(pdMS_TO_TICKS(ULTRASONIC_SENSOR_READ_DELAY));
+//    }
+//}
 
 // void
 // distance_task(__unused void *params)