From ea9318b74795bfec56ff676a09234af646898006 Mon Sep 17 00:00:00 2001 From: Richie <2837357W@student.gla.ac.uk> Date: Fri, 20 Oct 2023 21:26:48 +0800 Subject: [PATCH] re structured --- .clang-format | 136 +++++++++ CMakeLists.txt | 8 - frtos/CMakeLists.txt | 6 +- frtos/{ => config}/FreeRTOSConfig.h | 0 frtos/{ => config}/lwipopts.h | 0 frtos/{ => config}/lwipopts_examples_common.h | 2 +- frtos/config/motor_config.h | 34 +++ frtos/motor/CMakeLists.txt | 16 ++ frtos/motor/motor_direction.h | 30 ++ frtos/motor/motor_init.h | 73 +++++ frtos/motor/motor_speed.h | 196 +++++++++++++ frtos/motor/motor_test.c | 64 +++++ frtos/rtos_car.c | 27 +- frtos/wheel/CMakeLists.txt | 1 - frtos/wheel/wheel.h | 269 ------------------ 15 files changed, 560 insertions(+), 302 deletions(-) create mode 100644 .clang-format rename frtos/{ => config}/FreeRTOSConfig.h (100%) rename frtos/{ => config}/lwipopts.h (100%) rename frtos/{ => config}/lwipopts_examples_common.h (98%) create mode 100644 frtos/config/motor_config.h create mode 100644 frtos/motor/CMakeLists.txt create mode 100644 frtos/motor/motor_direction.h create mode 100644 frtos/motor/motor_init.h create mode 100644 frtos/motor/motor_speed.h create mode 100644 frtos/motor/motor_test.c delete mode 100644 frtos/wheel/CMakeLists.txt delete mode 100644 frtos/wheel/wheel.h diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..01b2ef9 --- /dev/null +++ b/.clang-format @@ -0,0 +1,136 @@ +--- +Language: Cpp +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveMacros: true +AlignConsecutiveAssignments: true +AlignConsecutiveDeclarations: true +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: TopLevel +AlwaysBreakAfterReturnType: TopLevelDefinitions +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterCaseLabel: false + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: false + AfterObjCDeclaration: true + AfterStruct: true + AfterUnion: true + AfterExternBlock: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: All +BreakBeforeBraces: Custom +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeComma +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeComma +BreakAfterJavaFieldAnnotations: true +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: false +DeriveLineEnding: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: false +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + - Regex: '.*' + Priority: 1 + SortPriority: 0 +IncludeIsMainRegex: '(Test)?$' +IncludeIsMainSourceRegex: '' +IndentCaseLabels: true +IndentGotoLabels: true +IndentPPDirectives: None +IndentWidth: 4 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 4 +ObjCSpaceAfterProperty: true +ObjCSpaceBeforeProtocolList: false +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Right +ReflowComments: true +SortIncludes: false +SortUsingDeclarations: false +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: true +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: false +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +Standard: Latest +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseCRLF: false +UseTab: Never +... + diff --git a/CMakeLists.txt b/CMakeLists.txt index 47277d7..ae76876 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,14 +25,6 @@ if (PICO_CYW43_SUPPORTED) # set by PICO_BOARD=pico_w message("Using WIFI_PASSWORD from environment") endif() - set(WIFI_SSID "${WIFI_SSID}" CACHE INTERNAL "WiFi SSID for examples") - set(WIFI_PASSWORD "${WIFI_PASSWORD}" CACHE INTERNAL "WiFi password for examples") - add_subdirectory(frtos) - if (NOT TARGET pico_btstack_base) - message("Skipping Pico W Bluetooth examples as support is not available") - else() - # add_subdirectory(bt) - endif() endif() endif() diff --git a/frtos/CMakeLists.txt b/frtos/CMakeLists.txt index 3a6c4f4..e9403ef 100644 --- a/frtos/CMakeLists.txt +++ b/frtos/CMakeLists.txt @@ -1,6 +1,6 @@ include(FreeRTOS_Kernel_import.cmake) -add_subdirectory(wheel) +add_subdirectory(motor) add_subdirectory(line_sensor) add_executable(rtos_car rtos_car.c) @@ -13,8 +13,8 @@ target_compile_definitions(rtos_car PRIVATE PING_USE_SOCKETS=1 ) target_include_directories(rtos_car PRIVATE - ${CMAKE_CURRENT_LIST_DIR} - ${CMAKE_CURRENT_LIST_DIR}/wheel + ${CMAKE_CURRENT_LIST_DIR}/config + ${CMAKE_CURRENT_LIST_DIR}/motor ${CMAKE_CURRENT_LIST_DIR}/line_sensor ) target_link_libraries(rtos_car diff --git a/frtos/FreeRTOSConfig.h b/frtos/config/FreeRTOSConfig.h similarity index 100% rename from frtos/FreeRTOSConfig.h rename to frtos/config/FreeRTOSConfig.h diff --git a/frtos/lwipopts.h b/frtos/config/lwipopts.h similarity index 100% rename from frtos/lwipopts.h rename to frtos/config/lwipopts.h diff --git a/frtos/lwipopts_examples_common.h b/frtos/config/lwipopts_examples_common.h similarity index 98% rename from frtos/lwipopts_examples_common.h rename to frtos/config/lwipopts_examples_common.h index 217cb13..837b4cd 100644 --- a/frtos/lwipopts_examples_common.h +++ b/frtos/config/lwipopts_examples_common.h @@ -7,7 +7,7 @@ // allow override in some examples #ifndef NO_SYS -#define NO_SYS 1 +#define NO_SYS 0 #endif // allow override in some examples #ifndef LWIP_SOCKET diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h new file mode 100644 index 0000000..55c6646 --- /dev/null +++ b/frtos/config/motor_config.h @@ -0,0 +1,34 @@ + +#ifndef MOTOR_CONFIG_H +#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 DIRECTION_PIN_RIGHT_IN1 11U +#define DIRECTION_PIN_RIGHT_IN2 12U + +#define DIRECTION_PIN_LEFT_IN3 19U +#define DIRECTION_PIN_LEFT_IN4 20U + +#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2) +#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1) +#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) +#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) + +#define SPEED_PIN_RIGHT 15U +#define SPEED_PIN_LEFT 16U + +#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 START_SPEED 4900U +#define MAX_SPEED 4900U +#define MIN_SPEED 0U // To be changed + +#endif /* MOTOR_CONFIG_H */ \ No newline at end of file diff --git a/frtos/motor/CMakeLists.txt b/frtos/motor/CMakeLists.txt new file mode 100644 index 0000000..6d4f305 --- /dev/null +++ b/frtos/motor/CMakeLists.txt @@ -0,0 +1,16 @@ +add_executable(motor_test motor_test.c) + +target_link_libraries(motor_test + pico_cyw43_arch_lwip_sys_freertos + pico_stdlib + pico_lwip_iperf + FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap + hardware_pwm +) + +target_include_directories(motor_test PRIVATE + ../config +) + +pico_enable_stdio_usb(motor_test 1) +pico_add_extra_outputs(motor_test) \ No newline at end of file diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h new file mode 100644 index 0000000..1fe604c --- /dev/null +++ b/frtos/motor/motor_direction.h @@ -0,0 +1,30 @@ +/* + * @file motor_direction.c + * @brief control the direction of the wheels by setting the GPIO mask + * @author Richie + */ + +#include "motor_init.h" + +/*! + * @brief Set the direction of the wheels; can use bitwise OR to set both + * wheels such as DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD, it will + * set the left motor to go forward and the right motor to go backward within + * the same function. + * if the motor direction is not set, it will not move. + * @param direction The direction of the left and right wheels + * @param left_speed The speed of the left motor, from 0.0 to 1.0 + * @param right_speed The speed of the right motor, from 0.0 to 1.0 + */ +void +set_wheel_direction (uint32_t direction) +{ + static const uint32_t mask = DIRECTION_LEFT_FORWARD | + DIRECTION_LEFT_BACKWARD | + DIRECTION_RIGHT_FORWARD | + DIRECTION_RIGHT_BACKWARD; + + gpio_put_masked(mask, 0U); + gpio_set_mask(direction); +} + diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h new file mode 100644 index 0000000..0522b20 --- /dev/null +++ b/frtos/motor/motor_init.h @@ -0,0 +1,73 @@ +/* + * @file motor_init.h + * @brief define the constants and initialize the motor + * @author Richie + */ + +#ifndef MOTOR_INIT_H +#define MOTOR_INIT_H + +#include + +#include "pico/cyw43_arch.h" +#include "pico/stdlib.h" +#include "hardware/pwm.h" + +#include "FreeRTOS.h" +#include "task.h" +#include "semphr.h" + +#include "motor_config.h" + +uint g_slice_num_left = 0U; +uint g_slice_num_right = 0U; + +SemaphoreHandle_t g_wheel_speed_sem_left = NULL; +SemaphoreHandle_t g_wheel_speed_sem_right = NULL; + +void +motor_init(void) +{ + // Semaphore + g_wheel_speed_sem_left = xSemaphoreCreateBinary(); + g_wheel_speed_sem_right = xSemaphoreCreateBinary(); + + 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); + +} + +#endif /* MOTOR_INIT_H */ \ No newline at end of file diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h new file mode 100644 index 0000000..d3a19df --- /dev/null +++ b/frtos/motor/motor_speed.h @@ -0,0 +1,196 @@ +/* + * @file motor_speed.c + * @brief control the speed of the wheels by setting the PWM level, and + * monitor the speed by using edge interrupt and measure the time between + * each slot of the wheel, then calculate the speed of the wheel in cm/s, and + * adjust the speed of the wheel by using PID controller, and set the new PWM + * @author Richie + */ + +#include "motor_init.h" + +/*! + * @brief Set the speed of the wheels; can use bitwise OR to set both + * @param speed in range [0, 5000] + * @param side 0 for left, 1 for right + */ +void +set_wheel_speed(float speed, uint8_t side) +{ + if (side == 0U) + { + pwm_set_chan_level(g_slice_num_left, PWM_CHAN_A, (uint16_t)speed); + } + else + { + pwm_set_chan_level(g_slice_num_right, PWM_CHAN_B, (uint16_t)speed); + } +} + +void +h_left_wheel_sensor_isr_handler(void) +{ + 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 motor sensor isr\n"); + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_wheel_speed_sem_left, + &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } +} + +void +h_right_wheel_sensor_isr_handler(void) +{ + 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 motor sensor isr\n"); + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(g_wheel_speed_sem_right, + &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } +} + +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) + { + static uint64_t curr_time_left = 0u; + 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; + + 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); + + 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; + + 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 = 1u; // to avoid division by 0 + + elapsed_time_right = curr_time_right - prev_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); + + 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 new file mode 100644 index 0000000..fc802b9 --- /dev/null +++ b/frtos/motor/motor_test.c @@ -0,0 +1,64 @@ + + +#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() +{ + // isr to detect right motor slot + 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 motor 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); + + 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, + (void *) p_target_speed, + READ_LEFT_WHEEL_SPEED_PRIO, + &h_monitor_left_wheel_speed_task_handle); + + TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL; + xTaskCreate(monitor_right_wheel_speed_task, + "monitor_right_wheel_speed_task", + configMINIMAL_STACK_SIZE, + (void *) p_target_speed, + READ_RIGHT_WHEEL_SPEED_PRIO, + &h_monitor_right_wheel_speed_task_handle); + + vTaskStartScheduler(); +} + +int +main (void) +{ + stdio_usb_init(); + + sleep_ms(2000); + printf("Test started!\n"); + + motor_init(); + set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); + set_wheel_speed(START_SPEED, 0u); + + launch(); + + // for(;;); + + return (0); +} \ No newline at end of file diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index d41ffc4..2b9e283 100644 --- a/frtos/rtos_car.c +++ b/frtos/rtos_car.c @@ -7,18 +7,13 @@ // pico sdk libraries #include "pico/cyw43_arch.h" #include "pico/stdlib.h" -#include "hardware/adc.h" -#include "hardware/pwm.h" // FreeRTOS libraries #include "FreeRTOS.h" #include "task.h" -#include "queue.h" -#include "message_buffer.h" -#include "semphr.h" -#include "wheel.h" -#include "line_sensor.h" +#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) @@ -26,15 +21,12 @@ void launch() { - - - - // isr to detect right wheel slot + // isr to detect right motor slot 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 + // isr to detect left motor 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); @@ -68,18 +60,13 @@ int main (void) { stdio_usb_init(); - wheel_setup(); - sleep_ms(5000); - set_wheel_direction(DIRECTION_RIGHT_FORWARD); - set_wheel_speed(START_SPEED, 1u); + motor_init(); + + set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD); launch(); - while (1) - { - } - return (0); } diff --git a/frtos/wheel/CMakeLists.txt b/frtos/wheel/CMakeLists.txt deleted file mode 100644 index 8b13789..0000000 --- a/frtos/wheel/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ - diff --git a/frtos/wheel/wheel.h b/frtos/wheel/wheel.h deleted file mode 100644 index 41056a2..0000000 --- a/frtos/wheel/wheel.h +++ /dev/null @@ -1,269 +0,0 @@ -// GPIO 2 as the PWM output, GPIO 26 as the ADC input -#define PWM_PIN_LEFT 0U // chanel A -#define PWM_PIN_RIGHT 1U // chanel B - -#define DIRECTION_PIN_RIGHT_IN1 11U -#define DIRECTION_PIN_RIGHT_IN2 12U - -#define DIRECTION_PIN_LEFT_IN3 19U -#define DIRECTION_PIN_LEFT_IN4 20U - -#define DIRECTION_RIGHT_FORWARD (1U << DIRECTION_PIN_RIGHT_IN2) -#define DIRECTION_RIGHT_BACKWARD (1U << DIRECTION_PIN_RIGHT_IN1) -#define DIRECTION_LEFT_FORWARD (1U << DIRECTION_PIN_LEFT_IN4) -#define DIRECTION_LEFT_BACKWARD (1U << DIRECTION_PIN_LEFT_IN3) - -#define SPEED_PIN_RIGHT 15U -#define SPEED_PIN_LEFT 16U - -#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 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; - -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(); - - 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 -* wheels such as DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_BACKWARD, it will -* set the left wheel to go forward and the right wheel to go backward within -* the same function. -* if the wheel direction is not set, it will not move. -* @param direction The direction of the left and right wheels -* @param left_speed The speed of the left wheel, from 0.0 to 1.0 -* @param right_speed The speed of the right wheel, from 0.0 to 1.0 - */ -void -set_wheel_direction (uint32_t direction) -{ - static const uint32_t mask = DIRECTION_LEFT_FORWARD | - DIRECTION_LEFT_BACKWARD | - DIRECTION_RIGHT_FORWARD | - DIRECTION_RIGHT_BACKWARD; - - gpio_put_masked(mask, 0U); - gpio_set_mask(direction); -} - -/*! - * @brief Set the speed of the wheels; can use bitwise OR to set both - * @param speed in range [0, 5000] - * @param side 0 for left, 1 for right - */ -void -set_wheel_speed (float speed, uint8_t side) -{ - if (side == 0U) - { - pwm_set_chan_level(g_slice_num_left, - PWM_CHAN_A, - (uint16_t) speed); - } - else - { - pwm_set_chan_level(g_slice_num_right, - PWM_CHAN_B, - (uint16_t) speed); - } - -} - -void -h_left_wheel_sensor_isr_handler (void) -{ - 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 -h_right_wheel_sensor_isr_handler (void) -{ - 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); - } -} - -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) - { - static uint64_t curr_time_left = 0u; - 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; - - 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); - - } - - } - -} - -void -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) - { - 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 = 1u; // to avoid division by 0 - - elapsed_time_right = curr_time_right - prev_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); - - 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); - - } - - } - -} \ No newline at end of file