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

Merge remote-tracking branch 'origin/main'

This commit is contained in:
Devoalda 2023-10-14 09:10:39 +08:00
parent 85911aea8e 376453398a
commit 6d717120b9
3 changed files with 210 additions and 94 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
)

24
frtos/car/rtos_car.c
View File

@ -25,11 +25,27 @@
void void
launch() launch()
{ {
// isr to detect right wheel slot
gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_RIGHT,
h_right_wheel_sensor_isr_handler);
// isr to detect left wheel slot
gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_LEFT,
h_left_wheel_sensor_isr_handler);
irq_set_enabled(IO_IRQ_BANK0, true);
static volatile float * p_target_speed = NULL;
static volatile float target_speed = 20.0f; // cm/s
p_target_speed = &target_speed;
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",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, (void *) p_target_speed,
READ_LEFT_WHEEL_SPEED_PRIO, READ_LEFT_WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle); &h_monitor_left_wheel_speed_task_handle);
@ -37,7 +53,7 @@ launch()
xTaskCreate(monitor_right_wheel_speed_task, xTaskCreate(monitor_right_wheel_speed_task,
"monitor_right_wheel_speed_task", "monitor_right_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, (void *) p_target_speed,
READ_RIGHT_WHEEL_SPEED_PRIO, READ_RIGHT_WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle); &h_monitor_right_wheel_speed_task_handle);
@ -49,10 +65,10 @@ main (void)
{ {
stdio_usb_init(); stdio_usb_init();
wheel_setup(); wheel_setup();
sleep_ms(2000); sleep_ms(5000);
set_wheel_direction(DIRECTION_RIGHT_FORWARD); set_wheel_direction(DIRECTION_RIGHT_FORWARD);
set_wheel_speed(1.f); set_wheel_speed(START_SPEED, 1u);
launch(); launch();

312
frtos/car/wheel.h
View File

@ -13,126 +13,37 @@
#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) #define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4)
#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) #define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3)
#define SPEED_PIN_RIGHT 27U // ADC0 #define SPEED_PIN_RIGHT 15U
#define SPEED_PIN_LEFT 26U // ADC1 #define SPEED_PIN_LEFT 16U
#define PWM_CLK_DIV 250.f #define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U #define PWM_WRAP 5000U
#define ADC_READING_TRESHOLD 3500u #define PID_KP 10.f
#define PID_KI 0.0f
#define PID_KD 0.0f
#define SPEED_READING_TRESHOLD_MSEC 1000u #define START_SPEED 1500U
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
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 gpio_init(SPEED_PIN_RIGHT);
adc_init(); gpio_init(SPEED_PIN_LEFT);
adc_gpio_init(SPEED_PIN_RIGHT); gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN);
adc_gpio_init(SPEED_PIN_LEFT); 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 +76,194 @@ 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);
}
/*!
* @brief Set the speed of the wheels; can use bitwise OR to set both
* @param speed in range [0, 5000]
* @param side 0 for left, 1 for right
*/
void
set_wheel_speed (float speed, uint8_t side)
{
if (side == 0U)
{
pwm_set_chan_level(g_slice_num_left,
PWM_CHAN_A,
(uint16_t) speed);
}
else
{
pwm_set_chan_level(g_slice_num_right,
PWM_CHAN_B,
(uint16_t) speed);
}
}
void
h_left_wheel_sensor_isr_handler (void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
// printf("left wheel sensor isr\n");
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void
h_right_wheel_sensor_isr_handler (void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_RIGHT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
// printf("right wheel sensor isr\n");
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
float
compute_pid(float target_speed, float current_speed, float * integral, float * prev_error)
{
float error = target_speed - current_speed;
*integral += error;
float derivative = error - *prev_error;
float control_signal = PID_KP * error +
PID_KI * (*integral) +
PID_KD * derivative;
*prev_error = error;
return control_signal;
}
void
monitor_left_wheel_speed_task (void *pvParameters)
{
static float * target_speed = NULL;
*target_speed = * (float *) 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 = 1u; // to avoid division by 0
elapsed_time_left = curr_time_left - prev_time_left;
prev_time_left = curr_time_left;
static float speed_left = 0.f;
// speed in cm/s; speed = distance / time
// distance = circumference / 20
// circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
// distance = 20.4203522483 cm / 20 = 1.02101761242 cm
speed_left = (float)
(1.02101761242f / (elapsed_time_left / 1000000.f));
printf("left speed: %f cm/s\n", speed_left);
}
}
}
void
monitor_right_wheel_speed_task (void *pvParameters)
{
// volatile float * target_speed = (float *) pvParameters;
static volatile float * target_speed = NULL;
target_speed = (float *) 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 = 1u; // to avoid division by 0
elapsed_time_right = curr_time_right - prev_time_right;
prev_time_right = curr_time_right;
static float speed_right = 0.f;
speed_right = (float)
(1.02101761242f / (elapsed_time_right / 1000000.f));
printf("right speed: %f cm/s\n", speed_right);
static float control_signal = 0.f;
static float integral = 0.f;
static float prev_error = 0.f;
control_signal = compute_pid(*target_speed,
speed_right,
&integral,
&prev_error);
static float new_pwm = START_SPEED;
if (new_pwm + control_signal > MAX_SPEED)
{
new_pwm = MAX_SPEED;
}
else if (new_pwm + control_signal < MIN_SPEED)
{
new_pwm = MIN_SPEED;
}
else
{
new_pwm = new_pwm + control_signal;
}
printf("control signal: %f\n", control_signal);
printf("new pwm: %f\n\n", new_pwm);
set_wheel_speed(new_pwm, 1u);
}
}
}