diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 56ed759..80dd78b 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -24,26 +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 0U #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] + * @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; + float target_speed_cms; uint16_t pwm_level; - SemaphoreHandle_t * sem; + SemaphoreHandle_t * p_sem; uint * p_slice_num; - uint channel; + 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 6fe9adf..db15a81 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -45,10 +45,10 @@ h_wheel_sensor_isr_handler(void) * @return The control signal */ float -compute_pid(const volatile float * target_speed, - const 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; *integral += error; @@ -72,8 +72,7 @@ void monitor_wheel_speed_task(void *pvParameters) { volatile motor_speed_t *p_motor_speed = NULL; - p_motor_speed = (motor_speed_t *)pvParameters; - SemaphoreHandle_t *p_sem = p_motor_speed->sem; + p_motor_speed = (motor_speed_t *)pvParameters; float speed = 0.f; @@ -89,8 +88,8 @@ monitor_wheel_speed_task(void *pvParameters) for (;;) { - if (xSemaphoreTake(*p_sem, pdMS_TO_TICKS(1000)) == - pdTRUE) + if (xSemaphoreTake(*p_motor_speed->p_sem, pdMS_TO_TICKS(100)) + == pdTRUE) { curr_time = time_us_64(); elapsed_time = curr_time - prev_time; @@ -106,13 +105,12 @@ monitor_wheel_speed_task(void *pvParameters) } else { - printf("speed: 0 cm/s\n"); + speed = 0.f; + printf("stopped\n"); } - control_signal = compute_pid(&(p_motor_speed->target_speed), - &speed, - &integral, - &prev_error); + control_signal = compute_pid( + &(p_motor_speed->target_speed_cms), &speed, &integral, &prev_error); if (new_pwm + control_signal > MAX_SPEED) { @@ -131,7 +129,7 @@ monitor_wheel_speed_task(void *pvParameters) printf("new pwm: %f\n\n", new_pwm); pwm_set_chan_level(*p_motor_speed->p_slice_num, - p_motor_speed->channel, - (int16_t) new_pwm); + 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 9762ffb..5543ebb 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -3,8 +3,22 @@ #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() @@ -19,47 +33,35 @@ launch() irq_set_enabled(IO_IRQ_BANK0, true); - 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_wheel_speed_task, - "monitor_left_wheel_speed_task", - configMINIMAL_STACK_SIZE, - (void *) p_motor_speed_left, - 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_wheel_speed_task, "monitor_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)p_motor_speed_right, - 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(); diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 4dd32b7..986ea33 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -15,9 +15,6 @@ #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) - void launch() {