Merge pull request #3 from Devoalda/main
Updated Line Sensor files + test
This commit is contained in:
Richie Wang
GitHub
commit
b8850d80fb
|
|
@ -2,6 +2,7 @@ include(FreeRTOS_Kernel_import.cmake)
|
||||||
|
|
||||||
add_subdirectory(motor)
|
add_subdirectory(motor)
|
||||||
add_subdirectory(line_sensor)
|
add_subdirectory(line_sensor)
|
||||||
|
add_subdirectory(car)
|
||||||
|
|
||||||
add_executable(rtos_car rtos_car.c)
|
add_executable(rtos_car rtos_car.c)
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,15 @@
|
||||||
|
add_library(
|
||||||
|
car
|
||||||
|
car.h
|
||||||
|
)
|
||||||
|
|
||||||
|
target_link_libraries(
|
||||||
|
car
|
||||||
|
hardware_adc
|
||||||
|
pico_stdlib
|
||||||
|
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
|
||||||
|
)
|
||||||
|
|
||||||
|
target_include_directories(car
|
||||||
|
PRIVATE ../config
|
||||||
|
)
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
/**
|
||||||
|
* @brief Common Car Functionalities for the RTOS Car
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef CAR_H
|
||||||
|
#define CAR_H
|
||||||
|
|
||||||
|
#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 */
|
||||||
|
|
@ -0,0 +1,44 @@
|
||||||
|
/**
|
||||||
|
* @brief Car Init
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef CAR_INIT_H
|
||||||
|
#define CAR_INIT_H
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#include "pico/stdlib.h"
|
||||||
|
|
||||||
|
#include "FreeRTOS.h"
|
||||||
|
#include "task.h"
|
||||||
|
|
||||||
|
#include "line_sensor_config.h"
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
/* Common Car State Structure (TODO: TBC)*/
|
||||||
|
|
||||||
|
static car_state_t g_car_state;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif /* CAR_INIT_H */
|
||||||
|
|
@ -0,0 +1,16 @@
|
||||||
|
#ifndef CONFIG_H
|
||||||
|
#define CONFIG_H
|
||||||
|
|
||||||
|
/* ADC Configuration */
|
||||||
|
|
||||||
|
#define LINE_SENSOR_READ_DELAY ( 100 )
|
||||||
|
|
||||||
|
#define LEFT_SENSOR_PIN ( 26 )
|
||||||
|
#define RIGHT_SENSOR_PIN ( 27 )
|
||||||
|
|
||||||
|
|
||||||
|
/* Map */
|
||||||
|
#define MAP_START_SYMBOL ( 5 )
|
||||||
|
#define MAP_SIZE 20
|
||||||
|
|
||||||
|
#endif //CONFIG_H
|
||||||
|
|
@ -1,14 +1,18 @@
|
||||||
add_library(line_sensor
|
add_executable(
|
||||||
line_sensor.c
|
line_sensor_test
|
||||||
line_sensor.h
|
line_sensor_test.c
|
||||||
Config.h
|
|
||||||
)
|
)
|
||||||
|
|
||||||
target_link_libraries(line_sensor
|
target_link_libraries(
|
||||||
|
line_sensor_test
|
||||||
hardware_adc
|
hardware_adc
|
||||||
pico_stdlib
|
pico_stdlib
|
||||||
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
|
FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
|
||||||
)
|
)
|
||||||
|
|
||||||
target_include_directories(line_sensor
|
target_include_directories(line_sensor_test
|
||||||
PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}")
|
PRIVATE ../config
|
||||||
|
)
|
||||||
|
|
||||||
|
pico_enable_stdio_usb(line_sensor_test 1)
|
||||||
|
pico_add_extra_outputs(line_sensor_test)
|
||||||
|
|
@ -1,19 +0,0 @@
|
||||||
#ifndef CONFIG_H
|
|
||||||
#define CONFIG_H
|
|
||||||
|
|
||||||
/* ADC Configuration */
|
|
||||||
#define VREF ( 3.3f )
|
|
||||||
|
|
||||||
#define ADC_READING_DELAY_MS ( 300 )
|
|
||||||
|
|
||||||
#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
|
|
||||||
|
|
||||||
/* Map */
|
|
||||||
#define MAP_START_SYMBOL ( 5 )
|
|
||||||
#define MAP_SIZE 20
|
|
||||||
|
|
||||||
#endif //CONFIG_H
|
|
||||||
|
|
@ -1,143 +0,0 @@
|
||||||
/*
|
|
||||||
* 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 */
|
|
||||||
|
|
||||||
|
|
@ -1,83 +1,10 @@
|
||||||
#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 { // Unused, useful for readability
|
|
||||||
LINE_DETECTED = 0,
|
|
||||||
LINE_NOT_DETECTED = 1,
|
|
||||||
} 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;
|
|
||||||
|
|
||||||
// Semaphore
|
|
||||||
SemaphoreHandle_t g_left_sensor_sem = NULL;
|
|
||||||
SemaphoreHandle_t g_right_sensor_sem = NULL;
|
|
||||||
|
|
||||||
// Queue
|
|
||||||
static MessageBufferHandle_t left_sensor_msg_buffer; // Left Sensor Buffer
|
|
||||||
static MessageBufferHandle_t right_sensor_msg_buffer; // Right Sensor Buffer
|
|
||||||
|
|
||||||
// Car State Struct
|
|
||||||
static car_state_t g_car_state;
|
|
||||||
|
|
||||||
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
|
* @file line_sensor.h
|
||||||
*
|
* @brief Monitor the line sensor and update the car state accordingly
|
||||||
* This function will setup the Line Sensor by initializing it as an input
|
* @author Woon Jun Wei
|
||||||
*/
|
*/
|
||||||
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);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
#include "line_sensor_init.h"
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Monitor the left sensor
|
* @brief Monitor the left sensor
|
||||||
|
|
@ -207,77 +134,4 @@ monitor_direction_task(__unused void *params) {
|
||||||
break;
|
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);
|
|
||||||
*/
|
|
||||||
|
|
@ -0,0 +1,118 @@
|
||||||
|
/**
|
||||||
|
* @file line_sensor_init.h
|
||||||
|
* @brief Initialise the line sensor
|
||||||
|
* @author Woon Jun Wei
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef LINE_SENSOR_INIT_H
|
||||||
|
#define LINE_SENSOR_INIT_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 "line_sensor_config.h"
|
||||||
|
|
||||||
|
typedef enum { // Unused, useful for readability
|
||||||
|
LINE_DETECTED = 0,
|
||||||
|
LINE_NOT_DETECTED = 1,
|
||||||
|
} 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;
|
||||||
|
|
||||||
|
// Semaphore
|
||||||
|
SemaphoreHandle_t g_left_sensor_sem = NULL;
|
||||||
|
SemaphoreHandle_t g_right_sensor_sem = NULL;
|
||||||
|
|
||||||
|
// Queue
|
||||||
|
static MessageBufferHandle_t left_sensor_msg_buffer; // Left Sensor Buffer
|
||||||
|
static MessageBufferHandle_t right_sensor_msg_buffer; // Right Sensor Buffer
|
||||||
|
|
||||||
|
// Car State Struct
|
||||||
|
static car_state_t g_car_state;
|
||||||
|
|
||||||
|
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();
|
||||||
|
|
||||||
|
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 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;
|
||||||
|
}
|
||||||
|
#endif /* LINE_SENSOR_INIT_H */
|
||||||
|
|
@ -0,0 +1,65 @@
|
||||||
|
|
||||||
|
#include "line_sensor.h"
|
||||||
|
|
||||||
|
#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
|
||||||
|
#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
|
||||||
|
|
||||||
|
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
|
||||||
|
|
||||||
|
void
|
||||||
|
launch()
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
|
||||||
|
struct repeating_timer g_right_sensor_timer;
|
||||||
|
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
|
||||||
|
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,
|
||||||
|
READ_LEFT_SENSOR_PRIO,
|
||||||
|
&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,
|
||||||
|
READ_RIGHT_SENSOR_PRIO,
|
||||||
|
&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);
|
||||||
|
|
||||||
|
vTaskStartScheduler();
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
main (void)
|
||||||
|
{
|
||||||
|
stdio_usb_init();
|
||||||
|
|
||||||
|
sleep_ms(2000);
|
||||||
|
printf("Test started!\n");
|
||||||
|
|
||||||
|
line_sensor_setup();
|
||||||
|
initialize_car_state();
|
||||||
|
|
||||||
|
launch();
|
||||||
|
|
||||||
|
return (0);
|
||||||
|
}
|
||||||
|
|
@ -1,25 +0,0 @@
|
||||||
#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
|
|
||||||
|
|
@ -15,10 +15,59 @@
|
||||||
#include "motor_speed.h"
|
#include "motor_speed.h"
|
||||||
#include "motor_direction.h"
|
#include "motor_direction.h"
|
||||||
|
|
||||||
|
#include "line_sensor.h"
|
||||||
|
|
||||||
|
#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
|
||||||
|
#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
|
||||||
|
|
||||||
|
#define READ_LEFT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
|
||||||
|
#define READ_RIGHT_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
|
||||||
|
|
||||||
|
#define DIRECTION_TASK_PRIORITY (tskIDLE_PRIORITY + 3UL)
|
||||||
|
|
||||||
|
/* Common Car State Structure (TODO: TBC)*/
|
||||||
|
//static car_state_t g_car_state;
|
||||||
|
|
||||||
void
|
void
|
||||||
launch()
|
launch()
|
||||||
{
|
{
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
struct repeating_timer g_right_sensor_timer;
|
||||||
|
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
|
||||||
|
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,
|
||||||
|
READ_LEFT_SENSOR_PRIO,
|
||||||
|
&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,
|
||||||
|
READ_RIGHT_SENSOR_PRIO,
|
||||||
|
&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);
|
||||||
|
|
||||||
vTaskStartScheduler();
|
vTaskStartScheduler();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -27,6 +76,14 @@ main (void)
|
||||||
{
|
{
|
||||||
stdio_usb_init();
|
stdio_usb_init();
|
||||||
|
|
||||||
|
motor_init();
|
||||||
|
|
||||||
|
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
|
||||||
|
|
||||||
|
line_sensor_setup();
|
||||||
|
|
||||||
|
initialize_car_state(); // TODO: Could be common functionality, To confirm
|
||||||
|
// during Integration
|
||||||
launch();
|
launch();
|
||||||
|
|
||||||
return (0);
|
return (0);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue