devoalda
/
INF2004_Project
mirror of https://github.com/L4ncelot-R/freeRTOS-car.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feature(Line Sensor): Line Sensor Modified to timer interrupts. Including Direction + Orientation Change handlers

This commit is contained in:
Devoalda 2023-10-14 09:10:30 +08:00
parent 7c5b7e819e
commit 85911aea8e
6 changed files with 440 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
frtos/line_sensor/CMakeLists.txt
View File

@ -6,7 +6,9 @@ add_library(line_sensor
target_link_libraries(line_sensor
hardware_adc
pico_stdlib)
pico_stdlib
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
)
target_include_directories(line_sensor
PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}")

4
frtos/line_sensor/Config.h
View File

@ -6,8 +6,8 @@
#define ADC_READING_DELAY_MS ( 300 )
#define ADC_PIN_ONE ( 26 )
#define ADC_PIN_TWO ( 27 ) // Comment this line out
#define LEFT_SENSOR_PIN ( 26 )
#define RIGHT_SENSOR_PIN ( 27 ) // Comment this line out
// if you only have one
#define THRESHOLD ( VREF / 2 ) // 50% of VREF

143
frtos/line_sensor/FreeRTOSConfig.h Normal file
View File

@ -0,0 +1,143 @@
/*
* FreeRTOS V202111.00
* Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H
/*-----------------------------------------------------------
* Application specific definitions.
*
* These definitions should be adjusted for your particular hardware and
* application requirements.
*
* THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
* FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
*
* See http://www.freertos.org/a00110.html
*----------------------------------------------------------*/
/* Scheduler Related */
#define configUSE_PREEMPTION 1
#define configUSE_TICKLESS_IDLE 0
#define configUSE_IDLE_HOOK 0
#define configUSE_TICK_HOOK 0
#define configTICK_RATE_HZ ( ( TickType_t ) 1000 )
#define configMAX_PRIORITIES 32
#define configMINIMAL_STACK_SIZE ( configSTACK_DEPTH_TYPE ) 256
#define configUSE_16_BIT_TICKS 0
#define configIDLE_SHOULD_YIELD 1
/* Synchronization Related */
#define configUSE_MUTEXES 1
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_APPLICATION_TASK_TAG 0
#define configUSE_COUNTING_SEMAPHORES 1
#define configQUEUE_REGISTRY_SIZE 8
#define configUSE_QUEUE_SETS 1
#define configUSE_TIME_SLICING 1
#define configUSE_NEWLIB_REENTRANT 0
// todo need this for lwip FreeRTOS sys_arch to compile
#define configENABLE_BACKWARD_COMPATIBILITY 1
#define configNUM_THREAD_LOCAL_STORAGE_POINTERS 5
/* System */
#define configSTACK_DEPTH_TYPE uint32_t
#define configMESSAGE_BUFFER_LENGTH_TYPE size_t
/* Memory allocation related definitions. */
#define configSUPPORT_STATIC_ALLOCATION 0
#define configSUPPORT_DYNAMIC_ALLOCATION 1
#define configTOTAL_HEAP_SIZE (128*1024)
#define configAPPLICATION_ALLOCATED_HEAP 0
/* Hook function related definitions. */
#define configCHECK_FOR_STACK_OVERFLOW 0
#define configUSE_MALLOC_FAILED_HOOK 0
#define configUSE_DAEMON_TASK_STARTUP_HOOK 0
/* Run time and task stats gathering related definitions. */
#define configGENERATE_RUN_TIME_STATS 0
#define configUSE_TRACE_FACILITY 1
#define configUSE_STATS_FORMATTING_FUNCTIONS 0
/* Co-routine related definitions. */
#define configUSE_CO_ROUTINES 0
#define configMAX_CO_ROUTINE_PRIORITIES 1
/* Software timer related definitions. */
#define configUSE_TIMERS 1
#define configTIMER_TASK_PRIORITY ( configMAX_PRIORITIES - 1 )
#define configTIMER_QUEUE_LENGTH 10
#define configTIMER_TASK_STACK_DEPTH 1024
/* Interrupt nesting behaviour configuration. */
/*
#define configKERNEL_INTERRUPT_PRIORITY [dependent of processor]
#define configMAX_SYSCALL_INTERRUPT_PRIORITY [dependent on processor and application]
#define configMAX_API_CALL_INTERRUPT_PRIORITY [dependent on processor and application]
*/
#if FREE_RTOS_KERNEL_SMP // set by the RP2040 SMP port of FreeRTOS
/* SMP port only */
#define configNUM_CORES 1
#define configTICK_CORE 0
#define configRUN_MULTIPLE_PRIORITIES 1
#define configUSE_CORE_AFFINITY 0
#endif
/* RP2040 specific */
#define configSUPPORT_PICO_SYNC_INTEROP 1
#define configSUPPORT_PICO_TIME_INTEROP 1
#include <assert.h>
/* Define to trap errors during development. */
#define configASSERT(x) assert(x)
/* Set the following definitions to 1 to include the API function, or zero
to exclude the API function. */
#define INCLUDE_vTaskPrioritySet 1
#define INCLUDE_uxTaskPriorityGet 1
#define INCLUDE_vTaskDelete 1
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskDelayUntil 1
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_xTaskGetCurrentTaskHandle 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
#define INCLUDE_xTaskGetIdleTaskHandle 1
#define INCLUDE_eTaskGetState 1
#define INCLUDE_xTimerPendFunctionCall 1
#define INCLUDE_xTaskAbortDelay 1
#define INCLUDE_xTaskGetHandle 1
#define INCLUDE_xTaskResumeFromISR 1
#define INCLUDE_xQueueGetMutexHolder 1
/* A header file that defines trace macro can be included here. */
#endif /* FREERTOS_CONFIG_H */

131
frtos/line_sensor/line_sensor.c
View File

@ -1,89 +1,20 @@
#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);
//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
@ -167,43 +98,3 @@ print_map() {
}
}
/**
* @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();
}

272
frtos/line_sensor/line_sensor.h
View File

@ -1,47 +1,283 @@
#ifndef LINE_SENSOR_H
#define LINE_SENSOR_H
#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/adc.h"
#include "FreeRTOS.h"
#include "task.h"
#include "message_buffer.h"
#include "semphr.h"
#include "Config.h"
typedef enum
{
LINE_DETECTED = 1,
LINE_NOT_DETECTED = 0,
typedef enum { // Unused, useful for readability
LINE_DETECTED = 0,
LINE_NOT_DETECTED = 1,
} state_t;
typedef enum
{
typedef enum {
ERROR = 0,
RIGHT = 1,
LEFT = 2,
FORWARD = 3
} direction_t;
typedef enum
{
typedef enum {
NORTH = 0,
EAST = 1,
SOUTH = 2,
WEST = 3,
} orientation_t;
typedef struct
{
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();
// Semaphore
SemaphoreHandle_t g_left_sensor_sem = NULL;
SemaphoreHandle_t g_right_sensor_sem = NULL;
static inline state_t get_state(uint gpio);
static inline direction_t get_current_dir();
// Queue
static MessageBufferHandle_t left_sensor_msg_buffer; // Left Sensor Buffer
static MessageBufferHandle_t right_sensor_msg_buffer; // Right Sensor Buffer
void line_sensor_task(__unused void *params);
// Car State Struct
static car_state_t g_car_state;
#endif
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;
}
/**
* @brief Setup the Line Sensor
*
* This function will setup the Line Sensor by initializing it as an input
*/
static inline void
line_sensor_setup() {
g_left_sensor_sem = xSemaphoreCreateBinary();
g_right_sensor_sem = xSemaphoreCreateBinary();
// Setup GPIO Interrupts
uint mask = (1 << LEFT_SENSOR_PIN) | (1 << RIGHT_SENSOR_PIN);
// Initialise 2 GPIO pins and set them to input
gpio_init_mask(mask);
gpio_set_dir_in_masked(mask);
left_sensor_msg_buffer = xMessageBufferCreate(30);
right_sensor_msg_buffer = xMessageBufferCreate(30);
}
/**
* @brief Monitor the left sensor
*
* This function will monitor the left sensor and send the state to the
* left sensor message buffer, used to calculate the direction of the car
*
* @param params
*/
void
monitor_left_sensor_task(__unused void *params) {
for (;;)
{
if (xSemaphoreTake(g_left_sensor_sem, portMAX_DELAY) == pdTRUE)
{
// Get Current State
state_t state = gpio_get(LEFT_SENSOR_PIN);
xMessageBufferSend(left_sensor_msg_buffer,
&state,
sizeof(state_t),
0);
}
}
}
/**
* @brief Monitor the right sensor
*
* This function will monitor the right sensor and send the state to the
* right sensor message buffer, used to calculate the direction of the car
*
* @param params
*/
void
monitor_right_sensor_task(__unused void *params) {
for (;;)
{
if (xSemaphoreTake(g_right_sensor_sem, portMAX_DELAY) == pdTRUE)
{
// Get Current State
state_t state = gpio_get(RIGHT_SENSOR_PIN);
xMessageBufferSend(right_sensor_msg_buffer,
&state,
sizeof(state_t),
0);
}
}
}
/**
* @brief Monitor the direction and Oritentation of the car
*
* This function will monitor the direction and orientation of the car
* and update the car state accordingly
*
* @param params
*/
void
monitor_direction_task(__unused void *params) {
state_t left_state;
state_t right_state;
for (;;)
{
// Receive from Buffer
xMessageBufferReceive(left_sensor_msg_buffer,
&left_state,
sizeof(state_t),
portMAX_DELAY);
xMessageBufferReceive(right_sensor_msg_buffer,
&right_state,
sizeof(state_t),
portMAX_DELAY);
g_car_state.current_direction = (left_state << 1) | right_state;
switch (g_car_state.current_direction)
{
case FORWARD:
break;
case RIGHT:
g_car_state.orientation = (g_car_state.orientation + 1) & 0x03;
break;
case LEFT:
g_car_state.orientation = (g_car_state.orientation - 1) & 0x03;
break;
default:
break;
}
switch (g_car_state.current_direction)
{
case FORWARD:
printf("Direction: Forward\n");
break;
case RIGHT:
printf("Direction: Right\n");
break;
case LEFT:
printf("Direction: Left\n");
break;
default:
printf("Direction: Error\n");
break;
}
switch (g_car_state.orientation)
{
case NORTH:
printf("Orientation: North\n");
break;
case EAST:
printf("Orientation: East\n");
break;
case SOUTH:
printf("Orientation: South\n");
break;
case WEST:
printf("Orientation: West\n");
break;
default:
printf("Orientation: Error\n");
break;
}
}
}
/**
* @brief Timer Interrupt Handler for the left sensor
* @param rt
* @return True (To keep the timer running)
*/
bool h_left_sensor_timer_handler(repeating_timer_t *repeatingTimer) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_left_sensor_sem,
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
return true;
}
/**
* @brief Timer Interrupt Handler for the right sensor
*
* @param repeatingTimer
* @return True (To keep the timer running)
*/
bool h_right_sensor_timer_handler(repeating_timer_t *repeatingTimer) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_right_sensor_sem,
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
return true;
}
/**
* Main Launch body
* This is to be in the main launch function for FreeRTOS
*
* // Repeating timer
* struct repeating_timer g_left_sensor_timer;
* add_repeating_timer_ms(100,
* h_left_sensor_timer_handler,
* NULL,
* &g_left_sensor_timer);
* struct repeating_timer g_right_sensor_timer;
* add_repeating_timer_ms(100,
* h_right_sensor_timer_handler,
* NULL,
* &g_right_sensor_timer);
* TaskHandle_t h_monitor_left_sensor_task;
* xTaskCreate(monitor_left_sensor_task,
* "Monitor Left Sensor Task",
* configMINIMAL_STACK_SIZE,
* NULL,
* LEFT_TASK_PRIORITY,
* &h_monitor_left_sensor_task);
* TaskHandle_t h_monitor_right_sensor_task;
* xTaskCreate(monitor_right_sensor_task,
* "Monitor Right Sensor Task",
* configMINIMAL_STACK_SIZE,
* NULL,
* RIGHT_TASK_PRIORITY,
* &h_monitor_right_sensor_task);
* TaskHandle_t h_monitor_direction_task;
* xTaskCreate(monitor_direction_task,
* "Monitor Direction Task",
* configMINIMAL_STACK_SIZE,
* NULL,
* DIRECTION_TASK_PRIORITY,
* &h_monitor_direction_task);
*/

25
frtos/line_sensor/lwipopts.h Normal file
View File

@ -0,0 +1,25 @@
#ifndef _LWIPOPTS_H
#define _LWIPOPTS_H
// Generally you would define your own explicit list of lwIP options
// (see https://www.nongnu.org/lwip/2_1_x/group__lwip__opts.html)
//
// This example uses a common include to avoid repetition
#include "lwipopts_examples_common.h"
#if !NO_SYS
#define TCPIP_THREAD_STACKSIZE 1024
#define DEFAULT_THREAD_STACKSIZE 1024
#define DEFAULT_RAW_RECVMBOX_SIZE 8
#define TCPIP_MBOX_SIZE 8
#define LWIP_TIMEVAL_PRIVATE 0
// not necessary, can be done either way
#define LWIP_TCPIP_CORE_LOCKING_INPUT 1
// ping_thread sets socket receive timeout, so enable this feature
#define LWIP_SO_RCVTIMEO 1
#endif
#endif