diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index f22a356..dbc411e 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -13,17 +13,17 @@ #define DIRECTION_PIN_LEFT_IN3 19U #define DIRECTION_PIN_LEFT_IN4 20U -#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2) -#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1) -#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) -#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) +#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2) +#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1) +#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) +#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) -#define DIRECTION_FORWARD (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD) -#define DIRECTION_BACKWARD (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD) -#define DIRECTION_LEFT (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_FORWARD) -#define DIRECTION_RIGHT (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD) +#define DIRECTION_FORWARD (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD) +#define DIRECTION_BACKWARD (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD) +#define DIRECTION_LEFT (DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_FORWARD) +#define DIRECTION_RIGHT (DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD) -#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) +#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD) #define SPEED_PIN_RIGHT 15U #define SPEED_PIN_LEFT 16U @@ -34,9 +34,9 @@ #define MAX_PWM_LEVEL 99U #define MIN_PWM_LEVEL 0U -#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) -#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL) -#define WHEEL_PID_PRIO (tskIDLE_PRIORITY + 1UL) +#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) +#define WHEEL_CONTROL_PRIO (tskIDLE_PRIORITY + 1UL) +#define WHEEL_PID_PRIO (tskIDLE_PRIORITY + 1UL) /*! * @brief Structure for the motor speed parameters @@ -70,6 +70,7 @@ typedef struct */ typedef struct { + bool use_pid; float kp_value; float ki_value; float kd_value; @@ -84,10 +85,10 @@ typedef struct */ typedef struct { - motor_speed_t speed; - SemaphoreHandle_t sem; - motor_pwm_t pwm; - motor_pid_t pid; + motor_speed_t speed; + motor_pwm_t pwm; + SemaphoreHandle_t *p_sem; + motor_pid_t *p_pid; } motor_t; typedef struct @@ -97,6 +98,14 @@ typedef struct volatile bool is_running; } distance_to_stop_t; -volatile bool g_use_pid = true; +SemaphoreHandle_t g_left_sem; +SemaphoreHandle_t g_right_sem; + +typedef struct +{ + motor_t *p_left_motor; + motor_t *p_right_motor; + +} car_struct_t; #endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h index b4f10ff..6989302 100644 --- a/frtos/motor/motor_direction.h +++ b/frtos/motor/motor_direction.h @@ -65,7 +65,7 @@ check_direction(float current_direction, float target_direction, float range) } void -spin_to_yaw(float target_yaw) +spin_to_yaw(float target_yaw, car_struct_t *car_struct) { updateDirection(); float initial_yaw = g_direction.yaw; @@ -83,9 +83,9 @@ spin_to_yaw(float target_yaw) set_wheel_direction(DIRECTION_LEFT); } - set_wheel_speed_synced(80u); + set_wheel_speed_synced(80u, car_struct); - g_use_pid = false; + car_struct->p_right_motor->p_pid->use_pid = false; for (;;) { @@ -93,12 +93,12 @@ spin_to_yaw(float target_yaw) if (check_direction(g_direction.yaw, target_yaw, 1)) { set_wheel_direction(DIRECTION_MASK); - set_wheel_speed_synced(0u); + set_wheel_speed_synced(0u, car_struct); break; } } - g_use_pid = true; + car_struct->p_right_motor->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 7ac9b70..03db765 100644 --- a/frtos/motor/motor_init.h +++ b/frtos/motor/motor_init.h @@ -19,28 +19,28 @@ #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, - .pid.kp_value = 600.f, - .pid.ki_value = 66.67f, - .pid.kd_value = 1350.f,}; +//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(void) +motor_init(car_struct_t * car_struct) { // Semaphore - g_motor_left.sem = xSemaphoreCreateBinary(); - g_motor_right.sem = xSemaphoreCreateBinary(); + g_left_sem = xSemaphoreCreateBinary(); + g_right_sem = xSemaphoreCreateBinary(); gpio_init(SPEED_PIN_RIGHT); gpio_init(SPEED_PIN_LEFT); @@ -62,22 +62,24 @@ motor_init(void) gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM); gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM); - g_motor_left.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_LEFT); - g_motor_right.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 // 125MHz / 50 = 2500kHz - pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV); - pwm_set_clkdiv(g_motor_right.pwm.slice_num, PWM_CLK_DIV); + pwm_set_clkdiv(car_struct->p_left_motor->pwm.slice_num, PWM_CLK_DIV); + pwm_set_clkdiv(car_struct->p_right_motor->pwm.slice_num, PWM_CLK_DIV); // L289N can accept up to 40kHz // 2500kHz / 100 = 25kHz - pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U)); - pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U)); + pwm_set_wrap(car_struct->p_left_motor->pwm.slice_num, (PWM_WRAP - 1U)); + pwm_set_wrap(car_struct->p_right_motor->pwm.slice_num, (PWM_WRAP - 1U)); - pwm_set_enabled(g_motor_left.pwm.slice_num, true); - pwm_set_enabled(g_motor_right.pwm.slice_num, true); + pwm_set_enabled(car_struct->p_left_motor->pwm.slice_num, true); + pwm_set_enabled(car_struct->p_right_motor->pwm.slice_num, true); } #endif /* MOTOR_INIT_H */ \ No newline at end of file diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 706b27f..b56acd2 100644 --- a/frtos/motor/motor_pid.h +++ b/frtos/motor/motor_pid.h @@ -19,10 +19,10 @@ * @return The control signal */ float -compute_pid(float *integral, float *prev_error) +compute_pid(float *integral, float *prev_error, car_struct_t *car_struct) { - float error - = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; + float error = car_struct->p_left_motor->speed.distance_cm + - car_struct->p_right_motor->speed.distance_cm; printf("%f\n", error); @@ -30,42 +30,35 @@ compute_pid(float *integral, float *prev_error) float derivative = error - *prev_error; - float control_signal = g_motor_right.pid.kp_value * error - + g_motor_right.pid.ki_value * (*integral) - + g_motor_right.pid.kd_value * derivative; + 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; *prev_error = error; return control_signal; } -float -compute_i_controller(float *integral) -{ - float error - = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; - - printf("%f\n", error); - - *integral += error; - - return g_motor_right.pid.ki_value * (*integral); -} - bool -repeating_pid_handler(__unused struct repeating_timer *t) +repeating_pid_handler(struct repeating_timer *t) { + car_struct_t *car_strut = (car_struct_t *)t->user_data; + static float integral = 0.0f; static float prev_error = 0.0f; - if (!g_use_pid) + if (!car_strut->p_right_motor->p_pid->use_pid) { return true; } - float control_signal = compute_pid(&integral, &prev_error); + float control_signal = compute_pid(&integral, &prev_error, car_strut); - float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; + printf("control: %f\n", control_signal); + + float temp + = (float)car_strut->p_right_motor->pwm.level + control_signal * 0.05f; if (temp > MAX_PWM_LEVEL) { @@ -77,43 +70,12 @@ repeating_pid_handler(__unused struct repeating_timer *t) temp = MIN_PWM_LEVEL + 1u; } - g_motor_right.pwm.level = (uint16_t)temp; - pwm_set_chan_level(g_motor_right.pwm.slice_num, - g_motor_right.pwm.channel, - g_motor_right.pwm.level); + printf("temp: %f\n", temp); - // printf("speed: %f cm/s\n", g_motor_right.speed.current_cms); - // printf("distance: %f cm\n", g_motor_right.speed.distance_cm); + set_wheel_speed((uint32_t)temp, car_strut->p_right_motor); - return true; -} - -bool -repeating_i_handler(__unused struct repeating_timer *t) -{ - static float integral = 0.0f; - - if (!g_use_pid) - { - integral = 0.0f; // reset once disabled - return true; - } - - float control_signal = compute_i_controller(&integral); - - float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f; - - if (temp > MAX_PWM_LEVEL) - { - temp = MAX_PWM_LEVEL; - } - - if (temp <= MIN_PWM_LEVEL) - { - temp = MIN_PWM_LEVEL; - } - - set_wheel_speed((uint32_t)temp, &g_motor_right); + printf("speed: %f cm/s\n", car_strut->p_right_motor->speed.current_cms); + printf("distance: %f cm\n", car_strut->p_right_motor->speed.distance_cm); return true; } diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index be0072e..8c8da52 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -19,7 +19,7 @@ h_wheel_sensor_isr_handler(void) gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_motor_left.sem, + xSemaphoreGiveFromISR(g_left_sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } @@ -29,7 +29,7 @@ h_wheel_sensor_isr_handler(void) gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL); BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_motor_right.sem, + xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } @@ -53,8 +53,8 @@ monitor_wheel_speed_task(void *pvParameters) for (;;) { - if ((xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) - == pdTRUE) && (g_use_pid == true)) + if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) + == pdTRUE) && (p_motor->p_pid->use_pid == true)) { curr_time = time_us_64(); elapsed_time = curr_time - prev_time; @@ -91,15 +91,15 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor) * @param pwm_level The pwm_level of the wheels, from 0 to 99 */ void -set_wheel_speed_synced(uint32_t pwm_level) +set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *car_strut) { if (pwm_level > MAX_PWM_LEVEL) { pwm_level = MAX_PWM_LEVEL; } - set_wheel_speed(pwm_level, &g_motor_left); - set_wheel_speed(pwm_level, &g_motor_right); + set_wheel_speed(pwm_level, car_strut->p_left_motor); + set_wheel_speed(pwm_level, car_strut->p_right_motor); } ///*! diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index 5661284..230b854 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -5,26 +5,25 @@ #include "motor_pid.h" void -motor_control_task(__unused void *params) +motor_control_task(void *params) { + car_struct_t *car_struct = (car_struct_t *)params; for (;;) { set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed(90u, &g_motor_left); - set_wheel_speed(90u, &g_motor_right); + set_wheel_speed_synced(90u, car_struct); vTaskDelay(pdMS_TO_TICKS(10000)); revert_wheel_direction(); - set_wheel_speed(90u, &g_motor_left); - set_wheel_speed(90u, &g_motor_right); + set_wheel_speed_synced(90u, car_struct); vTaskDelay(pdMS_TO_TICKS(10000)); } } void -launch() +launch(car_struct_t *car_strut) { // isr to detect right motor slot gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); @@ -38,8 +37,8 @@ launch() // PID timer struct repeating_timer pid_timer; - add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); -// add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &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 // @@ -47,7 +46,7 @@ launch() xTaskCreate(monitor_wheel_speed_task, "monitor_left_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)&g_motor_left, + (void *)car_strut->p_left_motor, WHEEL_SPEED_PRIO, &h_monitor_left_wheel_speed_task_handle); @@ -57,7 +56,7 @@ launch() xTaskCreate(monitor_wheel_speed_task, "monitor_wheel_speed_task", configMINIMAL_STACK_SIZE, - (void *)&g_motor_right, + (void *)car_strut->p_right_motor, WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); @@ -66,7 +65,7 @@ launch() xTaskCreate(motor_control_task, "motor_turning_task", configMINIMAL_STACK_SIZE, - NULL, + (void *)car_strut, WHEEL_CONTROL_PRIO, &h_motor_turning_task_handle); @@ -81,9 +80,37 @@ main(void) sleep_ms(4000); printf("Test started!\n"); - motor_init(); + motor_pid_t g_pid = { + .kp_value = 600.f, + .ki_value = 66.67f, + .kd_value = 1350.f, + .use_pid = true, + }; - launch(); + 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, + }; + + motor_init(&car_struct); + + launch(&car_struct); // for(;;);