From aeb39084ab41c10b3c8c2f4eafc100cb9e5cc63d Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Fri, 13 Oct 2023 16:41:13 +0800 Subject: [PATCH 1/4] interrupt based speed monitoring --- frtos/CMakeLists.txt | 6 +- frtos/car/rtos_car.c | 12 +++ frtos/car/wheel.h | 197 +++++++++++++++++++++++-------------------- 3 files changed, 119 insertions(+), 96 deletions(-) diff --git a/frtos/CMakeLists.txt b/frtos/CMakeLists.txt index ba96573..d972636 100644 --- a/frtos/CMakeLists.txt +++ b/frtos/CMakeLists.txt @@ -1,6 +1,4 @@ include(FreeRTOS_Kernel_import.cmake) -add_subdirectory( - car - line_sensor -) +add_subdirectory(car) +add_subdirectory(line_sensor) diff --git a/frtos/car/rtos_car.c b/frtos/car/rtos_car.c index adffa9f..2b925b3 100644 --- a/frtos/car/rtos_car.c +++ b/frtos/car/rtos_car.c @@ -25,6 +25,18 @@ void launch() { + // isr to detect left wheel slot + gpio_set_irq_enabled_with_callback(SPEED_PIN_RIGHT, + GPIO_IRQ_EDGE_FALL, + true, + &right_wheel_sensor_isr); + + // isr to detect right wheel slot + gpio_set_irq_enabled_with_callback(SPEED_PIN_LEFT, + GPIO_IRQ_EDGE_FALL, + true, + &left_wheel_sensor_isr); + TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; xTaskCreate(monitor_left_wheel_speed_task, "monitor_left_wheel_speed_task", diff --git a/frtos/car/wheel.h b/frtos/car/wheel.h index 8eda25d..9a46b99 100644 --- a/frtos/car/wheel.h +++ b/frtos/car/wheel.h @@ -26,7 +26,58 @@ uint g_slice_num_left = 0U; uint g_slice_num_right = 0U; -SemaphoreHandle_t g_wheel_speed_sem = NULL; +SemaphoreHandle_t g_wheel_speed_sem_left = NULL; +SemaphoreHandle_t g_wheel_speed_sem_right = NULL; + +void +wheel_setup(void) +{ + // Semaphore + g_wheel_speed_sem_left = xSemaphoreCreateBinary(); + g_wheel_speed_sem_right = xSemaphoreCreateBinary(); + + // Speed + /* adc_init(); + adc_gpio_init(SPEED_PIN_RIGHT); + adc_gpio_init(SPEED_PIN_LEFT);*/ + + gpio_init(SPEED_PIN_RIGHT); + gpio_init(SPEED_PIN_LEFT); + gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN); + gpio_set_dir(SPEED_PIN_LEFT, GPIO_IN); + + // Initialize direction pins as outputs + gpio_init(DIRECTION_PIN_RIGHT_IN1); + gpio_init(DIRECTION_PIN_RIGHT_IN2); + gpio_init(DIRECTION_PIN_LEFT_IN3); + gpio_init(DIRECTION_PIN_LEFT_IN4); + + gpio_set_dir(DIRECTION_PIN_RIGHT_IN1, GPIO_OUT); + gpio_set_dir(DIRECTION_PIN_RIGHT_IN2, GPIO_OUT); + gpio_set_dir(DIRECTION_PIN_LEFT_IN3, GPIO_OUT); + gpio_set_dir(DIRECTION_PIN_LEFT_IN4, GPIO_OUT); + + // Initialise PWM + gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM); + gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM); + + g_slice_num_left = pwm_gpio_to_slice_num(PWM_PIN_LEFT); + g_slice_num_right = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); + + // NOTE: PWM clock is 125MHz for raspberrypi pico w by default + + // 125MHz / 250 = 500kHz + pwm_set_clkdiv(g_slice_num_left, PWM_CLK_DIV); + pwm_set_clkdiv(g_slice_num_right, PWM_CLK_DIV); + + // have them to be 500kHz / 5000 = 100Hz + pwm_set_wrap(g_slice_num_left, (PWM_WRAP - 1U)); + pwm_set_wrap(g_slice_num_right, (PWM_WRAP - 1U)); + + pwm_set_enabled(g_slice_num_left, true); + pwm_set_enabled(g_slice_num_right, true); +} + /*! * @brief Set the direction of the wheels; can use bitwise OR to set both @@ -50,67 +101,6 @@ set_wheel_direction (uint32_t direction) gpio_set_mask(direction); } -uint8_t -get_wheel_slot_count (uint adc_pin) -{ - adc_select_input(adc_pin); - - uint8_t count = 0u; - TickType_t xEndTime = xTaskGetTickCount() + - pdMS_TO_TICKS(SPEED_READING_TRESHOLD_MSEC); - - static volatile bool b_is_adc_high = false; - - while (xTaskGetTickCount() < xEndTime) - { - /* - * If the sensor value is below the threshold and the previous reading - * was above the threshold, then increment the count. - */ - if (b_is_adc_high && (adc_read() < ADC_READING_TRESHOLD)) - { - count++; - b_is_adc_high = false; - } - else if (!b_is_adc_high && (adc_read() >= ADC_READING_TRESHOLD)) - { - b_is_adc_high = true; - } - } - - b_is_adc_high = false; - - return count; - -} - -void -monitor_left_wheel_speed_task (__unused void * p_params) -{ - for (;;) - { - xSemaphoreTake(g_wheel_speed_sem, portMAX_DELAY); - printf("left wheel: %d\n", get_wheel_slot_count(0)); - xSemaphoreGive(g_wheel_speed_sem); - vTaskDelay(pdMS_TO_TICKS(10)); - } -} - -void -monitor_right_wheel_speed_task (__unused void * p_params) -{ - static uint8_t count = 0u; - - for (;;) - { - xSemaphoreTake(g_wheel_speed_sem, portMAX_DELAY); - count = get_wheel_slot_count(1); - printf("right wheel: %f rounds\n", (((float) count) / 20.f)); - xSemaphoreGive(g_wheel_speed_sem); - vTaskDelay(pdMS_TO_TICKS(10)); - } -} - void set_wheel_speed (float speed) { @@ -124,44 +114,67 @@ set_wheel_speed (float speed) } void -wheel_setup(void) +left_wheel_sensor_isr (__unused uint gpio, __unused uint32_t events) { - // Semaphore - g_wheel_speed_sem = xSemaphoreCreateMutex(); + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} - // Speed - adc_init(); - adc_gpio_init(SPEED_PIN_RIGHT); - adc_gpio_init(SPEED_PIN_LEFT); +void +right_wheel_sensor_isr (__unused uint gpio, __unused uint32_t events) +{ + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} - // Initialize direction pins as outputs - gpio_init(DIRECTION_PIN_RIGHT_IN1); - gpio_init(DIRECTION_PIN_RIGHT_IN2); - gpio_init(DIRECTION_PIN_LEFT_IN3); - gpio_init(DIRECTION_PIN_LEFT_IN4); +void +monitor_left_wheel_speed_task (__unused void *pvParameters) +{ + for (;;) + { + if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE) + { + static uint64_t curr_time_left = 0u; + curr_time_left = time_us_64(); - gpio_set_dir(DIRECTION_PIN_RIGHT_IN1, GPIO_OUT); - gpio_set_dir(DIRECTION_PIN_RIGHT_IN2, GPIO_OUT); - gpio_set_dir(DIRECTION_PIN_LEFT_IN3, GPIO_OUT); - gpio_set_dir(DIRECTION_PIN_LEFT_IN4, GPIO_OUT); - // Initialise PWM - gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM); - gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM); + static uint64_t prev_time_left = 0u; + static uint64_t elapsed_time_left = 0u; - g_slice_num_left = pwm_gpio_to_slice_num(PWM_PIN_LEFT); - g_slice_num_right = pwm_gpio_to_slice_num(PWM_PIN_RIGHT); + elapsed_time_left = curr_time_left - prev_time_left; - // NOTE: PWM clock is 125MHz for raspberrypi pico w by default + printf("time elapsed: %llu\n", elapsed_time_left); - // 125MHz / 250 = 500kHz - pwm_set_clkdiv(g_slice_num_left, PWM_CLK_DIV); - pwm_set_clkdiv(g_slice_num_right, PWM_CLK_DIV); + prev_time_left = curr_time_left; - // have them to be 500kHz / 5000 = 100Hz - pwm_set_wrap(g_slice_num_left, (PWM_WRAP - 1U)); - pwm_set_wrap(g_slice_num_right, (PWM_WRAP - 1U)); + } + + } + +} + +void +monitor_right_wheel_speed_task (__unused void *pvParameters) +{ + for (;;) + { + if (xSemaphoreTake(g_wheel_speed_sem_right, portMAX_DELAY) == pdTRUE) + { + 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 = 0u; + + elapsed_time_right = curr_time_right - prev_time_right; + + printf("time elapsed: %llu\n", elapsed_time_right); + + prev_time_right = curr_time_right; + } + + } - pwm_set_enabled(g_slice_num_left, true); - pwm_set_enabled(g_slice_num_right, true); } \ No newline at end of file From 84e22bc48861aa1912891284f8a713030eea794a Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Fri, 13 Oct 2023 17:44:57 +0800 Subject: [PATCH 2/4] modified speed function; prep for pid --- frtos/car/rtos_car.c | 2 +- frtos/car/wheel.h | 25 ++++++++++++++++++------- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/frtos/car/rtos_car.c b/frtos/car/rtos_car.c index 2b925b3..c46b6e5 100644 --- a/frtos/car/rtos_car.c +++ b/frtos/car/rtos_car.c @@ -64,7 +64,7 @@ main (void) sleep_ms(2000); set_wheel_direction(DIRECTION_RIGHT_FORWARD); - set_wheel_speed(1.f); + set_wheel_speed(1.f, 1u); launch(); diff --git a/frtos/car/wheel.h b/frtos/car/wheel.h index 9a46b99..b9932cc 100644 --- a/frtos/car/wheel.h +++ b/frtos/car/wheel.h @@ -101,16 +101,27 @@ set_wheel_direction (uint32_t direction) gpio_set_mask(direction); } +/*! + * @brief Set the speed of the wheels; can use bitwise OR to set both + * @param speed in range [0.0, 1.0] + * @param side 0 for left, 1 for right + */ void -set_wheel_speed (float speed) +set_wheel_speed (float speed, uint8_t side) { - pwm_set_chan_level(g_slice_num_left, - PWM_CHAN_A, - (short) (PWM_WRAP * speed)); + if (side == 0U) + { + pwm_set_chan_level(g_slice_num_left, + PWM_CHAN_A, + (short) (PWM_WRAP * speed)); + } + else + { + pwm_set_chan_level(g_slice_num_right, + PWM_CHAN_B, + (short) (PWM_WRAP * speed)); + } - pwm_set_chan_level(g_slice_num_right, - PWM_CHAN_B, - (short) (PWM_WRAP * speed)); } void From 4862fdc23a57705160f487c5e11dc767156cc8f6 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Fri, 13 Oct 2023 19:19:49 +0800 Subject: [PATCH 3/4] fixed interrupt issue --- frtos/car/rtos_car.c | 20 ++++++++-------- frtos/car/wheel.h | 57 +++++++++++++++++++++++++++++++------------- 2 files changed, 51 insertions(+), 26 deletions(-) diff --git a/frtos/car/rtos_car.c b/frtos/car/rtos_car.c index c46b6e5..70aa7e9 100644 --- a/frtos/car/rtos_car.c +++ b/frtos/car/rtos_car.c @@ -25,17 +25,17 @@ void launch() { - // isr to detect left wheel slot - gpio_set_irq_enabled_with_callback(SPEED_PIN_RIGHT, - GPIO_IRQ_EDGE_FALL, - true, - &right_wheel_sensor_isr); - // isr to detect right wheel slot - gpio_set_irq_enabled_with_callback(SPEED_PIN_LEFT, - GPIO_IRQ_EDGE_FALL, - true, - &left_wheel_sensor_isr); + 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 wheel 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); TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL; xTaskCreate(monitor_left_wheel_speed_task, diff --git a/frtos/car/wheel.h b/frtos/car/wheel.h index b9932cc..ef50f58 100644 --- a/frtos/car/wheel.h +++ b/frtos/car/wheel.h @@ -13,8 +13,8 @@ #define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) #define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) -#define SPEED_PIN_RIGHT 27U // ADC0 -#define SPEED_PIN_LEFT 26U // ADC1 +#define SPEED_PIN_RIGHT 15U +#define SPEED_PIN_LEFT 16U #define PWM_CLK_DIV 250.f #define PWM_WRAP 5000U @@ -125,19 +125,31 @@ set_wheel_speed (float speed, uint8_t side) } void -left_wheel_sensor_isr (__unused uint gpio, __unused uint32_t events) +h_left_wheel_sensor_isr_handler (void) { - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + 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 wheel sensor isr\n"); + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } } void -right_wheel_sensor_isr (__unused uint gpio, __unused uint32_t events) +h_right_wheel_sensor_isr_handler (void) { - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + 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 wheel sensor isr\n"); + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } } void @@ -152,14 +164,22 @@ monitor_left_wheel_speed_task (__unused void *pvParameters) static uint64_t prev_time_left = 0u; - static uint64_t elapsed_time_left = 0u; + static uint64_t elapsed_time_left = 1u; // to avoid division by 0 elapsed_time_left = curr_time_left - prev_time_left; - printf("time elapsed: %llu\n", elapsed_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)); + + printf("left speed: %f cm/s\n", speed_left); + } } @@ -177,13 +197,18 @@ monitor_right_wheel_speed_task (__unused void *pvParameters) curr_time_right = time_us_64(); static uint64_t prev_time_right = 0u; - static uint64_t elapsed_time_right = 0u; + static uint64_t elapsed_time_right = 1u; // to avoid division by 0 elapsed_time_right = curr_time_right - prev_time_right; - printf("time elapsed: %llu\n", elapsed_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); } } From 376453398aca654f473da1b71f013471ac73efac Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Fri, 13 Oct 2023 23:29:42 +0800 Subject: [PATCH 4/4] implemented pid for right wheel, pending fine-tuning --- frtos/car/rtos_car.c | 12 ++++--- frtos/car/wheel.h | 85 +++++++++++++++++++++++++++++++++++--------- 2 files changed, 77 insertions(+), 20 deletions(-) diff --git a/frtos/car/rtos_car.c b/frtos/car/rtos_car.c index 70aa7e9..b3d4474 100644 --- a/frtos/car/rtos_car.c +++ b/frtos/car/rtos_car.c @@ -37,11 +37,15 @@ launch() 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, - NULL, + (void *) p_target_speed, READ_LEFT_WHEEL_SPEED_PRIO, &h_monitor_left_wheel_speed_task_handle); @@ -49,7 +53,7 @@ launch() xTaskCreate(monitor_right_wheel_speed_task, "monitor_right_wheel_speed_task", configMINIMAL_STACK_SIZE, - NULL, + (void *) p_target_speed, READ_RIGHT_WHEEL_SPEED_PRIO, &h_monitor_right_wheel_speed_task_handle); @@ -61,10 +65,10 @@ main (void) { stdio_usb_init(); wheel_setup(); - sleep_ms(2000); + sleep_ms(5000); set_wheel_direction(DIRECTION_RIGHT_FORWARD); - set_wheel_speed(1.f, 1u); + set_wheel_speed(START_SPEED, 1u); launch(); diff --git a/frtos/car/wheel.h b/frtos/car/wheel.h index ef50f58..41056a2 100644 --- a/frtos/car/wheel.h +++ b/frtos/car/wheel.h @@ -19,9 +19,13 @@ #define PWM_CLK_DIV 250.f #define PWM_WRAP 5000U -#define ADC_READING_TRESHOLD 3500u +#define PID_KP 10.f +#define PID_KI 0.0f +#define PID_KD 0.0f -#define SPEED_READING_TRESHOLD_MSEC 1000u +#define START_SPEED 1500U +#define MAX_SPEED 4900U +#define MIN_SPEED 0U // To be changed uint g_slice_num_left = 0U; uint g_slice_num_right = 0U; @@ -36,11 +40,6 @@ wheel_setup(void) g_wheel_speed_sem_left = xSemaphoreCreateBinary(); g_wheel_speed_sem_right = xSemaphoreCreateBinary(); - // Speed - /* adc_init(); - adc_gpio_init(SPEED_PIN_RIGHT); - adc_gpio_init(SPEED_PIN_LEFT);*/ - gpio_init(SPEED_PIN_RIGHT); gpio_init(SPEED_PIN_LEFT); gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN); @@ -103,7 +102,7 @@ set_wheel_direction (uint32_t direction) /*! * @brief Set the speed of the wheels; can use bitwise OR to set both - * @param speed in range [0.0, 1.0] + * @param speed in range [0, 5000] * @param side 0 for left, 1 for right */ void @@ -112,14 +111,14 @@ set_wheel_speed (float speed, uint8_t side) if (side == 0U) { pwm_set_chan_level(g_slice_num_left, - PWM_CHAN_A, - (short) (PWM_WRAP * speed)); + PWM_CHAN_A, + (uint16_t) speed); } else { pwm_set_chan_level(g_slice_num_right, PWM_CHAN_B, - (short) (PWM_WRAP * speed)); + (uint16_t) speed); } } @@ -131,7 +130,7 @@ h_left_wheel_sensor_isr_handler (void) { gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); - printf("left wheel sensor isr\n"); + // printf("left wheel sensor isr\n"); BaseType_t xHigherPriorityTaskWoken = pdFALSE; xSemaphoreGiveFromISR(g_wheel_speed_sem_left, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); @@ -145,16 +144,36 @@ h_right_wheel_sensor_isr_handler (void) { gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL); - printf("right wheel sensor isr\n"); + // printf("right wheel sensor isr\n"); BaseType_t xHigherPriorityTaskWoken = pdFALSE; xSemaphoreGiveFromISR(g_wheel_speed_sem_right, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } } -void -monitor_left_wheel_speed_task (__unused void *pvParameters) +float +compute_pid(float target_speed, float current_speed, float * integral, float * prev_error) { + float error = target_speed - current_speed; + *integral += error; + + float derivative = error - *prev_error; + + float control_signal = PID_KP * error + + PID_KI * (*integral) + + PID_KD * derivative; + + *prev_error = error; + + return control_signal; +} + +void +monitor_left_wheel_speed_task (void *pvParameters) +{ + static float * target_speed = NULL; + *target_speed = * (float *) pvParameters; + for (;;) { if (xSemaphoreTake(g_wheel_speed_sem_left, portMAX_DELAY) == pdTRUE) @@ -187,8 +206,12 @@ monitor_left_wheel_speed_task (__unused void *pvParameters) } void -monitor_right_wheel_speed_task (__unused void *pvParameters) +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) @@ -209,6 +232,36 @@ monitor_right_wheel_speed_task (__unused void *pvParameters) (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(*target_speed, + 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); + } }