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

interrupt based speed monitoring

This commit is contained in:
Richie 2023-10-13 16:41:13 +08:00
parent c00531307f
commit aeb39084ab
3 changed files with 119 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
frtos/CMakeLists.txt
View File

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

12
frtos/car/rtos_car.c
View File

@ -25,6 +25,18 @@
void void
launch() launch()
{ {
// isr to detect left wheel slot
gpio_set_irq_enabled_with_callback(SPEED_PIN_RIGHT,
GPIO_IRQ_EDGE_FALL,
true,
&right_wheel_sensor_isr);
// isr to detect right wheel slot
gpio_set_irq_enabled_with_callback(SPEED_PIN_LEFT,
GPIO_IRQ_EDGE_FALL,
true,
&left_wheel_sensor_isr);
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_left_wheel_speed_task, xTaskCreate(monitor_left_wheel_speed_task,
"monitor_left_wheel_speed_task", "monitor_left_wheel_speed_task",

211
frtos/car/wheel.h
View File

@ -26,113 +26,25 @@
uint g_slice_num_left = 0U; uint g_slice_num_left = 0U;
uint g_slice_num_right = 0U; uint g_slice_num_right = 0U;
SemaphoreHandle_t g_wheel_speed_sem = NULL; SemaphoreHandle_t g_wheel_speed_sem_left = NULL;
SemaphoreHandle_t g_wheel_speed_sem_right = NULL;
/*!
* @brief Set the direction of the wheels; can use bitwise OR to set both
* wheels such as DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD, it will
* set the left wheel to go forward and the right wheel to go backward within
* the same function.
* if the wheel direction is not set, it will not move.
* @param direction The direction of the left and right wheels
* @param left_speed The speed of the left wheel, from 0.0 to 1.0
* @param right_speed The speed of the right wheel, from 0.0 to 1.0
*/
void
set_wheel_direction (uint32_t direction)
{
static const uint32_t mask = DIRECTION_LEFT_FORWARD |
DIRECTION_LEFT_BACKWARD |
DIRECTION_RIGHT_FORWARD |
DIRECTION_RIGHT_BACKWARD;
gpio_put_masked(mask, 0U);
gpio_set_mask(direction);
}
uint8_t
get_wheel_slot_count (uint adc_pin)
{
adc_select_input(adc_pin);
uint8_t count = 0u;
TickType_t xEndTime = xTaskGetTickCount() +
pdMS_TO_TICKS(SPEED_READING_TRESHOLD_MSEC);
static volatile bool b_is_adc_high = false;
while (xTaskGetTickCount() < xEndTime)
{
/*
* If the sensor value is below the threshold and the previous reading
* was above the threshold, then increment the count.
*/
if (b_is_adc_high && (adc_read() < ADC_READING_TRESHOLD))
{
count++;
b_is_adc_high = false;
}
else if (!b_is_adc_high && (adc_read() >= ADC_READING_TRESHOLD))
{
b_is_adc_high = true;
}
}
b_is_adc_high = false;
return count;
}
void
monitor_left_wheel_speed_task (__unused void * p_params)
{
for (;;)
{
xSemaphoreTake(g_wheel_speed_sem, portMAX_DELAY);
printf("left wheel: %d\n", get_wheel_slot_count(0));
xSemaphoreGive(g_wheel_speed_sem);
vTaskDelay(pdMS_TO_TICKS(10));
}
}
void
monitor_right_wheel_speed_task (__unused void * p_params)
{
static uint8_t count = 0u;
for (;;)
{
xSemaphoreTake(g_wheel_speed_sem, portMAX_DELAY);
count = get_wheel_slot_count(1);
printf("right wheel: %f rounds\n", (((float) count) / 20.f));
xSemaphoreGive(g_wheel_speed_sem);
vTaskDelay(pdMS_TO_TICKS(10));
}
}
void
set_wheel_speed (float speed)
{
pwm_set_chan_level(g_slice_num_left,
PWM_CHAN_A,
(short) (PWM_WRAP * speed));
pwm_set_chan_level(g_slice_num_right,
PWM_CHAN_B,
(short) (PWM_WRAP * speed));
}
void void
wheel_setup(void) wheel_setup(void)
{ {
// Semaphore // Semaphore
g_wheel_speed_sem = xSemaphoreCreateMutex(); g_wheel_speed_sem_left = xSemaphoreCreateBinary();
g_wheel_speed_sem_right = xSemaphoreCreateBinary();
// Speed // Speed
adc_init(); /* adc_init();
adc_gpio_init(SPEED_PIN_RIGHT); adc_gpio_init(SPEED_PIN_RIGHT);
adc_gpio_init(SPEED_PIN_LEFT); adc_gpio_init(SPEED_PIN_LEFT);*/
gpio_init(SPEED_PIN_RIGHT);
gpio_init(SPEED_PIN_LEFT);
gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN);
gpio_set_dir(SPEED_PIN_LEFT, GPIO_IN);
// Initialize direction pins as outputs // Initialize direction pins as outputs
gpio_init(DIRECTION_PIN_RIGHT_IN1); gpio_init(DIRECTION_PIN_RIGHT_IN1);
@ -165,3 +77,104 @@ wheel_setup(void)
pwm_set_enabled(g_slice_num_left, true); pwm_set_enabled(g_slice_num_left, true);
pwm_set_enabled(g_slice_num_right, true); pwm_set_enabled(g_slice_num_right, true);
} }
/*!
* @brief Set the direction of the wheels; can use bitwise OR to set both
* wheels such as DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD, it will
* set the left wheel to go forward and the right wheel to go backward within
* the same function.
* if the wheel direction is not set, it will not move.
* @param direction The direction of the left and right wheels
* @param left_speed The speed of the left wheel, from 0.0 to 1.0
* @param right_speed The speed of the right wheel, from 0.0 to 1.0
*/
void
set_wheel_direction (uint32_t direction)
{
static const uint32_t mask = DIRECTION_LEFT_FORWARD |
DIRECTION_LEFT_BACKWARD |
DIRECTION_RIGHT_FORWARD |
DIRECTION_RIGHT_BACKWARD;
gpio_put_masked(mask, 0U);
gpio_set_mask(direction);
}
void
set_wheel_speed (float speed)
{
pwm_set_chan_level(g_slice_num_left,
PWM_CHAN_A,
(short) (PWM_WRAP * speed));
pwm_set_chan_level(g_slice_num_right,
PWM_CHAN_B,
(short) (PWM_WRAP * speed));
}
void
left_wheel_sensor_isr (__unused uint gpio, __unused uint32_t events)
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
void
right_wheel_sensor_isr (__unused uint gpio, __unused uint32_t events)
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
void
monitor_left_wheel_speed_task (__unused void *pvParameters)
{
for (;;)
{
if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE)
{
static uint64_t curr_time_left = 0u;
curr_time_left = time_us_64();
static uint64_t prev_time_left = 0u;
static uint64_t elapsed_time_left = 0u;
elapsed_time_left = curr_time_left - prev_time_left;
printf("time elapsed: %llu\n", elapsed_time_left);
prev_time_left = curr_time_left;
}
}
}
void
monitor_right_wheel_speed_task (__unused void *pvParameters)
{
for (;;)
{
if (xSemaphoreTake(g_wheel_speed_sem_right, portMAX_DELAY) == pdTRUE)
{
static uint64_t curr_time_right = 0u;
curr_time_right = time_us_64();
static uint64_t prev_time_right = 0u;
static uint64_t elapsed_time_right = 0u;
elapsed_time_right = curr_time_right - prev_time_right;
printf("time elapsed: %llu\n", elapsed_time_right);
prev_time_right = curr_time_right;
}
}
}