diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 80dd78b..1deb183 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -25,8 +25,8 @@ #define PWM_WRAP 5000U #define PID_KP 3.f -#define PID_KI 0.01f -#define PID_KD 0.05f +#define PID_KI 0.01 // 0.01f +#define PID_KD 0.05f // 0.05f #define MAX_SPEED 4900U #define MIN_SPEED 0U // To be changed @@ -41,6 +41,7 @@ */ typedef struct { float target_speed_cms; + float current_speed_cms; uint16_t pwm_level; SemaphoreHandle_t * p_sem; uint * p_slice_num; diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index afcc1b9..189a9ef 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -27,15 +27,15 @@ 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 }; + .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 }; + .p_sem = &g_wheel_speed_sem_right, + .p_slice_num = &g_slice_num_right, + .pwm_channel = PWM_CHAN_B }; void motor_init(void) diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h new file mode 100644 index 0000000..34a446a --- /dev/null +++ b/frtos/motor/motor_pid.h @@ -0,0 +1,72 @@ +/* + * @file motor_pid.h + * @brief control the speed of the wheels by setting the PWM level, using PID + * controller + * @author Richie + */ + +#include "motor_init.h" + +/*! + * @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(const volatile float *target_speed, + const volatile 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 +motor_pid_task(void *p_param) +{ + motor_speed_t *p_motor_speed = p_param; + float integral = 0.0f; + float prev_error = 0.0f; + + for (;;) + { + float control_signal = compute_pid(&(p_motor_speed->target_speed_cms), + &(p_motor_speed->current_speed_cms), + &integral, &prev_error); + + if (p_motor_speed->pwm_level + control_signal > MAX_SPEED) + { + p_motor_speed->pwm_level = MAX_SPEED; + } + else if (p_motor_speed->pwm_level + control_signal < MIN_SPEED) + { + p_motor_speed->pwm_level = MIN_SPEED; + } + else + { + p_motor_speed->pwm_level = p_motor_speed->pwm_level + control_signal; + } + + printf("control signal: %f\n", control_signal); + printf("new pwm: %hu\n\n", p_motor_speed->pwm_level); + + pwm_set_chan_level(*p_motor_speed->p_slice_num, + p_motor_speed->pwm_channel, + p_motor_speed->pwm_level); + + vTaskDelay(pdMS_TO_TICKS(50)); + } +} \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index db15a81..bb91ce7 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -1,9 +1,6 @@ /* * @file motor_speed.c - * @brief control the speed of the wheels by setting the PWM level, and - * monitor the speed by using edge interrupt and measure the time between - * each slot of the wheel, then calculate the speed of the wheel in cm/s, and - * adjust the speed of the wheel by using PID controller, and set the new PWM + * @brief monitor and update the speed of the wheels * @author Richie */ @@ -36,32 +33,6 @@ h_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(const volatile float *target_speed, - const 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; -} /*! * @brief Task to monitor and control the speed of the wheel @@ -74,18 +45,10 @@ monitor_wheel_speed_task(void *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(*p_motor_speed->p_sem, pdMS_TO_TICKS(100)) @@ -99,37 +62,16 @@ monitor_wheel_speed_task(void *pvParameters) // distance = circumference / 20 // circumference = 2 * pi * 3.25 cm = 20.4203522483 cm // distance = 20.4203522483 cm / 20 = 1.02101761242 cm - speed = (float)(1.02101761242f / (elapsed_time / 1000000.f)); + p_motor_speed->current_speed_cms = (float) (1.02101761242f / + (elapsed_time / + 1000000.f)); - printf("speed: %f cm/s\n", speed); + printf("speed: %f cm/s\n", p_motor_speed->current_speed_cms); } else { - speed = 0.f; + p_motor_speed->current_speed_cms = 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 5543ebb..7370d91 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -2,6 +2,7 @@ #include "motor_speed.h" #include "motor_direction.h" +#include "motor_pid.h" #define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) @@ -10,13 +11,18 @@ test_speed_change_task(void *p_param) { for (;;) { + g_motor_speed_left.target_speed_cms = 50.0f; + g_motor_speed_right.target_speed_cms = 50.0f; + vTaskDelay(pdMS_TO_TICKS(5000)); + g_motor_speed_left.target_speed_cms = 20.0f; g_motor_speed_right.target_speed_cms = 20.0f; - vTaskDelay(pdMS_TO_TICKS(10000)); + vTaskDelay(pdMS_TO_TICKS(5000)); + + g_motor_speed_left.target_speed_cms = 0.0f; + g_motor_speed_right.target_speed_cms = 0.0f; + vTaskDelay(pdMS_TO_TICKS(5000)); - g_motor_speed_left.target_speed_cms = 40.0f; - g_motor_speed_right.target_speed_cms = 40.0f; - vTaskDelay(pdMS_TO_TICKS(10000)); } } @@ -33,6 +39,8 @@ launch() irq_set_enabled(IO_IRQ_BANK0, true); + // Left wheel + // // TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; // xTaskCreate(monitor_wheel_speed_task, // "monitor_left_wheel_speed_task", @@ -41,6 +49,16 @@ launch() // WHEEL_SPEED_PRIO, // &h_monitor_left_wheel_speed_task_handle); +// TaskHandle_t h_motor_pid_left_task_handle = NULL; +// xTaskCreate(motor_pid_task, +// "motor_pid_task", +// configMINIMAL_STACK_SIZE, +// (void *)&g_motor_speed_left, +// WHEEL_SPEED_PRIO, +// &h_motor_pid_left_task_handle); + + // Right wheel + // TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; xTaskCreate(monitor_wheel_speed_task, "monitor_wheel_speed_task", @@ -49,6 +67,16 @@ launch() WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); + TaskHandle_t h_motor_pid_right_task_handle = NULL; + xTaskCreate(motor_pid_task, + "motor_pid_task", + configMINIMAL_STACK_SIZE, + (void *)&g_motor_speed_right, + WHEEL_SPEED_PRIO, + &h_motor_pid_right_task_handle); + + // Test speed change + // TaskHandle_t h_test_speed_change_task_handle = NULL; xTaskCreate(test_speed_change_task, "test_speed_change_task",