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();

274
frtos/car/wheel.h
View File

@ -13,20 +13,70 @@
#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;
void
wheel_setup(void)
{
// Semaphore
g_wheel_speed_sem_left = xSemaphoreCreateBinary();
g_wheel_speed_sem_right = xSemaphoreCreateBinary();
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
gpio_init(DIRECTION_PIN_RIGHT_IN1);
gpio_init(DIRECTION_PIN_RIGHT_IN2);
gpio_init(DIRECTION_PIN_LEFT_IN3);
gpio_init(DIRECTION_PIN_LEFT_IN4);
gpio_set_dir(DIRECTION_PIN_RIGHT_IN1, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_RIGHT_IN2, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_LEFT_IN3, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_LEFT_IN4, GPIO_OUT);
// Initialise PWM
gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM);
gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM);
g_slice_num_left = pwm_gpio_to_slice_num(PWM_PIN_LEFT);
g_slice_num_right = pwm_gpio_to_slice_num(PWM_PIN_RIGHT);
// NOTE: PWM clock is 125MHz for raspberrypi pico w by default
// 125MHz / 250 = 500kHz
pwm_set_clkdiv(g_slice_num_left, PWM_CLK_DIV);
pwm_set_clkdiv(g_slice_num_right, PWM_CLK_DIV);
// have them to be 500kHz / 5000 = 100Hz
pwm_set_wrap(g_slice_num_left, (PWM_WRAP - 1U));
pwm_set_wrap(g_slice_num_right, (PWM_WRAP - 1U));
pwm_set_enabled(g_slice_num_left, true);
pwm_set_enabled(g_slice_num_right, true);
}
/*! /*!
* @brief Set the direction of the wheels; can use bitwise OR to set both * @brief Set the direction of the wheels; can use bitwise OR to set both
@ -50,118 +100,170 @@ set_wheel_direction (uint32_t direction)
gpio_set_mask(direction); gpio_set_mask(direction);
} }
uint8_t /*!
get_wheel_slot_count (uint adc_pin) * @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)
{ {
adc_select_input(adc_pin); if (side == 0U)
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)
{ {
/* pwm_set_chan_level(g_slice_num_left,
* If the sensor value is below the threshold and the previous reading PWM_CHAN_A,
* was above the threshold, then increment the count. (uint16_t) speed);
*/ }
if (b_is_adc_high && (adc_read() < ADC_READING_TRESHOLD)) else
{ {
count++; pwm_set_chan_level(g_slice_num_right,
b_is_adc_high = false; PWM_CHAN_B,
} (uint16_t) speed);
else if (!b_is_adc_high && (adc_read() >= ADC_READING_TRESHOLD))
{
b_is_adc_high = true;
}
} }
b_is_adc_high = false;
return count;
} }
void void
monitor_left_wheel_speed_task (__unused void * p_params) h_left_wheel_sensor_isr_handler (void)
{ {
for (;;) if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL)
{ {
xSemaphoreTake(g_wheel_speed_sem, portMAX_DELAY); gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
printf("left wheel: %d\n", get_wheel_slot_count(0));
xSemaphoreGive(g_wheel_speed_sem); // printf("left wheel sensor isr\n");
vTaskDelay(pdMS_TO_TICKS(10)); BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
} }
} }
void void
monitor_right_wheel_speed_task (__unused void * p_params) h_right_wheel_sensor_isr_handler (void)
{ {
static uint8_t count = 0u; 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 (;;) for (;;)
{ {
xSemaphoreTake(g_wheel_speed_sem, portMAX_DELAY); if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE)
count = get_wheel_slot_count(1); {
printf("right wheel: %f rounds\n", (((float) count) / 20.f)); static uint64_t curr_time_left = 0u;
xSemaphoreGive(g_wheel_speed_sem); curr_time_left = time_us_64();
vTaskDelay(pdMS_TO_TICKS(10));
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 void
set_wheel_speed (float speed) monitor_right_wheel_speed_task (void *pvParameters)
{ {
pwm_set_chan_level(g_slice_num_left, // volatile float * target_speed = (float *) pvParameters;
PWM_CHAN_A, static volatile float * target_speed = NULL;
(short) (PWM_WRAP * speed)); target_speed = (float *) pvParameters;
pwm_set_chan_level(g_slice_num_right, for (;;)
PWM_CHAN_B, {
(short) (PWM_WRAP * speed)); if (xSemaphoreTake(g_wheel_speed_sem_right, portMAX_DELAY) == pdTRUE)
} {
static uint64_t curr_time_right = 0u;
curr_time_right = time_us_64();
void static uint64_t prev_time_right = 0u;
wheel_setup(void) static uint64_t elapsed_time_right = 1u; // to avoid division by 0
{
// Semaphore
g_wheel_speed_sem = xSemaphoreCreateMutex();
// Speed elapsed_time_right = curr_time_right - prev_time_right;
adc_init();
adc_gpio_init(SPEED_PIN_RIGHT);
adc_gpio_init(SPEED_PIN_LEFT);
// Initialize direction pins as outputs prev_time_right = curr_time_right;
gpio_init(DIRECTION_PIN_RIGHT_IN1);
gpio_init(DIRECTION_PIN_RIGHT_IN2);
gpio_init(DIRECTION_PIN_LEFT_IN3);
gpio_init(DIRECTION_PIN_LEFT_IN4);
gpio_set_dir(DIRECTION_PIN_RIGHT_IN1, GPIO_OUT); static float speed_right = 0.f;
gpio_set_dir(DIRECTION_PIN_RIGHT_IN2, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_LEFT_IN3, GPIO_OUT);
gpio_set_dir(DIRECTION_PIN_LEFT_IN4, GPIO_OUT);
// Initialise PWM speed_right = (float)
gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM); (1.02101761242f / (elapsed_time_right / 1000000.f));
gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM);
g_slice_num_left = pwm_gpio_to_slice_num(PWM_PIN_LEFT); printf("right speed: %f cm/s\n", speed_right);
g_slice_num_right = pwm_gpio_to_slice_num(PWM_PIN_RIGHT);
// NOTE: PWM clock is 125MHz for raspberrypi pico w by default static float control_signal = 0.f;
static float integral = 0.f;
static float prev_error = 0.f;
// 125MHz / 250 = 500kHz control_signal = compute_pid(*target_speed,
pwm_set_clkdiv(g_slice_num_left, PWM_CLK_DIV); speed_right,
pwm_set_clkdiv(g_slice_num_right, PWM_CLK_DIV); &integral,
&prev_error);
// have them to be 500kHz / 5000 = 100Hz static float new_pwm = START_SPEED;
pwm_set_wrap(g_slice_num_left, (PWM_WRAP - 1U));
pwm_set_wrap(g_slice_num_right, (PWM_WRAP - 1U)); 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);
}
}
pwm_set_enabled(g_slice_num_left, true);
pwm_set_enabled(g_slice_num_right, true);
} }