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
{
bool use_pid;
float kp_value;
float ki_value;
float kd_value;
@ -85,9 +86,9 @@ typedef struct
typedef struct
{
motor_speed_t speed;
SemaphoreHandle_t sem;
motor_pwm_t pwm;
motor_pid_t pid;
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 */

10
frtos/motor/motor_direction.h
View File

@ -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));
}

54
frtos/motor/motor_init.h
View File

@ -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 */

78
frtos/motor/motor_pid.h
View File

@ -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;
}

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);
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);
}
///*!

53
frtos/motor/motor_test.c
View File

@ -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(;;);