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

Merge remote-tracking branch 'origin/main' 

# Conflicts:
#	frtos/rtos_car.c
This commit is contained in:
Devoalda 2023-10-21 14:22:46 +08:00
parent 1aab873c2b 1a350fcc01
commit 97730bbb90
5 changed files with 139 additions and 189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
frtos/config/motor_config.h
View File

@ -3,9 +3,10 @@
#define MOTOR_CONFIG_H
// ENA and ENB on the L298N
#define PWM_PIN_LEFT 0U // chanel A
#define PWM_PIN_RIGHT 1U // chanel B
#define PWM_PIN_LEFT 0U // chanel A
// IN1, IN2, IN3, IN4 on the L298N
#define DIRECTION_PIN_RIGHT_IN1 11U
#define DIRECTION_PIN_RIGHT_IN2 12U
@ -23,12 +24,27 @@
#define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U
#define PID_KP 10.f
#define PID_KI 0.0f
#define PID_KD 0.0f
#define PID_KP 3.f
#define PID_KI 0.01f
#define PID_KD 0.05f
#define START_SPEED 4900U
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
/*!
* @brief Structure for the motor speed
* @param target_speed The target speed of the wheel, in cm/s
* @param pwm_level The pwm level of the wheel, from 0 to 5000
* @param sem The semaphore for the wheel
* @param p_slice_num The pointer to the slice number of the wheel
* @param channel The pwm channel of the wheel, left A or right B
*/
typedef struct {
float target_speed_cms;
uint16_t pwm_level;
SemaphoreHandle_t * p_sem;
uint * p_slice_num;
uint pwm_channel;
} motor_speed_t;
#endif /* MOTOR_CONFIG_H */

13
frtos/motor/motor_init.h
View File

@ -25,6 +25,18 @@ uint g_slice_num_right = 0U;
SemaphoreHandle_t g_wheel_speed_sem_left = NULL;
SemaphoreHandle_t g_wheel_speed_sem_right = NULL;
motor_speed_t g_motor_speed_left = { .target_speed_cms = 0.0f,
.pwm_level = 2500u,
.p_sem = &g_wheel_speed_sem_left,
.p_slice_num = &g_slice_num_left,
.pwm_channel = PWM_CHAN_A };
motor_speed_t g_motor_speed_right = { .target_speed_cms = 0.0f,
.pwm_level = 2500u,
.p_sem = &g_wheel_speed_sem_right,
.p_slice_num = &g_slice_num_right,
.pwm_channel = PWM_CHAN_B };
void
motor_init(void)
{
@ -67,7 +79,6 @@ motor_init(void)
pwm_set_enabled(g_slice_num_left, true);
pwm_set_enabled(g_slice_num_right, true);
}
#endif /* MOTOR_INIT_H */

197
frtos/motor/motor_speed.h
View File

@ -10,46 +10,25 @@
#include "motor_init.h"
/*!
* @brief Set the speed of the wheels; can use bitwise OR to set both
* @param speed in range [0, 5000]
* @param side 0 for left, 1 for right
* @brief Interrupt handler for the wheel sensor
*/
void
set_wheel_speed(float speed, uint8_t side)
{
if (side == 0U)
{
pwm_set_chan_level(g_slice_num_left, PWM_CHAN_A, (uint16_t)speed);
}
else
{
pwm_set_chan_level(g_slice_num_right, PWM_CHAN_B, (uint16_t)speed);
}
}
void
h_left_wheel_sensor_isr_handler(void)
h_wheel_sensor_isr_handler(void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
// printf("left motor sensor isr\n");
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_left,
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void
h_right_wheel_sensor_isr_handler(void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_RIGHT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
// printf("right motor sensor isr\n");
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_wheel_speed_sem_right,
&xHigherPriorityTaskWoken);
@ -57,13 +36,21 @@ h_right_wheel_sensor_isr_handler(void)
}
}
/*!
* @brief Compute the control signal using PID controller
* @param target_speed The target speed of the wheel
* @param current_speed The current speed of the wheel
* @param integral The integral term of the PID controller
* @param prev_error The previous error of the PID controller
* @return The control signal
*/
float
compute_pid(float target_speed,
float current_speed,
float *integral,
float *prev_error)
compute_pid(const volatile float *target_speed,
const float *current_speed,
float *integral,
float *prev_error)
{
float error = target_speed - current_speed;
float error = *target_speed - *current_speed;
*integral += error;
float derivative = error - *prev_error;
@ -76,121 +63,73 @@ compute_pid(float target_speed,
return control_signal;
}
/*!
* @brief Task to monitor and control the speed of the wheel
* @param pvParameters motor_speed_t struct pointer
* @see motor_speed_t
*/
void
monitor_left_wheel_speed_task(void *pvParameters)
monitor_wheel_speed_task(void *pvParameters)
{
// static float * target_speed = NULL;
// *target_speed = * (float *) pvParameters;
volatile motor_speed_t *p_motor_speed = NULL;
p_motor_speed = (motor_speed_t *)pvParameters;
float speed = 0.f;
float new_pwm = p_motor_speed->pwm_level;
uint64_t curr_time = 0u;
uint64_t prev_time = 0u;
uint64_t elapsed_time = 0u;
float control_signal = 0.f;
float integral = 0.f;
float prev_error = 0.f;
for (;;)
{
if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE)
if (xSemaphoreTake(*p_motor_speed->p_sem, pdMS_TO_TICKS(100))
== pdTRUE)
{
static uint64_t curr_time_left = 0u;
curr_time_left = time_us_64();
curr_time = time_us_64();
elapsed_time = curr_time - prev_time;
prev_time = curr_time;
static uint64_t prev_time_left = 0u;
static uint64_t elapsed_time_left = 1u; // to avoid division by 0
elapsed_time_left = curr_time_left - prev_time_left;
prev_time_left = curr_time_left;
static float speed_left = 0.f;
// speed in cm/s; speed = distance / time
// distance = circumference / 20
// circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
// distance = 20.4203522483 cm / 20 = 1.02101761242 cm
speed_left
= (float)(1.02101761242f / (elapsed_time_left / 1000000.f));
speed = (float)(1.02101761242f / (elapsed_time / 1000000.f));
printf("left speed: %f cm/s\n", speed_left);
static float control_signal = 0.f;
static float integral = 0.f;
static float prev_error = 0.f;
control_signal = compute_pid(
*(float *)pvParameters, speed_left, &integral, &prev_error);
static float new_pwm = START_SPEED;
if (new_pwm + control_signal > MAX_SPEED)
{
new_pwm = MAX_SPEED;
}
else if (new_pwm + control_signal < MIN_SPEED)
{
new_pwm = MIN_SPEED;
}
else
{
new_pwm = new_pwm + control_signal;
}
printf("control signal: %f\n", control_signal);
printf("new pwm: %f\n\n", new_pwm);
set_wheel_speed(new_pwm, 0u);
printf("speed: %f cm/s\n", speed);
}
}
}
void
monitor_right_wheel_speed_task(void *pvParameters)
{
// volatile float * target_speed = (float *) pvParameters;
// static volatile float * target_speed = NULL;
// target_speed = (float *) pvParameters;
for (;;)
{
if (xSemaphoreTake(g_wheel_speed_sem_right, portMAX_DELAY) == pdTRUE)
else
{
static uint64_t curr_time_right = 0u;
curr_time_right = time_us_64();
static uint64_t prev_time_right = 0u;
static uint64_t elapsed_time_right = 1u; // to avoid division by 0
elapsed_time_right = curr_time_right - prev_time_right;
prev_time_right = curr_time_right;
static float speed_right = 0.f;
speed_right
= (float)(1.02101761242f / (elapsed_time_right / 1000000.f));
printf("right speed: %f cm/s\n", speed_right);
static float control_signal = 0.f;
static float integral = 0.f;
static float prev_error = 0.f;
control_signal = compute_pid(
*(float *)pvParameters, speed_right, &integral, &prev_error);
static float new_pwm = START_SPEED;
if (new_pwm + control_signal > MAX_SPEED)
{
new_pwm = MAX_SPEED;
}
else if (new_pwm + control_signal < MIN_SPEED)
{
new_pwm = MIN_SPEED;
}
else
{
new_pwm = new_pwm + control_signal;
}
printf("control signal: %f\n", control_signal);
printf("new pwm: %f\n\n", new_pwm);
set_wheel_speed(new_pwm, 1u);
speed = 0.f;
printf("stopped\n");
}
control_signal = compute_pid(
&(p_motor_speed->target_speed_cms), &speed, &integral, &prev_error);
if (new_pwm + control_signal > MAX_SPEED)
{
new_pwm = MAX_SPEED;
}
else if (new_pwm + control_signal < MIN_SPEED)
{
new_pwm = MIN_SPEED;
}
else
{
new_pwm = new_pwm + control_signal;
}
printf("control signal: %f\n", control_signal);
printf("new pwm: %f\n\n", new_pwm);
pwm_set_chan_level(*p_motor_speed->p_slice_num,
p_motor_speed->pwm_channel,
(int16_t)new_pwm);
}
}

61
frtos/motor/motor_test.c
View File

@ -3,49 +3,65 @@
#include "motor_speed.h"
#include "motor_direction.h"
#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
void
test_speed_change_task(void *p_param)
{
for (;;)
{
g_motor_speed_left.target_speed_cms = 20.0f;
g_motor_speed_right.target_speed_cms = 20.0f;
vTaskDelay(pdMS_TO_TICKS(10000));
g_motor_speed_left.target_speed_cms = 40.0f;
g_motor_speed_right.target_speed_cms = 40.0f;
vTaskDelay(pdMS_TO_TICKS(10000));
}
}
void
launch()
{
// 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_right_wheel_sensor_isr_handler);
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_left_wheel_sensor_isr_handler);
gpio_add_raw_irq_handler(SPEED_PIN_LEFT, h_wheel_sensor_isr_handler);
irq_set_enabled(IO_IRQ_BANK0, true);
static volatile float * p_target_speed = NULL;
static volatile float target_speed = 20.0f; // cm/s
p_target_speed = &target_speed;
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_left_wheel_speed_task,
"monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *) p_target_speed,
READ_LEFT_WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle);
// TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
// xTaskCreate(monitor_wheel_speed_task,
// "monitor_left_wheel_speed_task",
// configMINIMAL_STACK_SIZE,
// (void *)&g_motor_speed_left,
// WHEEL_SPEED_PRIO,
// &h_monitor_left_wheel_speed_task_handle);
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_right_wheel_speed_task,
"monitor_right_wheel_speed_task",
xTaskCreate(monitor_wheel_speed_task,
"monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *) p_target_speed,
READ_RIGHT_WHEEL_SPEED_PRIO,
(void *)&g_motor_speed_right,
WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle);
TaskHandle_t h_test_speed_change_task_handle = NULL;
xTaskCreate(test_speed_change_task,
"test_speed_change_task",
configMINIMAL_STACK_SIZE,
NULL,
WHEEL_SPEED_PRIO,
&h_test_speed_change_task_handle);
vTaskStartScheduler();
}
int
main (void)
main(void)
{
stdio_usb_init();
@ -54,7 +70,6 @@ main (void)
motor_init();
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
set_wheel_speed(START_SPEED, 0u);
launch();

31
frtos/rtos_car.c
View File

@ -31,37 +31,6 @@
void
launch()
{
// 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_right_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_left_wheel_sensor_isr_handler);
irq_set_enabled(IO_IRQ_BANK0, true);
static volatile float * p_target_speed = NULL;
static volatile float target_speed = 20.0f; // cm/s
p_target_speed = &target_speed;
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_left_wheel_speed_task,
"monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *) p_target_speed,
READ_LEFT_WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle);
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_right_wheel_speed_task,
"monitor_right_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *) p_target_speed,
READ_RIGHT_WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle);
struct repeating_timer g_left_sensor_timer;
add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,