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_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 0U
#define MAX_SPEED 4900U
#define MIN_SPEED 0U // To be changed
/*!
* @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 motor in cm/s
* @param pwm_level The current pwm level of the motor, in range [0, 5000]
* @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;
float target_speed_cms;
uint16_t pwm_level;
SemaphoreHandle_t * sem;
SemaphoreHandle_t * p_sem;
uint * p_slice_num;
uint channel;
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 */

28
frtos/motor/motor_speed.h
View File

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

66
frtos/motor/motor_test.c
View File

@ -3,8 +3,22 @@
#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()
@ -19,47 +33,35 @@ launch()
irq_set_enabled(IO_IRQ_BANK0, true);
static volatile motor_speed_t * p_motor_speed_left = NULL;
static volatile motor_speed_t motor_speed_left = {
.target_speed = 40.0f,
.pwm_level = 0u,
.sem = &g_wheel_speed_sem_left,
.p_slice_num = &g_slice_num_left,
.channel = PWM_CHAN_A
};
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_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_wheel_speed_task,
"monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *)p_motor_speed_right,
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();

3
frtos/rtos_car.c
View File

@ -15,9 +15,6 @@
#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)
void
launch()
{