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

modified pid with test function to change speed

This commit is contained in:
Richie 2023-10-21 10:22:12 +08:00
parent 9a9aac406e
commit 1a350fcc01
5 changed files with 70 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
frtos/config/motor_config.h
View File

@ -24,26 +24,27 @@
#define PWM_CLK_DIV 250.f #define PWM_CLK_DIV 250.f
#define PWM_WRAP 5000U #define PWM_WRAP 5000U
#define PID_KP 10.f #define PID_KP 3.f
#define PID_KI 0.0f #define PID_KI 0.01f
#define PID_KD 0.0f #define PID_KD 0.05f
#define START_SPEED 0U
#define MAX_SPEED 4900U #define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed #define MIN_SPEED 0U // To be changed
/*! /*!
* @brief Structure for the motor speed * @brief Structure for the motor speed
* @param side The side of the motor, 0 for left, 1 for right * @param target_speed The target speed of the wheel, in cm/s
* @param target_speed The target speed of the motor in cm/s * @param pwm_level The pwm level of the wheel, from 0 to 5000
* @param pwm_level The current pwm level of the motor, in range [0, 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 { typedef struct {
float target_speed; float target_speed_cms;
uint16_t pwm_level; uint16_t pwm_level;
SemaphoreHandle_t * sem; SemaphoreHandle_t * p_sem;
uint * p_slice_num; uint * p_slice_num;
uint channel; uint pwm_channel;
} motor_speed_t; } motor_speed_t;
#endif /* MOTOR_CONFIG_H */ #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_left = NULL;
SemaphoreHandle_t g_wheel_speed_sem_right = 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 void
motor_init(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_left, true);
pwm_set_enabled(g_slice_num_right, true); pwm_set_enabled(g_slice_num_right, true);
} }
#endif /* MOTOR_INIT_H */ #endif /* MOTOR_INIT_H */

26
frtos/motor/motor_speed.h
View File

@ -45,10 +45,10 @@ h_wheel_sensor_isr_handler(void)
* @return The control signal * @return The control signal
*/ */
float float
compute_pid(const volatile float * target_speed, compute_pid(const volatile float *target_speed,
const float * current_speed, const float *current_speed,
float * integral, float *integral,
float * prev_error) float *prev_error)
{ {
float error = *target_speed - *current_speed; float error = *target_speed - *current_speed;
*integral += error; *integral += error;
@ -73,7 +73,6 @@ monitor_wheel_speed_task(void *pvParameters)
{ {
volatile motor_speed_t *p_motor_speed = NULL; volatile motor_speed_t *p_motor_speed = NULL;
p_motor_speed = (motor_speed_t *)pvParameters; p_motor_speed = (motor_speed_t *)pvParameters;
SemaphoreHandle_t *p_sem = p_motor_speed->sem;
float speed = 0.f; float speed = 0.f;
@ -89,8 +88,8 @@ monitor_wheel_speed_task(void *pvParameters)
for (;;) for (;;)
{ {
if (xSemaphoreTake(*p_sem, pdMS_TO_TICKS(1000)) == if (xSemaphoreTake(*p_motor_speed->p_sem, pdMS_TO_TICKS(100))
pdTRUE) == pdTRUE)
{ {
curr_time = time_us_64(); curr_time = time_us_64();
elapsed_time = curr_time - prev_time; elapsed_time = curr_time - prev_time;
@ -106,13 +105,12 @@ monitor_wheel_speed_task(void *pvParameters)
} }
else else
{ {
printf("speed: 0 cm/s\n"); speed = 0.f;
printf("stopped\n");
} }
control_signal = compute_pid(&(p_motor_speed->target_speed), control_signal = compute_pid(
&speed, &(p_motor_speed->target_speed_cms), &speed, &integral, &prev_error);
&integral,
&prev_error);
if (new_pwm + control_signal > MAX_SPEED) if (new_pwm + control_signal > MAX_SPEED)
{ {
@ -131,7 +129,7 @@ monitor_wheel_speed_task(void *pvParameters)
printf("new pwm: %f\n\n", new_pwm); printf("new pwm: %f\n\n", new_pwm);
pwm_set_chan_level(*p_motor_speed->p_slice_num, pwm_set_chan_level(*p_motor_speed->p_slice_num,
p_motor_speed->channel, p_motor_speed->pwm_channel,
(int16_t) new_pwm); (int16_t)new_pwm);
} }
} }

66
frtos/motor/motor_test.c
View File

@ -3,8 +3,22 @@
#include "motor_speed.h" #include "motor_speed.h"
#include "motor_direction.h" #include "motor_direction.h"
#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL) #define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define READ_RIGHT_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 void
launch() launch()
@ -19,47 +33,35 @@ launch()
irq_set_enabled(IO_IRQ_BANK0, true); irq_set_enabled(IO_IRQ_BANK0, true);
static volatile motor_speed_t * p_motor_speed_left = NULL; // TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
static volatile motor_speed_t motor_speed_left = { // xTaskCreate(monitor_wheel_speed_task,
.target_speed = 40.0f, // "monitor_left_wheel_speed_task",
.pwm_level = 0u, // configMINIMAL_STACK_SIZE,
.sem = &g_wheel_speed_sem_left, // (void *)&g_motor_speed_left,
.p_slice_num = &g_slice_num_left, // WHEEL_SPEED_PRIO,
.channel = PWM_CHAN_A // &h_monitor_left_wheel_speed_task_handle);
};
p_motor_speed_left = &motor_speed_left;
static volatile motor_speed_t * p_motor_speed_right = NULL;
static volatile motor_speed_t motor_speed_right = {
.target_speed = 20.0f,
.pwm_level = 0u,
.sem = &g_wheel_speed_sem_right,
.p_slice_num = &g_slice_num_right,
.channel = PWM_CHAN_B
};
p_motor_speed_right = &motor_speed_right;
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
xTaskCreate(monitor_wheel_speed_task,
"monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *) p_motor_speed_left,
READ_LEFT_WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle);
TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
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 *)p_motor_speed_right, (void *)&g_motor_speed_right,
READ_RIGHT_WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle); &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(); vTaskStartScheduler();
} }
int int
main (void) main(void)
{ {
stdio_usb_init(); stdio_usb_init();

3
frtos/rtos_car.c
View File

@ -15,9 +15,6 @@
#include "motor_speed.h" #include "motor_speed.h"
#include "motor_direction.h" #include "motor_direction.h"
#define READ_LEFT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
#define READ_RIGHT_WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
void void
launch() launch()
{ {