devoalda
/
INF2004_Project
mirror of https://github.com/L4ncelot-R/freeRTOS-car.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

updated vtask and init

This commit is contained in:
Richie 2023-11-09 12:57:26 +08:00
parent 0a62f7738c
commit e9511eb8ce
6 changed files with 64 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
frtos/config/motor_config.h
View File

@ -85,10 +85,11 @@ typedef struct
*/ */
typedef struct typedef struct
{ {
motor_speed_t speed; motor_speed_t speed;
motor_pwm_t pwm; motor_pwm_t pwm;
SemaphoreHandle_t *p_sem; SemaphoreHandle_t * p_sem;
motor_pid_t *p_pid; bool * use_pid;
} motor_t; } motor_t;
typedef struct typedef struct
@ -103,8 +104,9 @@ SemaphoreHandle_t g_right_sem;
typedef struct typedef struct
{ {
motor_t *p_left_motor; motor_t * p_left_motor;
motor_t *p_right_motor; motor_t * p_right_motor;
motor_pid_t * p_pid;
} car_struct_t; } car_struct_t;

4
frtos/motor/motor_direction.h
View File

@ -85,7 +85,7 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct)
set_wheel_speed_synced(80u, 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 (;;) 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)); vTaskDelay(pdMS_TO_TICKS(50));
} }

45
frtos/motor/motor_init.h
View File

@ -19,29 +19,32 @@
#include "motor_config.h" #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 void
motor_init(car_struct_t * car_struct) motor_init(car_struct_t *car_struct)
{ {
// Semaphore // Semaphore
g_left_sem = xSemaphoreCreateBinary(); g_left_sem = xSemaphoreCreateBinary();
g_right_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_RIGHT);
gpio_init(SPEED_PIN_LEFT); gpio_init(SPEED_PIN_LEFT);
gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN); 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_LEFT, GPIO_FUNC_PWM);
gpio_set_function(PWM_PIN_RIGHT, 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 car_struct->p_left_motor->pwm.slice_num
(PWM_PIN_LEFT); = pwm_gpio_to_slice_num(PWM_PIN_LEFT);
car_struct->p_right_motor->pwm.slice_num = pwm_gpio_to_slice_num car_struct->p_right_motor->pwm.slice_num
(PWM_PIN_RIGHT); = pwm_gpio_to_slice_num(PWM_PIN_RIGHT);
// NOTE: PWM clock is 125MHz for raspberrypi pico w by default // NOTE: PWM clock is 125MHz for raspberrypi pico w by default

8
frtos/motor/motor_pid.h
View File

@ -31,9 +31,9 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
float derivative = error - *prev_error; float derivative = error - *prev_error;
float control_signal float control_signal
= car_struct->p_right_motor->p_pid->kp_value * error = car_struct->p_pid->kp_value * error
+ car_struct->p_right_motor->p_pid->ki_value * (*integral) + car_struct->p_pid->ki_value * (*integral)
+ car_struct->p_right_motor->p_pid->kd_value * derivative; + car_struct->p_pid->kd_value * derivative;
*prev_error = error; *prev_error = error;
@ -48,7 +48,7 @@ repeating_pid_handler(struct repeating_timer *t)
static float integral = 0.0f; static float integral = 0.0f;
static float prev_error = 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; return true;
} }

2
frtos/motor/motor_speed.h
View File

@ -54,7 +54,7 @@ monitor_wheel_speed_task(void *pvParameters)
for (;;) for (;;)
{ {
if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) 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(); curr_time = time_us_64();
elapsed_time = curr_time - prev_time; elapsed_time = curr_time - prev_time;

73
frtos/motor/motor_test.c
View File

@ -23,30 +23,15 @@ motor_control_task(void *params)
} }
void 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 // Left wheel
// //
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_wheel_speed_task, xTaskCreate(monitor_wheel_speed_task,
"monitor_left_wheel_speed_task", "monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)car_strut->p_left_motor, (void *)car_struct->p_left_motor,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle); &h_monitor_left_wheel_speed_task_handle);
@ -56,7 +41,7 @@ launch(car_struct_t *car_strut)
xTaskCreate(monitor_wheel_speed_task, xTaskCreate(monitor_wheel_speed_task,
"monitor_wheel_speed_task", "monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)car_strut->p_right_motor, (void *)car_struct->p_right_motor,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle); &h_monitor_right_wheel_speed_task_handle);
@ -65,11 +50,19 @@ launch(car_struct_t *car_strut)
xTaskCreate(motor_control_task, xTaskCreate(motor_control_task,
"motor_turning_task", "motor_turning_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)car_strut, (void *)car_struct,
WHEEL_CONTROL_PRIO, WHEEL_CONTROL_PRIO,
&h_motor_turning_task_handle); &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 int
@ -80,39 +73,23 @@ main(void)
sleep_ms(4000); sleep_ms(4000);
printf("Test started!\n"); printf("Test started!\n");
motor_pid_t g_pid = { motor_t g_motor_right;
.kp_value = 600.f, motor_t g_motor_left;
.ki_value = 66.67f, motor_pid_t g_pid;
.kd_value = 1350.f,
.use_pid = true,
};
motor_t g_motor_left = { car_struct_t car_struct = { .p_right_motor = &g_motor_right,
.pwm.level = 0u, .p_left_motor = &g_motor_left,
.pwm.channel = PWM_CHAN_A, .p_pid = &g_pid };
.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); 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); return (0);
} }