Merge branch 'L4ncelot-R:main' into main

README.md

@@ -44,6 +44,9 @@ Motor module consist of 4 components:
 
 ## Line Sensor
 
+The Line sensor package is in `frtos/line_sensor`, and its configuration is in `frtos/config/line_sensor_config.h`. It
+contains the drivers and FreeRTOS tasks to read the line sensor data and update the car's obstruction struct.
+
 ## Magnetometer
 
 The magnetometer used is the [LSM303DLHC](https://www.st.com/resource/en/datasheet/lsm303dlhc.pdf) from STMicroelectronics.

frtos/barcode_sensor/barcode_sensor.h

@@ -1,6 +1,7 @@
 /* Barcode sensor */
 
 #include "barcode_sensor_init.h"
+#include <string.h>
 
 #define MAX_BARCODES 10 // Define the maximum number of barcodes to store
 #define BARCODE_SENSOR_TIMER_PERIOD_MS 100 // Define the barcode sensor timer period
@@ -8,6 +9,9 @@
 // Define the barcode sensor timer
 static struct repeating_timer barcode_sensor_timer;
 
+// Define the barcode data array
+static uint32_t barcode_data_array[MAX_BARCODES];
+static int barcode_data_index = 0;
 
 /**
  * @brief Decode a Code 39 barcode
@@ -15,9 +19,9 @@ static struct repeating_timer barcode_sensor_timer;
  * This function decodes a Code 39 barcode represented as a 9-bit binary number.
  *
  * @param barcode_data Binary representation of the barcode data (9 bits)
- * @return Decoded value as an integer
+ * @return Decoded value as a character
  */
-int code39_decode(uint32_t barcode_data) {
+char code39_decode(uint32_t barcode_data) {
     // Define the binary representations of Code 39 characters
     const uint32_t code39_characters[] = {
         0b001001001,  // 0
@@ -30,18 +34,50 @@ int code39_decode(uint32_t barcode_data) {
         0b001010011,  // 7
         0b001011101,  // 8
         0b001111001,  // 9
-        // Add more character representations as needed
+        0b001010101,  // A
+        0b001010111,  // B
+        0b001011101,  // C
+        0b001101101,  // D
+        0b001110101,  // E
+        0b001110111,  // F
+        0b001111101,  // G
+        0b001101011,  // H
+        0b001011011,  // I
+        0b001010011,  // J
+        0b001101111,  // K
+        0b001111011,  // L
+        0b001110011,  // M
+        0b001110001,  // N
+        0b001101001,  // O
+        0b001011001,  // P
+        0b001010001,  // Q
+        0b001001101,  // R
+        0b001001111,  // S
+        0b001001011,  // T
+        0b001111011,  // U
+        0b001111101,  // V
+        0b001110101,  // W
+        0b001010111,  // X
+        0b001011111,  // Y
+        0b001101101,  // Z
+        0b001110011,  // -
+        0b001011001,  // .
+        0b001101111,  // space
+        0b001111111,  // $
+        0b001010011,  // /
+        0b001010001,  // +
+        0b001001001,  // %
     };
 
     // Compare the barcode data to known Code 39 character representations
-    for (int i = 0; i < 10; i++) {
+    for (int i = 0; i < 44; i++) {
         if (barcode_data == code39_characters[i]) {
-            return i;  // Return the decoded value (0-9)
+            return (char)(i + 48);  // Return the decoded value as a character
         }
     }
 
-    // If the barcode data does not match any known character, return -1 to indicate an error
+    // If the barcode data does not match any known character, return '?' to indicate an error
-    return -1;
+    return '?';
 }
 
 
@@ -61,7 +97,6 @@ void monitor_barcode_sensor_task(void *params) {
         if (xSemaphoreTake(g_barcode_sensor_sem, portMAX_DELAY) == pdTRUE) {
             // Check the flag or receive the message
             if (barcode_sensor_triggered == pdTRUE) {
-                uint32_t barcode_data = 0;
                 int bar_width = 0; // Variable to store the width of the current bar
 
                 for (int i = 0; i < 9; i++) {
@@ -78,23 +113,39 @@ void monitor_barcode_sensor_task(void *params) {
                             bar_width = 0; // Reset the bar width measurement
                         }
 
-                        barcode_data |= (1u << i);
+                        barcode_data_array[barcode_data_index] |= (1u << i);
                     }
                 }
 
-                printf("Barcode Data (binary): %09b\n", barcode_data);
+                printf("Barcode Data (binary): %09b\n", barcode_data_array[barcode_data_index]);
 
+                barcode_data_index++;
+
+                if (barcode_data_index >= MAX_BARCODES) {
                     // Decode the barcode data
-                int decoded_value = code39_decode(barcode_data);
+                    for (int i = 0; i < MAX_BARCODES; i++) {
+                        int ones_count = 0;
+                        uint32_t barcode_data = barcode_data_array[i];
 
-                if (decoded_value != -1) {
+                        // Count the number of ones in the barcode data
-                    printf("Decoded Value: %d\n", decoded_value);
+                        for (int j = 0; j < 9; j++) {
+                            if ((barcode_data >> j) & 1) {
+                                ones_count++;
+                            }
+                        }
+
+                        char decoded_value = code39_decode(barcode_data);
+
+                        printf("Decoded Value: %c\n", decoded_value);
                         // Store or process the decoded value as needed
 
                         // Send the decoded value instead of the raw barcode data
-                    xMessageBufferSend(barcode_sensor_msg_buffer, &decoded_value, sizeof(int), 0);
+                        xMessageBufferSend(barcode_sensor_msg_buffer, &decoded_value, sizeof(char), 0);
-                } else {
+                    }
-                    printf("Error: Unable to decode the barcode.\n");
+
+                    // Reset the barcode data array and index
+                    memset(barcode_data_array, 0, sizeof(barcode_data_array));
+                    barcode_data_index = 0;
                 }
 
                 // Reset the flag
@@ -103,22 +154,3 @@ void monitor_barcode_sensor_task(void *params) {
         }
     }
 }
-
-
-
-
-/**
- * @brief Monitor the barcode sensor
- * @param params
- */
-void
-monitor_barcode_task(__unused void *params) {
-    state_t barcode_state;
-
-        // Receive from Buffer
-        xMessageBufferReceive(barcode_sensor_msg_buffer,
-                              &barcode_state,
-                              sizeof(state_t),
-                              portMAX_DELAY);
-        
-}

frtos/barcode_sensor/barcode_sensor_init.h

@@ -2,6 +2,7 @@
 
 #ifndef BARCODE_SENSOR_INIT_H
 #define BARCODE_SENSOR_INIT_H
+#define DEBOUNCE_DELAY_MS 100
 
 #include <stdio.h>
 #include "pico/stdlib.h"
@@ -13,9 +14,10 @@
 #include "message_buffer.h"
 #include "semphr.h"
 
-#include "barcode_sensor_config.h"
+#include "line_sensor_config.h"
 #include "line_sensor_init.h"
 
+
 // Set barcode time to 0
 static TickType_t lastBarcodeTime = 0;
 

frtos/barcode_sensor/barcode_sensor_test.c

@@ -40,7 +40,7 @@ main (void)
     printf("Test started!\n");
 
     barcode_sensor_setup();
-    initialize_car_state();
+    // initialize_car_state();
 
     launch();
 

frtos/config/line_sensor_config.h

@@ -7,5 +7,6 @@
 
 #define LEFT_SENSOR_PIN    (26)
 #define RIGHT_SENSOR_PIN   (27)
+#define BARCODE_SENSOR_PIN (22)
 
 #endif // CONFIG_H

frtos/line_sensor/line_sensor_init.h

@@ -31,7 +31,8 @@ SemaphoreHandle_t g_left_sensor_sem = NULL;
 static inline void
 line_sensor_init(car_struct_t *p_car_struct)
 {
-    p_car_struct->obs->left_sensor_detected, p_car_struct->obs->left_sensor_detected = false;
+    p_car_struct->obs->left_sensor_detected,
+        p_car_struct->obs->left_sensor_detected = false;
 
     g_left_sensor_sem = xSemaphoreCreateBinary();
 
@@ -40,7 +41,6 @@ line_sensor_init(car_struct_t *p_car_struct)
     // Initialise 3 GPIO pins and set them to input
     gpio_init_mask(mask);
     gpio_set_dir_in_masked(mask);
-
 }
 
 /**
@@ -48,35 +48,37 @@ line_sensor_init(car_struct_t *p_car_struct)
  * @param rt
  * @return True (To keep the timer running)
  */
-bool h_left_sensor_timer_handler(repeating_timer_t *repeatingTimer) {
+bool
+h_left_sensor_timer_handler(repeating_timer_t *repeatingTimer)
+{
 
     BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-    xSemaphoreGiveFromISR(g_left_sensor_sem,
+    xSemaphoreGiveFromISR(g_left_sensor_sem, &xHigherPriorityTaskWoken);
-                          &xHigherPriorityTaskWoken);
     portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
     return true;
 }
 
-
 void
-monitor_line_sensor_task(void *pvParameters) {
+monitor_line_sensor_task(void *pvParameters)
+{
     volatile obs_t *p_obs = NULL;
     p_obs                 = (obs_t *)pvParameters;
 
     for (;;)
     {
-//        if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
+
-//        {
         // Set the flag to notify the task
         p_obs->left_sensor_detected  = gpio_get(LEFT_SENSOR_PIN);
         p_obs->right_sensor_detected = gpio_get(RIGHT_SENSOR_PIN);
 
-	    // printf("Left Sensor: %d\n", p_obs->line_detected);
         vTaskDelay(pdMS_TO_TICKS(LINE_SENSOR_READ_DELAY));
-//        }
     }
 }
 
+/**
+ * @brief Initialise the tasks for the line sensor
+ * @param car_struct  The car struct
+ */
 void
 line_tasks_init(car_struct_t *car_struct)
 {
@@ -89,5 +91,4 @@ line_tasks_init(car_struct_t *car_struct)
                 &h_monitor_line_sensor_task);
 }
 
-
 #endif /* LINE_SENSOR_INIT_H */

frtos/line_sensor/line_sensor_test.c

@@ -2,7 +2,6 @@
 #include "line_sensor_init.h"
 #include "car_config.h"
 
-
 int
 main(void)
 {

frtos/line_sensor/line_sensor_unused.c

@@ -1,100 +0,0 @@
-#include <stdio.h>
-#include "pico/stdlib.h"
-
-#include "hardware/adc.h"
-
-#include "FreeRTOS.h"
-#include "task.h"
-#include "queue.h"
-#include "semphr.h"
-
-#include "line_sensor.h"
-#include "string.h"
-
-//const float conversionFactor = 3.3f / (1 << 12);
-
-volatile u_int8_t map[MAP_SIZE][MAP_SIZE] = {0};
-
-
-/**
- * @brief Update the map based on the car's state
- *
- * @param car_state The current car state
- */
-static inline void
-update_map(car_state_t car_state) {
-    if (car_state.x >= 0 && car_state.x < MAP_SIZE &&
-        car_state.y >= 0 && car_state.y < MAP_SIZE) {
-        map[car_state.x][car_state.y] = 1;
-    }
-}
-
-/**
- * @brief Handle forward movement of the car
- *
- * @param car_state The current car state
- */
-static void
-handle_forward_movement(car_state_t *car_state) {
-    printf("FORWARD, ");
-    // TODO: Check car's actual forward movement
-    switch (car_state->orientation) {
-        case NORTH:
-            printf("NORTH\n");
-            car_state->y++;
-            break;
-        case EAST:
-            printf("EAST\n");
-            car_state->x++;
-            break;
-        case SOUTH:
-            printf("SOUTH\n");
-            car_state->y--;
-            break;
-        case WEST:
-            printf("WEST\n");
-            car_state->x--;
-            break;
-    }
-}
-
-/**
- * @brief Handle a right turn of the car
- *
- * Note: Bitwise AND with 0x03 to ensure that the orientation
- * is always between 0 and 3
- * @param car_state The current car state
- */
-static inline void
-handle_right_turn(car_state_t *car_state) {
-    car_state->orientation = (car_state->orientation + 1) & 0x03;
-}
-
-/**
- * @brief Handle a left turn of the car
- *
- * @param car_state The current car state
- */
-static inline void
-handle_left_turn(car_state_t *car_state) {
-    car_state->orientation = (car_state->orientation - 1) & 0x03;
-}
-
-/**
- * @brief Print the map to the console
- *
- * This function will print the map to the console
- */
-void
-print_map() {
-    // Invert the map, 0,0 is at the bottom left
-    for (int i = MAP_SIZE - 1; i >= 0; i --)
-    {
-        for (int j = 0; j < MAP_SIZE; j ++)
-        {
-            printf("%d ", map[j][i]);
-        }
-        printf("\n");
-    }
-}
-

frtos/magnetometer/LSM303DLHC_register.h

@@ -28,8 +28,6 @@
 #define LSM303_TEMP_OUT_H_M         0x31
 #define LSM303_TEMP_OUT_L_M         0x32
 
-
-
 #define ACCEL_ADDR                  0x19
 #define MAG_ADDR                    0x1E
 

frtos/magnetometer/magnetometer_direction.h

@@ -9,11 +9,15 @@
  *          The yaw is calculated using the magnetometer and accelerometer data
  *          The roll, pitch and yaw are combined to calculate the direction
  *          of the car with a complementary filter and compensating for the
- *          temperature.
+ *          temperature. (existing)
  *
  *          The complementary filter is used to combine the accelerometer
  *          and magnetometer data (yaw) to calculate the direction of the car
  *
+ *          Complementary Filter was used previously, but it was not accurate
+ *          enough. The yaw calculated from the magnetometer data was used
+ *          instead.
+ *
  *          Source:
  *          https://www.nxp.com/docs/en/application-note/AN3461.pdf
  *          https://ahrs.readthedocs.io/en/latest/filters/complementary.html
@@ -80,7 +84,8 @@ calculate_yaw_magnetometer(int16_t magnetometer[3])
  * @return              Compensated Yaw
  */
 float
-compensate_magnetometer(float yaw_mag, int16_t temperature) {
+compensate_magnetometer(float yaw_mag, int16_t temperature)
+{
     // Calculate temperature difference from the reference temperature
     uint delta_temp = temperature - TEMPERATURE_OFFSET;
 
@@ -317,46 +322,6 @@ updateDirection(volatile direction_t * g_direction)
     read_direction(accelerometer, magnetometer, g_direction);
 
     print_orientation_data(*g_direction);
-//
-//    switch (g_direction->orientation)
-//    {
-//        case NORTH:
-//            cur_y++;
-//            break;
-//        case EAST:
-//            cur_x++;
-//            break;
-//        case SOUTH:
-//            cur_y--;
-//            break;
-//        case WEST:
-//            cur_x--;
-//            break;
-//        case NORTH_EAST:
-//            cur_x++;
-//            cur_y++;
-//            break;
-//        case SOUTH_EAST:
-//            cur_x++;
-//            cur_y--;
-//            break;
-//        case SOUTH_WEST:
-//            cur_x--;
-//            cur_y--;
-//            break;
-//        case NORTH_WEST:
-//            cur_x--;
-//            cur_y++;
-//            break;
-//    }
-
-    // Update the map based on the direction of the car (N, E, S, W)
-    //    update_map(g_direction.orientation, cur_x, cur_y);
-
-    //    printf("Current Position: (%d, %d)\n", cur_x, cur_y);
-    //    print_map();
-
-    //    print_roll_and_pitch(g_direction.roll_angle, g_direction.pitch_angle);
 }
 
 void

frtos/magnetometer/map.h

@@ -9,9 +9,9 @@
 #ifndef TEST_PROJECT_MAP_H
 #define TEST_PROJECT_MAP_H
 
-
 // Define the grid structure
-typedef struct {
+typedef struct
+{
     bool **data; // 2D array to represent the grid
     int    rows; // Number of rows in the grid
     int    cols; // Number of columns in the grid
@@ -21,16 +21,20 @@ typedef struct {
 Grid *car_path_grid;
 
 // Function to create and initialize a grid
-Grid *create_grid(int rows, int cols) {
+Grid *
+create_grid(int rows, int cols)
+{
     Grid *grid = (Grid *)malloc(sizeof(Grid));
     grid->rows = rows;
     grid->cols = cols;
 
     // Allocate memory for the 2D array
     grid->data = (bool **)malloc(rows * sizeof(bool *));
-    for (int i = 0; i < rows; i++) {
+    for (int i = 0; i < rows; i++)
+    {
         grid->data[i] = (bool *)malloc(cols * sizeof(bool));
-        for (int j = 0; j < cols; j++) {
+        for (int j = 0; j < cols; j++)
+        {
             grid->data[i][j] = false; // Initialize to 'false' (unvisited)
         }
     }
@@ -39,23 +43,32 @@ Grid *create_grid(int rows, int cols) {
 }
 
 // Function to mark a cell as visited
-void mark_cell(Grid *grid, int row, int col) {
+void
-    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) {
+mark_cell(Grid *grid, int row, int col)
+{
+    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
+    {
         grid->data[row][col] = true;
     }
 }
 
 // Function to check if a cell has been visited
-bool is_cell_visited(Grid *grid, int row, int col) {
+bool
-    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols) {
+is_cell_visited(Grid *grid, int row, int col)
+{
+    if (row >= 0 && row < grid->rows && col >= 0 && col < grid->cols)
+    {
         return grid->data[row][col];
     }
     return false; // Consider out-of-bounds as unvisited
 }
 
 // Function to destroy the grid and free memory
-void destroy_grid(Grid *grid) {
+void
-    for (int i = 0; i < grid->rows; i++) {
+destroy_grid(Grid *grid)
+{
+    for (int i = 0; i < grid->rows; i++)
+    {
         free(grid->data[i]);
     }
     free(grid->data);
@@ -64,12 +77,15 @@ void destroy_grid(Grid *grid) {
 
 // Function to update the map based on car's current orientation
 // Function to update the map based on car's current orientation and position
-void update_map(int orientation, int cur_x, int cur_y) {
+void
+update_map(int orientation, int cur_x, int cur_y)
+{
     // Define offsets for different orientations
     int offset_x = 0;
     int offset_y = 0;
 
-    switch (orientation) {
+    switch (orientation)
+    {
         case NORTH:
             offset_y = 1;
             break;
@@ -105,13 +121,16 @@ void update_map(int orientation, int cur_x, int cur_y) {
     mark_cell(car_path_grid, cur_x + offset_x, cur_y + offset_y);
 }
 
-
 // Function to print the map
-void print_map() {
+void
+print_map()
+{
     // Invert the map, 0,0 is at the Middle
     // Print 1 for visited cells and 0 for unvisited cells
-    for (int i = car_path_grid->rows - 1; i >= 0; i--) {
+    for (int i = car_path_grid->rows - 1; i >= 0; i--)
-        for (int j = 0; j < car_path_grid->cols; j++) {
+    {
+        for (int j = 0; j < car_path_grid->cols; j++)
+        {
             (car_path_grid->data[j][i]) ? printf("1 ") : printf("0 ");
             //                case false:
             //                    printf("0 ");

frtos/motor/motor_init.h

@@ -38,9 +38,9 @@ motor_init(car_struct_t *car_struct)
     g_right_sem = xSemaphoreCreateBinary();
 
     car_struct->p_pid->use_pid  = true;
-    car_struct->p_pid->kp_value = 60.f;
+    car_struct->p_pid->kp_value = 600.f;
-    car_struct->p_pid->ki_value = 0.f;
+    car_struct->p_pid->ki_value = 66.67f;
-    car_struct->p_pid->kd_value = 135.f;
+    car_struct->p_pid->kd_value = 1350.f;
 
     // initialize the car_struct
     car_struct->p_left_motor->pwm.level         = 0u;

frtos/motor/motor_pid.h

@@ -67,7 +67,7 @@ repeating_pid_handler(struct repeating_timer *ppp_timer)
 
     if (temp <= MIN_PWM_LEVEL)
     {
-        temp = MIN_PWM_LEVEL + 1u;
+        temp = MIN_PWM_LEVEL;
     }
 
     set_wheel_speed((uint32_t)temp, car_strut->p_right_motor);

frtos/motor/motor_test.c

@@ -47,7 +47,7 @@ main(void)
                 "motor_turning_task",
                 configMINIMAL_STACK_SIZE,
                 (void *)&car_struct,
-                WHEEL_CONTROL_PRIO,
+                1,
                 &h_motor_turning_task_handle);
 
     // PID timer

frtos/rtos_car.c

@@ -19,6 +19,11 @@ check_line_touch(void *params)
            | (car_struct->obs->right_sensor_detected);
 }
 
+/**
+ * @brief Check if the car is on the line or if there is an obstacle
+ * @param params  The car_struct
+ * @return 1 if there is an obstacle, 0 otherwise
+ */
 bool
 check_collision(void *params)
 {
@@ -34,6 +39,8 @@ void
 motor_control_task(void *params)
 {
     car_struct_t *car_struct = (car_struct_t *)params;
+    set_wheel_direction(DIRECTION_FORWARD);
+    set_wheel_speed_synced(90u, car_struct);
 
     for (;;)
     {
@@ -41,24 +48,19 @@ motor_control_task(void *params)
         switch (temp)
         {
             default:
-                set_wheel_direction(DIRECTION_FORWARD);
-                set_wheel_speed_synced(90u, car_struct);
-                distance_to_stop(car_struct, 50.f);
-                vTaskDelay(pdMS_TO_TICKS(3000));
                 break;
             case 0b01:
-                car_struct->p_right_motor->speed.current_cms
+                car_struct->p_right_motor->speed.distance_cm
-                    += SLOT_DISTANCE_CM * 1000.f;
+                    -= SLOT_DISTANCE_CM;
                 break;
             case 0b10:
-                car_struct->p_right_motor->speed.current_cms
+                car_struct->p_right_motor->speed.distance_cm
-                    -= SLOT_DISTANCE_CM * 1000.f;
+                    += SLOT_DISTANCE_CM;
                 break;
             case 0b11:
-                // set_wheel_direction(DIRECTION_MASK);
+                turn(DIRECTION_LEFT, 90u, 90u, car_struct);
-                // set_wheel_speed_synced(0u, car_struct);
+                set_wheel_direction(DIRECTION_FORWARD);
-                // vTaskDelay(pdMS_TO_TICKS(1000));
+                set_wheel_speed_synced(90u, car_struct);
-                // turn(DIRECTION_LEFT, 90u, 90u, car_struct);
                 break;
         }
 
@@ -78,11 +80,12 @@ obs_task(void *params)
     {
         if (car_struct->obs->ultrasonic_detected)
         {
-            // turn(DIRECTION_LEFT, 180u, 90u, car_struct);
+//             turn(DIRECTION_LEFT, 130u, 90u, car_struct);
 //             set_wheel_direction(DIRECTION_FORWARD);
 //             set_wheel_speed_synced(90u, car_struct);
+//
             revert_wheel_direction();
-            distance_to_stop(car_struct, 25.f);
+            distance_to_stop(car_struct, 20.f);
         }
         vTaskDelay(pdMS_TO_TICKS(50));
     }
@@ -96,14 +99,14 @@ turn_task(void *params)
         for (;;)
         {
                 set_wheel_direction(DIRECTION_FORWARD);
-                set_wheel_speed_synced(90u, car_struct);
+                set_wheel_speed_synced(89u, car_struct);
 
-                distance_to_stop(car_struct, 50.f);
+                distance_to_stop(car_struct, 20.f);
                 vTaskDelay(pdMS_TO_TICKS(1000));
 
 //                turn_to_yaw(DIRECTION_LEFT, 230.f, 80u, car_struct);
 
-                turn(DIRECTION_RIGHT, 50.f, 90u, car_struct);
+                turn(DIRECTION_RIGHT, 80.f, 90u, car_struct);
                 vTaskDelay(pdMS_TO_TICKS(1000));
         }
 }
@@ -187,13 +190,13 @@ main(void)
 //                 &h_motor_turning_task_handle);
 
     // obs task
-    TaskHandle_t h_obs_task_handle = NULL;
+//    TaskHandle_t h_obs_task_handle = NULL;
-    xTaskCreate(obs_task,
+//    xTaskCreate(obs_task,
-                "obs_task",
+//                "obs_task",
-                configMINIMAL_STACK_SIZE,
+//                configMINIMAL_STACK_SIZE,
-                (void *)&car_struct,
+//                (void *)&car_struct,
-                PRIO,
+//                PRIO,
-                &h_obs_task_handle);
+//                &h_obs_task_handle);
 
     // turn task
         TaskHandle_t h_turn_task_handle = NULL;

frtos/rtos_car_line.c

@@ -0,0 +1,126 @@
+
+#include "line_sensor_init.h"
+#include "ultrasonic_sensor.h"
+#include "car_config.h"
+#include "motor_init.h"
+
+/*!
+ * @brief Check if the car is on the line
+ * @param params
+ * @return
+ */
+uint8_t
+check_line_touch(void *params)
+{
+    car_struct_t *car_struct = (car_struct_t *)params;
+
+    return (car_struct->obs->left_sensor_detected << 1)
+           | (car_struct->obs->right_sensor_detected);
+}
+
+void
+motor_control_task(void *params)
+{
+    car_struct_t *car_struct = (car_struct_t *)params;
+
+    set_wheel_direction(DIRECTION_FORWARD);
+    set_wheel_speed_synced(90u, car_struct);
+
+    for (;;)
+    {
+        uint8_t temp = check_line_touch(car_struct);
+        switch (temp)
+        {
+            default:
+                break;
+            case 0b01: // right
+                car_struct->p_right_motor->speed.distance_cm -=
+                    SLOT_DISTANCE_CM;
+                break;
+            case 0b10: //left
+                car_struct->p_right_motor->speed.distance_cm +=
+                    SLOT_DISTANCE_CM;
+                break;
+            case 0b11:
+                revert_wheel_direction();
+                distance_to_stop(car_struct, 17.f);
+                turn(DIRECTION_LEFT, 90u, 90u, car_struct);
+                set_wheel_direction(DIRECTION_FORWARD);
+                set_wheel_speed_synced(90u, car_struct);
+                break;
+        }
+
+        vTaskDelay(pdMS_TO_TICKS(50));
+    }
+}
+
+void
+h_main_irq_handler(void)
+{
+    if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL)
+    {
+        gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
+
+        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+        xSemaphoreGiveFromISR(g_left_sem, &xHigherPriorityTaskWoken);
+        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    }
+
+    if (gpio_get_irq_event_mask(SPEED_PIN_RIGHT) & GPIO_IRQ_EDGE_FALL)
+    {
+        gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
+
+        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+        xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken);
+        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+    }
+}
+
+int
+main(void)
+{
+    stdio_usb_init();
+
+    obs_t obs;
+
+    motor_t     motor_right;
+    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,
+                                .p_direction   = &direction };
+
+    // line
+    line_sensor_init(&car_struct);
+    line_tasks_init(&car_struct);
+    printf("Line sensor initialized!\n");
+
+    // motor
+    motor_init(&car_struct);
+    motor_tasks_init(&car_struct, &h_main_irq_handler);
+    printf("Motor initialized!\n");
+
+    sleep_ms(1000u);
+
+    // control task
+    TaskHandle_t h_motor_turning_task_handle = NULL;
+    xTaskCreate(motor_control_task,
+                "motor_turning_task",
+                configMINIMAL_STACK_SIZE,
+                (void *)&car_struct,
+                PRIO,
+                &h_motor_turning_task_handle);
+
+    // PID timer
+    struct repeating_timer pid_timer;
+    add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer);
+
+    vTaskStartScheduler();
+
+    return (0);
+}

frtos/ultrasonic_sensor/ultrasonic_init.h

@@ -18,7 +18,7 @@ check_obstacle(void *pvParameters)
     while (true)
     { // Put trigger pin high for 10us
         gpio_put(TRIG_PIN, 1);
-        sleep_us(10);
+        vTaskDelay(1);
         gpio_put(TRIG_PIN, 0);
 
         // Wait for echo pin to go high