From e9511eb8cef9c214020e0cc67ab52ca8af6bedd8 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Thu, 9 Nov 2023 12:57:26 +0800 Subject: [PATCH] updated vtask and init --- frtos/config/motor_config.h | 14 ++++--- frtos/motor/motor_direction.h | 4 +- frtos/motor/motor_init.h | 45 +++++++++++---------- frtos/motor/motor_pid.h | 8 ++-- frtos/motor/motor_speed.h | 2 +- frtos/motor/motor_test.c | 73 ++++++++++++----------------------- 6 files changed, 64 insertions(+), 82 deletions(-) diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index dbc411e..6aafd4f 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -85,10 +85,11 @@ typedef struct */ typedef struct { - motor_speed_t speed; - motor_pwm_t pwm; - SemaphoreHandle_t *p_sem; - motor_pid_t *p_pid; + motor_speed_t speed; + motor_pwm_t pwm; + SemaphoreHandle_t * p_sem; + bool * use_pid; + } motor_t; typedef struct @@ -103,8 +104,9 @@ SemaphoreHandle_t g_right_sem; typedef struct { - motor_t *p_left_motor; - motor_t *p_right_motor; + motor_t * p_left_motor; + motor_t * p_right_motor; + motor_pid_t * p_pid; } car_struct_t; diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index 6989302..8107d29 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -85,7 +85,7 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct) set_wheel_speed_synced(80u, car_struct); - car_struct->p_right_motor->p_pid->use_pid = false; + car_struct->p_pid->use_pid = false; for (;;) { @@ -98,7 +98,7 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct) } } - car_struct->p_right_motor->p_pid->use_pid = true; + car_struct->p_pid->use_pid = true; vTaskDelay(pdMS_TO_TICKS(50)); } diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h index 03db765..3b7aad3 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -19,29 +19,32 @@ #include "motor_config.h" -//motor_t g_motor_left = { .pwm.level = 0u, -// .pwm.channel = PWM_CHAN_A, -// .speed.distance_cm = 0.0f }; -// -//// classic ziegler nichols method -//// Ku = 1000, Tu = 0.9s, interval = 0.05s -//// Kp = 0.6 * Ku = 600 -//// Ki = 2 * Kp * 0.05 / Tu = 66.67 -//// Kd = Kp * Tu * 0.05 / 8 = 1350 -//motor_t g_motor_right = { .pwm.level = 0u, -// .pwm.channel = PWM_CHAN_B, -// .speed.distance_cm = 0.0f, -// .p_pid.kp_value = 600.f, -// .p_pid.ki_value = 66.67f, -// .p_pid.kd_value = 1350.f,}; - void -motor_init(car_struct_t * car_struct) +motor_init(car_struct_t *car_struct) { // Semaphore g_left_sem = xSemaphoreCreateBinary(); g_right_sem = xSemaphoreCreateBinary(); + car_struct->p_pid->use_pid = true; + car_struct->p_pid->kp_value = 600.f; + car_struct->p_pid->ki_value = 66.67f; + car_struct->p_pid->kd_value = 1350.f; + + // initialize the car_struct + car_struct->p_left_motor->pwm.level = 0u; + car_struct->p_left_motor->pwm.channel = PWM_CHAN_A; + car_struct->p_left_motor->speed.distance_cm = 0.0f; + car_struct->p_left_motor->p_sem = &g_left_sem; + car_struct->p_left_motor->use_pid = &car_struct->p_pid->use_pid; + + car_struct->p_right_motor->pwm.level = 0u; + car_struct->p_right_motor->pwm.channel = PWM_CHAN_B; + car_struct->p_right_motor->speed.distance_cm = 0.0f; + car_struct->p_right_motor->p_sem = &g_right_sem; + car_struct->p_right_motor->use_pid = &car_struct->p_pid->use_pid; + + // Initialize speed pins as inputs gpio_init(SPEED_PIN_RIGHT); gpio_init(SPEED_PIN_LEFT); gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN); @@ -62,10 +65,10 @@ motor_init(car_struct_t * car_struct) gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM); gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM); - car_struct->p_left_motor->pwm.slice_num = pwm_gpio_to_slice_num - (PWM_PIN_LEFT); - car_struct->p_right_motor->pwm.slice_num = pwm_gpio_to_slice_num - (PWM_PIN_RIGHT); + car_struct->p_left_motor->pwm.slice_num + = pwm_gpio_to_slice_num(PWM_PIN_LEFT); + car_struct->p_right_motor->pwm.slice_num + = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); // NOTE: PWM clock is 125MHz for raspberrypi pico w by default diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index b56acd2..62a9ff7 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -31,9 +31,9 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct) float derivative = error - *prev_error; float control_signal - = car_struct->p_right_motor->p_pid->kp_value * error - + car_struct->p_right_motor->p_pid->ki_value * (*integral) - + car_struct->p_right_motor->p_pid->kd_value * derivative; + = car_struct->p_pid->kp_value * error + + car_struct->p_pid->ki_value * (*integral) + + car_struct->p_pid->kd_value * derivative; *prev_error = error; @@ -48,7 +48,7 @@ repeating_pid_handler(struct repeating_timer *t) static float integral = 0.0f; static float prev_error = 0.0f; - if (!car_strut->p_right_motor->p_pid->use_pid) + if (!car_strut->p_pid->use_pid) { return true; } diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 8c8da52..881bb24 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -54,7 +54,7 @@ monitor_wheel_speed_task(void *pvParameters) for (;;) { if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) - == pdTRUE) && (p_motor->p_pid->use_pid == true)) + == pdTRUE) && (*p_motor->use_pid == true)) { curr_time = time_us_64(); elapsed_time = curr_time - prev_time; diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 230b854..7a45243 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -23,30 +23,15 @@ motor_control_task(void *params) } void -launch(car_struct_t *car_strut) +launch(car_struct_t *car_struct, void *isr_handler) { - // 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); - - // PID timer - struct repeating_timer pid_timer; - add_repeating_timer_ms(-50, repeating_pid_handler, car_strut, &pid_timer); - // add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); - // Left wheel // TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; xTaskCreate(monitor_wheel_speed_task, "monitor_left_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)car_strut->p_left_motor, + (void *)car_struct->p_left_motor, WHEEL_SPEED_PRIO, &h_monitor_left_wheel_speed_task_handle); @@ -56,7 +41,7 @@ launch(car_struct_t *car_strut) xTaskCreate(monitor_wheel_speed_task, "monitor_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)car_strut->p_right_motor, + (void *)car_struct->p_right_motor, WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); @@ -65,11 +50,19 @@ launch(car_struct_t *car_strut) xTaskCreate(motor_control_task, "motor_turning_task", configMINIMAL_STACK_SIZE, - (void *)car_strut, + (void *)car_struct, WHEEL_CONTROL_PRIO, &h_motor_turning_task_handle); - vTaskStartScheduler(); + // 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, 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, isr_handler); + + irq_set_enabled(IO_IRQ_BANK0, true); } int @@ -80,39 +73,23 @@ main(void) sleep_ms(4000); printf("Test started!\n"); - motor_pid_t g_pid = { - .kp_value = 600.f, - .ki_value = 66.67f, - .kd_value = 1350.f, - .use_pid = true, - }; + motor_t g_motor_right; + motor_t g_motor_left; + motor_pid_t g_pid; - motor_t g_motor_left = { - .pwm.level = 0u, - .pwm.channel = PWM_CHAN_A, - .speed.distance_cm = 0.0f, - .p_sem = &g_left_sem, - .p_pid = &g_pid, - }; - - motor_t g_motor_right = { - .pwm.level = 0u, - .pwm.channel = PWM_CHAN_B, - .speed.distance_cm = 0.0f, - .p_sem = &g_right_sem, - .p_pid = &g_pid, - }; - - car_struct_t car_struct = { - .p_left_motor = &g_motor_left, - .p_right_motor = &g_motor_right, - }; + car_struct_t car_struct = { .p_right_motor = &g_motor_right, + .p_left_motor = &g_motor_left, + .p_pid = &g_pid }; motor_init(&car_struct); - launch(&car_struct); + launch(&car_struct, &h_wheel_sensor_isr_handler); - // for(;;); + // PID timer + struct repeating_timer pid_timer; + add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer); + + vTaskStartScheduler(); return (0); } \ No newline at end of file