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

added wheel stop state check; now will start the motor from 0 to desire speed; no enough wire to test both motor at the same time, but tested one by one

This commit is contained in:
Richie 2023-10-20 23:48:35 +08:00
parent ea9318b747
commit 81e44dbf50
3 changed files with 80 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
frtos/config/motor_config.h
View File

@ -3,9 +3,10 @@
#define MOTOR_CONFIG_H #define MOTOR_CONFIG_H
// ENA and ENB on the L298N // ENA and ENB on the L298N
#define PWM_PIN_LEFT 0U // chanel A
#define PWM_PIN_RIGHT 1U // chanel B #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_IN1 11U
#define DIRECTION_PIN_RIGHT_IN2 12U #define DIRECTION_PIN_RIGHT_IN2 12U
@ -27,7 +28,7 @@
#define PID_KI 0.0f #define PID_KI 0.0f
#define PID_KD 0.0f #define PID_KD 0.0f
#define START_SPEED 4900U #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

39
frtos/motor/motor_speed.h
View File

@ -79,24 +79,22 @@ compute_pid(float target_speed,
void void
monitor_left_wheel_speed_task(void *pvParameters) monitor_left_wheel_speed_task(void *pvParameters)
{ {
// static float * target_speed = NULL; static volatile float speed_left = 0.f;
// *target_speed = * (float *) pvParameters; static volatile uint64_t curr_time_left = 0u;
curr_time_left = time_us_64();
for (;;) for (;;)
{ {
if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE) if (xSemaphoreTake(g_wheel_speed_sem_left, pdMS_TO_TICKS(1000)) ==
pdTRUE)
{ {
static uint64_t curr_time_left = 0u;
curr_time_left = time_us_64(); curr_time_left = time_us_64();
static uint64_t prev_time_left = 0u; static uint64_t prev_time_left = 0u;
static uint64_t elapsed_time_left = 1u; // to avoid division by 0 static uint64_t elapsed_time_left = 1u; // to avoid division by 0
elapsed_time_left = curr_time_left - prev_time_left; elapsed_time_left = curr_time_left - prev_time_left;
prev_time_left = curr_time_left; prev_time_left = curr_time_left;
static float speed_left = 0.f;
// speed in cm/s; speed = distance / time // speed in cm/s; speed = distance / time
// distance = circumference / 20 // distance = circumference / 20
// circumference = 2 * pi * 3.25 cm = 20.4203522483 cm // circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
@ -105,6 +103,11 @@ monitor_left_wheel_speed_task(void *pvParameters)
= (float)(1.02101761242f / (elapsed_time_left / 1000000.f)); = (float)(1.02101761242f / (elapsed_time_left / 1000000.f));
printf("left speed: %f cm/s\n", speed_left); printf("left speed: %f cm/s\n", speed_left);
}
else
{
printf("left speed: 0 cm/s\n");
}
static float control_signal = 0.f; static float control_signal = 0.f;
static float integral = 0.f; static float integral = 0.f;
@ -133,24 +136,21 @@ monitor_left_wheel_speed_task(void *pvParameters)
set_wheel_speed(new_pwm, 0u); set_wheel_speed(new_pwm, 0u);
} }
}
} }
void void
monitor_right_wheel_speed_task(void *pvParameters) monitor_right_wheel_speed_task(void *pvParameters)
{ {
// volatile float * target_speed = (float *) pvParameters; static volatile float speed_right = 0.f;
static volatile uint64_t curr_time_right = 0u;
// static volatile float * target_speed = NULL; curr_time_right = time_us_64();
// target_speed = (float *) pvParameters;
for (;;) for (;;)
{ {
if (xSemaphoreTake(g_wheel_speed_sem_right, portMAX_DELAY) == pdTRUE) if (xSemaphoreTake(g_wheel_speed_sem_right, pdMS_TO_TICKS(1000)) ==
pdTRUE)
{ {
static uint64_t curr_time_right = 0u;
curr_time_right = time_us_64(); curr_time_right = time_us_64();
static uint64_t prev_time_right = 0u; static uint64_t prev_time_right = 0u;
static uint64_t elapsed_time_right = 1u; // to avoid division by 0 static uint64_t elapsed_time_right = 1u; // to avoid division by 0
@ -158,12 +158,16 @@ monitor_right_wheel_speed_task(void *pvParameters)
prev_time_right = curr_time_right; prev_time_right = curr_time_right;
static float speed_right = 0.f;
speed_right speed_right
= (float)(1.02101761242f / (elapsed_time_right / 1000000.f)); = (float)(1.02101761242f / (elapsed_time_right / 1000000.f));
printf("right speed: %f cm/s\n", speed_right); printf("right speed: %f cm/s\n", speed_right);
}
else
{
curr_time_right = time_us_64();
printf("right speed: 0 cm/s\n");
}
static float control_signal = 0.f; static float control_signal = 0.f;
static float integral = 0.f; static float integral = 0.f;
@ -192,5 +196,4 @@ monitor_right_wheel_speed_task(void *pvParameters)
set_wheel_speed(new_pwm, 1u); set_wheel_speed(new_pwm, 1u);
} }
}
} }

3
frtos/motor/motor_test.c
View File

@ -22,7 +22,7 @@ launch()
irq_set_enabled(IO_IRQ_BANK0, true); irq_set_enabled(IO_IRQ_BANK0, true);
static volatile float * p_target_speed = NULL; static volatile float * p_target_speed = NULL;
static volatile float target_speed = 20.0f; // cm/s static volatile float target_speed = 30.0f; // cm/s
p_target_speed = &target_speed; p_target_speed = &target_speed;
TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
@ -54,7 +54,6 @@ main (void)
motor_init(); motor_init();
set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
set_wheel_speed(START_SPEED, 0u);
launch(); launch();