diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 55c6646..80dd78b 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -3,9 +3,10 @@ #define MOTOR_CONFIG_H // ENA and ENB on the L298N -#define PWM_PIN_LEFT 0U // chanel A #define PWM_PIN_RIGHT 1U // chanel B +#define PWM_PIN_LEFT 0U // chanel A +// IN1, IN2, IN3, IN4 on the L298N #define DIRECTION_PIN_RIGHT_IN1 11U #define DIRECTION_PIN_RIGHT_IN2 12U @@ -23,12 +24,27 @@ #define PWM_CLK_DIV 250.f #define PWM_WRAP 5000U -#define PID_KP 10.f -#define PID_KI 0.0f -#define PID_KD 0.0f +#define PID_KP 3.f +#define PID_KI 0.01f +#define PID_KD 0.05f -#define START_SPEED 4900U #define MAX_SPEED 4900U #define MIN_SPEED 0U // To be changed +/*! + * @brief Structure for the motor speed + * @param target_speed The target speed of the wheel, in cm/s + * @param pwm_level The pwm level of the wheel, from 0 to 5000 + * @param sem The semaphore for the wheel + * @param p_slice_num The pointer to the slice number of the wheel + * @param channel The pwm channel of the wheel, left A or right B + */ +typedef struct { + float target_speed_cms; + uint16_t pwm_level; + SemaphoreHandle_t * p_sem; + uint * p_slice_num; + uint pwm_channel; +} motor_speed_t; + #endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index 0522b20..afcc1b9 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -25,6 +25,18 @@ uint g_slice_num_right = 0U; SemaphoreHandle_t g_wheel_speed_sem_left = NULL; SemaphoreHandle_t g_wheel_speed_sem_right = NULL; +motor_speed_t g_motor_speed_left = { .target_speed_cms = 0.0f, + .pwm_level = 2500u, + .p_sem = &g_wheel_speed_sem_left, + .p_slice_num = &g_slice_num_left, + .pwm_channel = PWM_CHAN_A }; + +motor_speed_t g_motor_speed_right = { .target_speed_cms = 0.0f, + .pwm_level = 2500u, + .p_sem = &g_wheel_speed_sem_right, + .p_slice_num = &g_slice_num_right, + .pwm_channel = PWM_CHAN_B }; + void motor_init(void) { @@ -67,7 +79,6 @@ motor_init(void) pwm_set_enabled(g_slice_num_left, true); pwm_set_enabled(g_slice_num_right, true); - } #endif /* MOTOR_INIT_H */ \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index d3a19df..db15a81 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -10,46 +10,25 @@ #include "motor_init.h" /*! - * @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 + * @brief Interrupt handler for the wheel sensor */ 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) +h_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 motor 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 motor sensor isr\n"); BaseType_t xHigherPriorityTaskWoken = pdFALSE; xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken); @@ -57,13 +36,21 @@ h_right_wheel_sensor_isr_handler(void) } } +/*! + * @brief Compute the control signal using PID controller + * @param target_speed The target speed of the wheel + * @param current_speed The current speed of the wheel + * @param integral The integral term of the PID controller + * @param prev_error The previous error of the PID controller + * @return The control signal + */ float -compute_pid(float target_speed, - float current_speed, - float *integral, - float *prev_error) +compute_pid(const volatile float *target_speed, + const float *current_speed, + float *integral, + float *prev_error) { - float error = target_speed - current_speed; + float error = *target_speed - *current_speed; *integral += error; float derivative = error - *prev_error; @@ -76,121 +63,73 @@ compute_pid(float target_speed, return control_signal; } +/*! + * @brief Task to monitor and control the speed of the wheel + * @param pvParameters motor_speed_t struct pointer + * @see motor_speed_t + */ void -monitor_left_wheel_speed_task(void *pvParameters) +monitor_wheel_speed_task(void *pvParameters) { - // static float * target_speed = NULL; - // *target_speed = * (float *) pvParameters; + volatile motor_speed_t *p_motor_speed = NULL; + p_motor_speed = (motor_speed_t *)pvParameters; + + float speed = 0.f; + + float new_pwm = p_motor_speed->pwm_level; + + uint64_t curr_time = 0u; + uint64_t prev_time = 0u; + uint64_t elapsed_time = 0u; + + float control_signal = 0.f; + float integral = 0.f; + float prev_error = 0.f; for (;;) { - if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE) + if (xSemaphoreTake(*p_motor_speed->p_sem, pdMS_TO_TICKS(100)) + == pdTRUE) { - static uint64_t curr_time_left = 0u; - curr_time_left = time_us_64(); + curr_time = time_us_64(); + elapsed_time = curr_time - prev_time; + prev_time = curr_time; - 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)); + speed = (float)(1.02101761242f / (elapsed_time / 1000000.f)); - printf("left speed: %f cm/s\n", speed_left); - - static float control_signal = 0.f; - static float integral = 0.f; - static float prev_error = 0.f; - - control_signal = compute_pid( - *(float *)pvParameters, speed_left, &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, 0u); + printf("speed: %f cm/s\n", speed); } - } -} - -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) + else { - 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( - *(float *)pvParameters, 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); + speed = 0.f; + printf("stopped\n"); } + + control_signal = compute_pid( + &(p_motor_speed->target_speed_cms), &speed, &integral, &prev_error); + + 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); + + pwm_set_chan_level(*p_motor_speed->p_slice_num, + p_motor_speed->pwm_channel, + (int16_t)new_pwm); } } \ No newline at end of file diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index fc802b9..5543ebb 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -3,49 +3,65 @@ #include "motor_speed.h" #include "motor_direction.h" -#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) -#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) +#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) + +void +test_speed_change_task(void *p_param) +{ + for (;;) + { + g_motor_speed_left.target_speed_cms = 20.0f; + g_motor_speed_right.target_speed_cms = 20.0f; + vTaskDelay(pdMS_TO_TICKS(10000)); + + g_motor_speed_left.target_speed_cms = 40.0f; + g_motor_speed_right.target_speed_cms = 40.0f; + vTaskDelay(pdMS_TO_TICKS(10000)); + } +} void launch() { // isr to detect right motor 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); + gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, h_wheel_sensor_isr_handler); // isr to detect left motor 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); + gpio_add_raw_irq_handler(SPEED_PIN_LEFT, h_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; - xTaskCreate(monitor_left_wheel_speed_task, - "monitor_left_wheel_speed_task", - configMINIMAL_STACK_SIZE, - (void *) p_target_speed, - READ_LEFT_WHEEL_SPEED_PRIO, - &h_monitor_left_wheel_speed_task_handle); +// TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; +// xTaskCreate(monitor_wheel_speed_task, +// "monitor_left_wheel_speed_task", +// configMINIMAL_STACK_SIZE, +// (void *)&g_motor_speed_left, +// WHEEL_SPEED_PRIO, +// &h_monitor_left_wheel_speed_task_handle); TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; - xTaskCreate(monitor_right_wheel_speed_task, - "monitor_right_wheel_speed_task", + xTaskCreate(monitor_wheel_speed_task, + "monitor_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *) p_target_speed, - READ_RIGHT_WHEEL_SPEED_PRIO, + (void *)&g_motor_speed_right, + WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); + TaskHandle_t h_test_speed_change_task_handle = NULL; + xTaskCreate(test_speed_change_task, + "test_speed_change_task", + configMINIMAL_STACK_SIZE, + NULL, + WHEEL_SPEED_PRIO, + &h_test_speed_change_task_handle); + vTaskStartScheduler(); } int -main (void) +main(void) { stdio_usb_init(); @@ -54,7 +70,6 @@ main (void) motor_init(); set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); - set_wheel_speed(START_SPEED, 0u); launch(); diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 26ea84e..f5f3549 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -31,37 +31,6 @@ void launch() { - // isr to detect right motor 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 motor 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; - xTaskCreate(monitor_left_wheel_speed_task, - "monitor_left_wheel_speed_task", - configMINIMAL_STACK_SIZE, - (void *) p_target_speed, - READ_LEFT_WHEEL_SPEED_PRIO, - &h_monitor_left_wheel_speed_task_handle); - - TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; - xTaskCreate(monitor_right_wheel_speed_task, - "monitor_right_wheel_speed_task", - configMINIMAL_STACK_SIZE, - (void *) p_target_speed, - READ_RIGHT_WHEEL_SPEED_PRIO, - &h_monitor_right_wheel_speed_task_handle); struct repeating_timer g_left_sensor_timer; add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,