diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 2646ae2..56ed759 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -32,4 +32,18 @@ #define MAX_SPEED 4900U #define MIN_SPEED 0U // To be changed +/*! + * @brief Structure for the motor speed + * @param side The side of the motor, 0 for left, 1 for right + * @param target_speed The target speed of the motor in cm/s + * @param pwm_level The current pwm level of the motor, in range [0, 5000] + */ +typedef struct { + float target_speed; + uint16_t pwm_level; + SemaphoreHandle_t * sem; + uint * p_slice_num; + uint channel; +} motor_speed_t; + #endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 3d4db12..6fe9adf 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -9,24 +9,6 @@ #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 - */ -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); - } -} - /*! * @brief Interrupt handler for the wheel sensor */ @@ -63,12 +45,12 @@ h_wheel_sensor_isr_handler(void) * @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; @@ -81,47 +63,56 @@ 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 volatile float speed_left = 0.f; - static volatile uint64_t curr_time_left = 0u; - curr_time_left = time_us_64(); + volatile motor_speed_t *p_motor_speed = NULL; + p_motor_speed = (motor_speed_t *)pvParameters; + SemaphoreHandle_t *p_sem = p_motor_speed->sem; + + 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, pdMS_TO_TICKS(1000)) == + if (xSemaphoreTake(*p_sem, pdMS_TO_TICKS(1000)) == pdTRUE) { - 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; + curr_time = time_us_64(); + elapsed_time = curr_time - prev_time; + prev_time = curr_time; // 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); + printf("speed: %f cm/s\n", speed); } else { - printf("left speed: 0 cm/s\n"); + printf("speed: 0 cm/s\n"); } - 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; + control_signal = compute_pid(&(p_motor_speed->target_speed), + &speed, + &integral, + &prev_error); if (new_pwm + control_signal > MAX_SPEED) { @@ -139,66 +130,8 @@ monitor_left_wheel_speed_task(void *pvParameters) printf("control signal: %f\n", control_signal); printf("new pwm: %f\n\n", new_pwm); - set_wheel_speed(new_pwm, 0u); - } -} - -void -monitor_right_wheel_speed_task(void *pvParameters) -{ - static volatile float speed_right = 0.f; - static volatile uint64_t curr_time_right = 0u; - curr_time_right = time_us_64(); - - for (;;) - { - if (xSemaphoreTake(g_wheel_speed_sem_right, pdMS_TO_TICKS(1000)) == - pdTRUE) - { - 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; - - speed_right - = (float)(1.02101761242f / (elapsed_time_right / 1000000.f)); - - printf("right speed: %f cm/s\n", speed_right); - } - else - { - curr_time_right = time_us_64(); - printf("right speed: 0 cm/s\n"); - } - - 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); + pwm_set_chan_level(*p_motor_speed->p_slice_num, + p_motor_speed->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 4a5e236..9762ffb 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -19,23 +19,39 @@ launch() irq_set_enabled(IO_IRQ_BANK0, true); - static volatile float * p_target_speed = NULL; - static volatile float target_speed = 30.0f; // cm/s - p_target_speed = &target_speed; + static volatile motor_speed_t * p_motor_speed_left = NULL; + static volatile motor_speed_t motor_speed_left = { + .target_speed = 40.0f, + .pwm_level = 0u, + .sem = &g_wheel_speed_sem_left, + .p_slice_num = &g_slice_num_left, + .channel = PWM_CHAN_A + }; + p_motor_speed_left = &motor_speed_left; + + static volatile motor_speed_t * p_motor_speed_right = NULL; + static volatile motor_speed_t motor_speed_right = { + .target_speed = 20.0f, + .pwm_level = 0u, + .sem = &g_wheel_speed_sem_right, + .p_slice_num = &g_slice_num_right, + .channel = PWM_CHAN_B + }; + p_motor_speed_right = &motor_speed_right; TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; - xTaskCreate(monitor_left_wheel_speed_task, + xTaskCreate(monitor_wheel_speed_task, "monitor_left_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *) p_target_speed, + (void *) p_motor_speed_left, 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", + xTaskCreate(monitor_wheel_speed_task, + "monitor_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *) p_target_speed, + (void *)p_motor_speed_right, READ_RIGHT_WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 56453dd..4dd32b7 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -21,36 +21,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_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_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); vTaskStartScheduler(); } @@ -60,10 +30,6 @@ main (void) { stdio_usb_init(); - motor_init(); - - set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); - launch(); return (0);