Merge pull request #1 from Devoalda/main

frtos/CMakeLists.txt

@@ -1,3 +1,6 @@
 include(FreeRTOS_Kernel_import.cmake)
 
-add_subdirectory(car)
+add_subdirectory(
+        car
+        line_sensor
+)

frtos/line_sensor/CMakeLists.txt

@@ -0,0 +1,12 @@
+add_library(line_sensor
+        line_sensor.c
+        line_sensor.h
+        Config.h
+)
+
+target_link_libraries( line_sensor
+        hardware_adc
+        pico_stdlib)
+
+target_include_directories(line_sensor
+        PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}")

frtos/line_sensor/Config.h

@@ -0,0 +1,19 @@
+#ifndef CONFIG_H
+#define CONFIG_H
+
+/* ADC Configuration */
+#define VREF                                    ( 3.3f )
+
+#define ADC_READING_DELAY_MS                    ( 300 )
+
+#define ADC_PIN_ONE                             ( 26 )
+#define ADC_PIN_TWO                             ( 27 ) // Comment this line out
+                                                       // if you only have one
+
+#define THRESHOLD                               ( VREF / 2 ) // 50% of VREF
+
+/* Map */
+#define MAP_START_SYMBOL                        ( 5 )
+#define MAP_SIZE 20
+
+#endif //CONFIG_H

frtos/line_sensor/line_sensor.c

@@ -0,0 +1,209 @@
+#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 Get the status of the Line Sensor
+ * @param gpio The GPIO pin to read from
+ * @return state_t LINE_DETECTED or LINE_NOT_DETECTED
+ */
+static inline state_t
+get_state(uint gpio) {
+    adc_select_input(gpio - 26); // 26 is the first ADC pin (ADC0)
+
+    // ADC Conversion, vref is 3.3V, 12 bit resolution
+    float adc = (float) adc_read() * conversionFactor;
+
+    return (adc > THRESHOLD)
+           ? LINE_DETECTED : LINE_NOT_DETECTED;
+}
+
+/**
+ * @brief Get the Direction based on the Line Sensor
+ *
+ * This function will detect if the line sensor is detecting a line or not
+ * It will read from both ADC channels and return if both are detecting a line
+ *
+ * @return state_t LINE_DETECTED or LINE_NOT_DETECTED
+ */
+static inline
+direction_t get_current_dir() {
+#ifdef ADC_PIN_TWO
+    state_t state_two = get_state(ADC_PIN_TWO);
+#endif
+    state_t state_one = get_state(ADC_PIN_ONE);
+
+#ifdef ADC_PIN_TWO
+    direction_t direction = (state_one << 1) | state_two;
+#else
+    direction_t direction = state_one;
+#endif
+
+    return direction;
+}
+
+
+/**
+ * @brief Setup the Line Sensor
+ *
+ * This function will setup the Line Sensor by initializing the ADC
+ */
+void
+line_sensor_setup() {
+
+    adc_init();
+    adc_gpio_init(ADC_PIN_ONE);
+#ifdef ADC_PIN_TWO
+    adc_gpio_init(ADC_PIN_TWO);
+#endif
+
+#ifdef ADC_PIN_THREE            // Additional ADC Pin for Barcode Scanner
+    adc_gpio_init(ADC_PIN_THREE);
+#endif
+
+}
+
+/**
+ * @brief Initialize the car's initial state
+ *
+ * @return The initialized car state
+ */
+car_state_t
+initialize_car_state() {
+    car_state_t car_state;
+    car_state.x = MAP_SIZE >> 1;
+    car_state.y = MAP_SIZE >> 1;
+    car_state.current_direction = FORWARD;
+    car_state.orientation = NORTH;
+
+    map[car_state.x][car_state.y] = MAP_START_SYMBOL;
+
+    return car_state;
+}
+
+/**
+ * @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");
+    }
+}
+
+/**
+ * @brief FreeRTOS Task to read from the line sensor
+ *
+ * This task will read from the line sensor and print the state to the console
+ *
+ * @param params Pointer to the task parameters
+ */
+void
+line_sensor_task(__unused void *params) {
+
+    car_state_t car_state = initialize_car_state();
+
+    sleep_ms(2000);
+    printf("Starting Line Sensor Task\n");
+
+
+    for (u_int8_t i = 40; i != 0; i--) {
+        car_state.current_direction = get_current_dir();
+
+        switch (car_state.current_direction) {
+            case FORWARD:
+                handle_forward_movement(&car_state);
+                break;
+            case RIGHT:
+                handle_right_turn(&car_state);
+                break;
+            case LEFT:
+                handle_left_turn(&car_state);
+                break;
+            default:
+                break;
+        }
+
+        update_map(car_state);
+        sleep_ms(ADC_READING_DELAY_MS);
+    }
+
+    print_map();
+
+}

frtos/line_sensor/line_sensor.h

@@ -0,0 +1,47 @@
+#ifndef LINE_SENSOR_H
+#define LINE_SENSOR_H
+
+#include <stdio.h>
+#include "pico/stdlib.h"
+
+#include "hardware/adc.h"
+#include "Config.h"
+
+typedef enum
+{
+    LINE_DETECTED = 1,
+    LINE_NOT_DETECTED = 0,
+} state_t;
+
+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;
+
+void line_sensor_setup();
+
+static inline state_t get_state(uint gpio);
+static inline direction_t get_current_dir();
+
+void line_sensor_task(__unused void *params);
+
+#endif