diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h index 55c6646..2646ae2 100644 --- a/frtos/config/motor_config.h +++ b/frtos/config/motor_config.h @@ -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 @@ -27,7 +28,7 @@ #define PID_KI 0.0f #define PID_KD 0.0f -#define START_SPEED 4900U +#define START_SPEED 0U #define MAX_SPEED 4900U #define MIN_SPEED 0U // To be changed diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index d3a19df..cc989e0 100644 --- a/frtos/motor/motor_speed.h +++ b/frtos/motor/motor_speed.h @@ -79,24 +79,22 @@ compute_pid(float target_speed, void monitor_left_wheel_speed_task(void *pvParameters) { - // static float * target_speed = NULL; - // *target_speed = * (float *) pvParameters; + static volatile float speed_left = 0.f; + static volatile uint64_t curr_time_left = 0u; + curr_time_left = time_us_64(); 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 elapsed_time_left = 1u; // to avoid division by 0 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 // distance = circumference / 20 // circumference = 2 * pi * 3.25 cm = 20.4203522483 cm @@ -105,52 +103,54 @@ monitor_left_wheel_speed_task(void *pvParameters) = (float)(1.02101761242f / (elapsed_time_left / 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); } + else + { + printf("left speed: 0 cm/s\n"); + } + + 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); } } void monitor_right_wheel_speed_task(void *pvParameters) { - // volatile float * target_speed = (float *) pvParameters; - - // static volatile float * target_speed = NULL; - // target_speed = (float *) pvParameters; + static volatile float speed_right = 0.f; + static volatile uint64_t curr_time_right = 0u; + curr_time_right = time_us_64(); 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 elapsed_time_right = 1u; // to avoid division by 0 @@ -158,39 +158,42 @@ monitor_right_wheel_speed_task(void *pvParameters) 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); } + else + { + curr_time_right = time_us_64(); + printf("right speed: 0 cm/s\n"); + } + + 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); } } \ No newline at end of file diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c index fc802b9..7c58ee1 100644 --- a/frtos/motor/motor_test.c +++ b/frtos/motor/motor_test.c @@ -22,7 +22,7 @@ launch() irq_set_enabled(IO_IRQ_BANK0, true); 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; TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; @@ -54,7 +54,6 @@ main (void) motor_init(); set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); - set_wheel_speed(START_SPEED, 0u); launch();