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

use minimum global variables

This commit is contained in:
Richie 2023-11-09 11:38:20 +08:00
parent 191c8686c0
commit 0a62f7738c
6 changed files with 126 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
frtos/config/motor_config.h
View File

@ -70,6 +70,7 @@ typedef struct
*/ */
typedef struct typedef struct
{ {
bool use_pid;
float kp_value; float kp_value;
float ki_value; float ki_value;
float kd_value; float kd_value;
@ -85,9 +86,9 @@ typedef struct
typedef struct typedef struct
{ {
motor_speed_t speed; motor_speed_t speed;
SemaphoreHandle_t sem;
motor_pwm_t pwm; motor_pwm_t pwm;
motor_pid_t pid; SemaphoreHandle_t *p_sem;
motor_pid_t *p_pid;
} motor_t; } motor_t;
typedef struct typedef struct
@ -97,6 +98,14 @@ typedef struct
volatile bool is_running; volatile bool is_running;
} distance_to_stop_t; } 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 */ #endif /* MOTOR_CONFIG_H */

10
frtos/motor/motor_direction.h
View File

@ -65,7 +65,7 @@ check_direction(float current_direction, float target_direction, float range)
} }
void void
spin_to_yaw(float target_yaw) spin_to_yaw(float target_yaw, car_struct_t *car_struct)
{ {
updateDirection(); updateDirection();
float initial_yaw = g_direction.yaw; float initial_yaw = g_direction.yaw;
@ -83,9 +83,9 @@ spin_to_yaw(float target_yaw)
set_wheel_direction(DIRECTION_LEFT); 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 (;;) for (;;)
{ {
@ -93,12 +93,12 @@ spin_to_yaw(float target_yaw)
if (check_direction(g_direction.yaw, target_yaw, 1)) if (check_direction(g_direction.yaw, target_yaw, 1))
{ {
set_wheel_direction(DIRECTION_MASK); set_wheel_direction(DIRECTION_MASK);
set_wheel_speed_synced(0u); set_wheel_speed_synced(0u, car_struct);
break; break;
} }
} }
g_use_pid = true; car_struct->p_right_motor->p_pid->use_pid = true;
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(50));
} }

54
frtos/motor/motor_init.h
View File

@ -19,28 +19,28 @@
#include "motor_config.h" #include "motor_config.h"
motor_t g_motor_left = { .pwm.level = 0u, //motor_t g_motor_left = { .pwm.level = 0u,
.pwm.channel = PWM_CHAN_A, // .pwm.channel = PWM_CHAN_A,
.speed.distance_cm = 0.0f }; // .speed.distance_cm = 0.0f };
//
// classic ziegler nichols method //// classic ziegler nichols method
// Ku = 1000, Tu = 0.9s, interval = 0.05s //// Ku = 1000, Tu = 0.9s, interval = 0.05s
// Kp = 0.6 * Ku = 600 //// Kp = 0.6 * Ku = 600
// Ki = 2 * Kp * 0.05 / Tu = 66.67 //// Ki = 2 * Kp * 0.05 / Tu = 66.67
// Kd = Kp * Tu * 0.05 / 8 = 1350 //// Kd = Kp * Tu * 0.05 / 8 = 1350
motor_t g_motor_right = { .pwm.level = 0u, //motor_t g_motor_right = { .pwm.level = 0u,
.pwm.channel = PWM_CHAN_B, // .pwm.channel = PWM_CHAN_B,
.speed.distance_cm = 0.0f, // .speed.distance_cm = 0.0f,
.pid.kp_value = 600.f, // .p_pid.kp_value = 600.f,
.pid.ki_value = 66.67f, // .p_pid.ki_value = 66.67f,
.pid.kd_value = 1350.f,}; // .p_pid.kd_value = 1350.f,};
void void
motor_init(void) motor_init(car_struct_t * car_struct)
{ {
// Semaphore // Semaphore
g_motor_left.sem = xSemaphoreCreateBinary(); g_left_sem = xSemaphoreCreateBinary();
g_motor_right.sem = xSemaphoreCreateBinary(); g_right_sem = xSemaphoreCreateBinary();
gpio_init(SPEED_PIN_RIGHT); gpio_init(SPEED_PIN_RIGHT);
gpio_init(SPEED_PIN_LEFT); 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_LEFT, GPIO_FUNC_PWM);
gpio_set_function(PWM_PIN_RIGHT, 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); car_struct->p_left_motor->pwm.slice_num = pwm_gpio_to_slice_num
g_motor_right.pwm.slice_num = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); (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 // NOTE: PWM clock is 125MHz for raspberrypi pico w by default
// 125MHz / 50 = 2500kHz // 125MHz / 50 = 2500kHz
pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV); pwm_set_clkdiv(car_struct->p_left_motor->pwm.slice_num, PWM_CLK_DIV);
pwm_set_clkdiv(g_motor_right.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 // L289N can accept up to 40kHz
// 2500kHz / 100 = 25kHz // 2500kHz / 100 = 25kHz
pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U)); pwm_set_wrap(car_struct->p_left_motor->pwm.slice_num, (PWM_WRAP - 1U));
pwm_set_wrap(g_motor_right.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(car_struct->p_left_motor->pwm.slice_num, true);
pwm_set_enabled(g_motor_right.pwm.slice_num, true); pwm_set_enabled(car_struct->p_right_motor->pwm.slice_num, true);
} }
#endif /* MOTOR_INIT_H */ #endif /* MOTOR_INIT_H */

78
frtos/motor/motor_pid.h
View File

@ -19,10 +19,10 @@
* @return The control signal * @return The control signal
*/ */
float float
compute_pid(float *integral, float *prev_error) compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
{ {
float error float error = car_struct->p_left_motor->speed.distance_cm
= g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm; - car_struct->p_right_motor->speed.distance_cm;
printf("%f\n", error); printf("%f\n", error);
@ -30,42 +30,35 @@ compute_pid(float *integral, float *prev_error)
float derivative = error - *prev_error; float derivative = error - *prev_error;
float control_signal = g_motor_right.pid.kp_value * error float control_signal
+ g_motor_right.pid.ki_value * (*integral) = car_struct->p_right_motor->p_pid->kp_value * error
+ g_motor_right.pid.kd_value * derivative; + car_struct->p_right_motor->p_pid->ki_value * (*integral)
+ car_struct->p_right_motor->p_pid->kd_value * derivative;
*prev_error = error; *prev_error = error;
return control_signal; 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 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 integral = 0.0f;
static float prev_error = 0.0f; static float prev_error = 0.0f;
if (!g_use_pid) if (!car_strut->p_right_motor->p_pid->use_pid)
{ {
return true; 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) if (temp > MAX_PWM_LEVEL)
{ {
@ -77,43 +70,12 @@ repeating_pid_handler(__unused struct repeating_timer *t)
temp = MIN_PWM_LEVEL + 1u; temp = MIN_PWM_LEVEL + 1u;
} }
g_motor_right.pwm.level = (uint16_t)temp; printf("temp: %f\n", temp);
pwm_set_chan_level(g_motor_right.pwm.slice_num,
g_motor_right.pwm.channel,
g_motor_right.pwm.level);
// printf("speed: %f cm/s\n", g_motor_right.speed.current_cms); set_wheel_speed((uint32_t)temp, car_strut->p_right_motor);
// printf("distance: %f cm\n", g_motor_right.speed.distance_cm);
return true; 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);
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);
return true; return true;
} }

14
frtos/motor/motor_speed.h
View File

@ -19,7 +19,7 @@ h_wheel_sensor_isr_handler(void)
gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_motor_left.sem, xSemaphoreGiveFromISR(g_left_sem,
&xHigherPriorityTaskWoken); &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(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); gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_motor_right.sem, xSemaphoreGiveFromISR(g_right_sem,
&xHigherPriorityTaskWoken); &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
} }
@ -53,8 +53,8 @@ monitor_wheel_speed_task(void *pvParameters)
for (;;) for (;;)
{ {
if ((xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100))
== pdTRUE) && (g_use_pid == true)) == pdTRUE) && (p_motor->p_pid->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;
@ -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 * @param pwm_level The pwm_level of the wheels, from 0 to 99
*/ */
void 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) if (pwm_level > MAX_PWM_LEVEL)
{ {
pwm_level = MAX_PWM_LEVEL; pwm_level = MAX_PWM_LEVEL;
} }
set_wheel_speed(pwm_level, &g_motor_left); set_wheel_speed(pwm_level, car_strut->p_left_motor);
set_wheel_speed(pwm_level, &g_motor_right); set_wheel_speed(pwm_level, car_strut->p_right_motor);
} }
///*! ///*!

53
frtos/motor/motor_test.c
View File

@ -5,26 +5,25 @@
#include "motor_pid.h" #include "motor_pid.h"
void void
motor_control_task(__unused void *params) motor_control_task(void *params)
{ {
car_struct_t *car_struct = (car_struct_t *)params;
for (;;) for (;;)
{ {
set_wheel_direction(DIRECTION_FORWARD); set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed(90u, &g_motor_left); set_wheel_speed_synced(90u, car_struct);
set_wheel_speed(90u, &g_motor_right);
vTaskDelay(pdMS_TO_TICKS(10000)); vTaskDelay(pdMS_TO_TICKS(10000));
revert_wheel_direction(); revert_wheel_direction();
set_wheel_speed(90u, &g_motor_left); set_wheel_speed_synced(90u, car_struct);
set_wheel_speed(90u, &g_motor_right);
vTaskDelay(pdMS_TO_TICKS(10000)); vTaskDelay(pdMS_TO_TICKS(10000));
} }
} }
void void
launch() launch(car_struct_t *car_strut)
{ {
// isr to detect right motor slot // isr to detect right motor slot
gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
@ -38,8 +37,8 @@ launch()
// PID timer // PID timer
struct repeating_timer 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, car_strut, &pid_timer);
// add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer); // add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
// Left wheel // Left wheel
// //
@ -47,7 +46,7 @@ launch()
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 *)&g_motor_left, (void *)car_strut->p_left_motor,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle); &h_monitor_left_wheel_speed_task_handle);
@ -57,7 +56,7 @@ launch()
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 *)&g_motor_right, (void *)car_strut->p_right_motor,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle); &h_monitor_right_wheel_speed_task_handle);
@ -66,7 +65,7 @@ launch()
xTaskCreate(motor_control_task, xTaskCreate(motor_control_task,
"motor_turning_task", "motor_turning_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
NULL, (void *)car_strut,
WHEEL_CONTROL_PRIO, WHEEL_CONTROL_PRIO,
&h_motor_turning_task_handle); &h_motor_turning_task_handle);
@ -81,9 +80,37 @@ main(void)
sleep_ms(4000); sleep_ms(4000);
printf("Test started!\n"); 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(;;); // for(;;);