From 7c5b7e819ee4ed8d93a3abc7b8f25a90e4e62dd7 Mon Sep 17 00:00:00 2001 From: Devoalda Date: Thu, 12 Oct 2023 21:38:57 +0800 Subject: [PATCH] feature(Line Sensor): Line Sensor init --- frtos/CMakeLists.txt | 5 +- frtos/line_sensor/CMakeLists.txt | 12 ++ frtos/line_sensor/Config.h | 19 +++ frtos/line_sensor/line_sensor.c | 209 +++++++++++++++++++++++++++++++ frtos/line_sensor/line_sensor.h | 47 +++++++ 5 files changed, 291 insertions(+), 1 deletion(-) create mode 100644 frtos/line_sensor/CMakeLists.txt create mode 100644 frtos/line_sensor/Config.h create mode 100644 frtos/line_sensor/line_sensor.c create mode 100644 frtos/line_sensor/line_sensor.h diff --git a/frtos/CMakeLists.txt b/frtos/CMakeLists.txt index 3e3d248..ba96573 100644 --- a/frtos/CMakeLists.txt +++ b/frtos/CMakeLists.txt @@ -1,3 +1,6 @@ include(FreeRTOS_Kernel_import.cmake) -add_subdirectory(car) +add_subdirectory( + car + line_sensor +) diff --git a/frtos/line_sensor/CMakeLists.txt b/frtos/line_sensor/CMakeLists.txt new file mode 100644 index 0000000..47ecca5 --- /dev/null +++ b/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}") \ No newline at end of file diff --git a/frtos/line_sensor/Config.h b/frtos/line_sensor/Config.h new file mode 100644 index 0000000..8a36d39 --- /dev/null +++ b/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 diff --git a/frtos/line_sensor/line_sensor.c b/frtos/line_sensor/line_sensor.c new file mode 100644 index 0000000..53dbcfd --- /dev/null +++ b/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(); + +} \ No newline at end of file diff --git a/frtos/line_sensor/line_sensor.h b/frtos/line_sensor/line_sensor.h new file mode 100644 index 0000000..36d7d6b --- /dev/null +++ b/frtos/line_sensor/line_sensor.h @@ -0,0 +1,47 @@ +#ifndef LINE_SENSOR_H +#define LINE_SENSOR_H + +#include +#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 \ No newline at end of file