From 376453398aca654f473da1b71f013471ac73efac Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Fri, 13 Oct 2023 23:29:42 +0800 Subject: [PATCH] implemented pid for right wheel, pending fine-tuning --- frtos/car/rtos_car.c | 12 ++++--- frtos/car/wheel.h | 85 +++++++++++++++++++++++++++++++++++--------- 2 files changed, 77 insertions(+), 20 deletions(-) diff --git a/frtos/car/rtos_car.c b/frtos/car/rtos_car.c index 70aa7e9..b3d4474 100644 --- a/frtos/car/rtos_car.c +++ b/frtos/car/rtos_car.c @@ -37,11 +37,15 @@ launch() 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, - NULL, + (void *) p_target_speed, READ_LEFT_WHEEL_SPEED_PRIO, &h_monitor_left_wheel_speed_task_handle); @@ -49,7 +53,7 @@ launch() xTaskCreate(monitor_right_wheel_speed_task, "monitor_right_wheel_speed_task", configMINIMAL_STACK_SIZE, - NULL, + (void *) p_target_speed, READ_RIGHT_WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); @@ -61,10 +65,10 @@ main (void) { stdio_usb_init(); wheel_setup(); - sleep_ms(2000); + sleep_ms(5000); set_wheel_direction(DIRECTION_RIGHT_FORWARD); - set_wheel_speed(1.f, 1u); + set_wheel_speed(START_SPEED, 1u); launch(); diff --git a/frtos/car/wheel.h b/frtos/car/wheel.h index ef50f58..41056a2 100644 --- a/frtos/car/wheel.h +++ b/frtos/car/wheel.h @@ -19,9 +19,13 @@ #define PWM_CLK_DIV 250.f #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_right = 0U; @@ -36,11 +40,6 @@ wheel_setup(void) g_wheel_speed_sem_left = xSemaphoreCreateBinary(); g_wheel_speed_sem_right = xSemaphoreCreateBinary(); - // Speed - /* adc_init(); - adc_gpio_init(SPEED_PIN_RIGHT); - adc_gpio_init(SPEED_PIN_LEFT);*/ - gpio_init(SPEED_PIN_RIGHT); gpio_init(SPEED_PIN_LEFT); gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN); @@ -103,7 +102,7 @@ set_wheel_direction (uint32_t direction) /*! * @brief Set the speed of the wheels; can use bitwise OR to set both - * @param speed in range [0.0, 1.0] + * @param speed in range [0, 5000] * @param side 0 for left, 1 for right */ void @@ -112,14 +111,14 @@ set_wheel_speed (float speed, uint8_t side) if (side == 0U) { pwm_set_chan_level(g_slice_num_left, - PWM_CHAN_A, - (short) (PWM_WRAP * speed)); + PWM_CHAN_A, + (uint16_t) speed); } else { pwm_set_chan_level(g_slice_num_right, PWM_CHAN_B, - (short) (PWM_WRAP * speed)); + (uint16_t) speed); } } @@ -131,7 +130,7 @@ h_left_wheel_sensor_isr_handler (void) { gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); - printf("left wheel sensor isr\n"); + // printf("left wheel sensor isr\n"); BaseType_t xHigherPriorityTaskWoken = pdFALSE; xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); @@ -145,16 +144,36 @@ h_right_wheel_sensor_isr_handler (void) { gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL); - printf("right wheel sensor isr\n"); + // printf("right wheel sensor isr\n"); BaseType_t xHigherPriorityTaskWoken = pdFALSE; xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } } -void -monitor_left_wheel_speed_task (__unused void *pvParameters) +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 (;;) { if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE) @@ -187,8 +206,12 @@ monitor_left_wheel_speed_task (__unused void *pvParameters) } void -monitor_right_wheel_speed_task (__unused void *pvParameters) +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) @@ -209,6 +232,36 @@ monitor_right_wheel_speed_task (__unused void *pvParameters) (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(*target_speed, + 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); + } }