From 6ba0535af9c2689e49dcc1796ba09276c7b3bbd2 Mon Sep 17 00:00:00 2001 From: AlexanderHD27 Date: Thu, 17 Oct 2024 12:52:39 +0200 Subject: [PATCH] Got Free RTOS 2 work --- motor-control/firmware/.vscode/launch.json | 2 +- motor-control/firmware/CMakeLists.txt | 9 +- motor-control/firmware/cmake/freeRTOS.cmake | 14 ++ .../include/freeRTOS/FreeRTOSConfig.h | 101 ++++++++ motor-control/firmware/lib/FreeRTOS-Kernel | 2 +- motor-control/firmware/lib/picotool | 1 - .../firmware/scripts/remote/flash.exp | 2 - .../firmware/scripts/remote/run_openocd.exp | 2 +- .../firmware/scripts/remote/run_openocd.sh | 2 +- motor-control/firmware/src/main.cpp | 238 +++++++++++++++--- 10 files changed, 337 insertions(+), 36 deletions(-) create mode 100644 motor-control/firmware/cmake/freeRTOS.cmake create mode 100644 motor-control/firmware/include/freeRTOS/FreeRTOSConfig.h delete mode 160000 motor-control/firmware/lib/picotool diff --git a/motor-control/firmware/.vscode/launch.json b/motor-control/firmware/.vscode/launch.json index e904b95..779bb20 100755 --- a/motor-control/firmware/.vscode/launch.json +++ b/motor-control/firmware/.vscode/launch.json @@ -13,7 +13,7 @@ "type": "cortex-debug", "servertype": "external", "gdbPath" : "gdb-multiarch", - "gdbTarget": "localhost:3333", + "gdbTarget": "gobot-rpi:3333", "device": "RP2040", "configFiles": [ "interface/cmsis-dap.cfg", diff --git a/motor-control/firmware/CMakeLists.txt b/motor-control/firmware/CMakeLists.txt index 1249716..e955789 100755 --- a/motor-control/firmware/CMakeLists.txt +++ b/motor-control/firmware/CMakeLists.txt @@ -9,9 +9,10 @@ else() endif() #SET(PICO_TOOLCHAIN_PATH "${PICO_SDK_PATH}/cmake/preload/toolchains/") -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") +SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") include(${PICO_SDK_PATH}/pico_sdk_init.cmake) +include(cmake/freeRTOS.cmake) #include(pico_extras_import_optional.cmake) @@ -45,6 +46,7 @@ add_executable(main target_include_directories(main PUBLIC include/ + include/freeRTOS build/ ) @@ -56,12 +58,17 @@ pico_generate_pio_header(main ${CMAKE_CURRENT_LIST_DIR}/src/tmc2209/pulser.pio) # pull in common dependencies target_link_libraries(main pico_stdlib + pico_time pico_multicore + pico_stdio_usb hardware_pio tmc2209_driver + FreeRTOS ) +pico_enable_stdio_usb(main 1) +pico_enable_stdio_uart(main 0) # create map/bin/hex file etc. pico_add_extra_outputs(main) diff --git a/motor-control/firmware/cmake/freeRTOS.cmake b/motor-control/firmware/cmake/freeRTOS.cmake new file mode 100644 index 0000000..90f2548 --- /dev/null +++ b/motor-control/firmware/cmake/freeRTOS.cmake @@ -0,0 +1,14 @@ +SET(FreeRTOS_DIR ${CMAKE_CURRENT_LIST_DIR}/../lib/FreeRTOS-Kernel) +FILE(GLOB FreeRTOS_src ${FreeRTOS_DIR}/*.c) + +add_library(FreeRTOS STATIC + ${FreeRTOS_src} + ${FreeRTOS_DIR}/portable/GCC/ARM_CM0/port.c + ${FreeRTOS_DIR}/portable/MemMang/heap_4.c +) + +target_include_directories(FreeRTOS PUBLIC + ${FreeRTOS_DIR}/portable/GCC/ARM_CM0/ + ${FreeRTOS_DIR}/include/ + ${CMAKE_CURRENT_LIST_DIR}/../include/freeRTOS +) \ No newline at end of file diff --git a/motor-control/firmware/include/freeRTOS/FreeRTOSConfig.h b/motor-control/firmware/include/freeRTOS/FreeRTOSConfig.h new file mode 100644 index 0000000..9be5aa2 --- /dev/null +++ b/motor-control/firmware/include/freeRTOS/FreeRTOSConfig.h @@ -0,0 +1,101 @@ +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +#define configENABLE_MPU 1 + +#define configUSE_PREEMPTION 1 +#define configUSE_PORT_OPTIMISED_TASK_SELECTION 0 +#define configUSE_TICKLESS_IDLE 0 +#define configCPU_CLOCK_HZ 133000000 +#define configSYSTICK_CLOCK_HZ 1000000 +#define configTICK_RATE_HZ 1000 +#define configMAX_PRIORITIES 4 +#define configMINIMAL_STACK_SIZE 256 +#define configMAX_TASK_NAME_LEN 16 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 1 +#define configUSE_TASK_NOTIFICATIONS 1 +#define configTASK_NOTIFICATION_ARRAY_ENTRIES 3 +#define configUSE_MUTEXES 0 +#define configUSE_RECURSIVE_MUTEXES 0 +#define configUSE_COUNTING_SEMAPHORES 1 +#define configUSE_ALTERNATIVE_API 0 /* Deprecated! */ +#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_QUEUE_SETS 0 +#define configUSE_TIME_SLICING 1 +#define configUSE_NEWLIB_REENTRANT 0 +#define configENABLE_BACKWARD_COMPATIBILITY 0 +#define configNUM_THREAD_LOCAL_STORAGE_POINTERS 5 +#define configSTACK_DEPTH_TYPE uint16_t +#define configMESSAGE_BUFFER_LENGTH_TYPE size_t + +/* Memory allocation related definitions. */ +#define configSUPPORT_STATIC_ALLOCATION 0 +#define configSUPPORT_DYNAMIC_ALLOCATION 1 +#define configTOTAL_HEAP_SIZE 50000 +#define configAPPLICATION_ALLOCATED_HEAP 0 + +/* Hook function related definitions. */ +#define configUSE_IDLE_HOOK 0 +#define configUSE_TICK_HOOK 0 +#define configCHECK_FOR_STACK_OVERFLOW 0 +#define configUSE_MALLOC_FAILED_HOOK 0 +#define configUSE_DAEMON_TASK_STARTUP_HOOK 0 + +/* Run time and task stats gathering related definitions. */ +#define configGENERATE_RUN_TIME_STATS 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_STATS_FORMATTING_FUNCTIONS 0 + +/* Co-routine related definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES 1 + +/* Software timer related definitions. */ +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY 3 +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE + +/* Interrupt nesting behaviour configuration. */ +#define configKERNEL_INTERRUPT_PRIORITY [dependent of processor] +#define configMAX_SYSCALL_INTERRUPT_PRIORITY [dependent on processor and application] +#define configMAX_API_CALL_INTERRUPT_PRIORITY [dependent on processor and application] + +/* Define to trap errors during development. */ +//#define configASSERT( ( x ) ) assert() + +/* FreeRTOS MPU specific definitions. */ +#define configINCLUDE_APPLICATION_DEFINED_PRIVILEGED_FUNCTIONS 0 +#define configTOTAL_MPU_REGIONS 8 /* Default value. */ +#define configTEX_S_C_B_FLASH 0x07UL /* Default value. */ +#define configTEX_S_C_B_SRAM 0x07UL /* Default value. */ +#define configENFORCE_SYSTEM_CALLS_FROM_KERNEL_ONLY 1 + +/* Optional functions - most linkers will remove unused functions anyway. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_xResumeFromISR 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 0 +#define INCLUDE_xTaskGetIdleTaskHandle 0 +#define INCLUDE_eTaskGetState 0 +#define INCLUDE_xEventGroupSetBitFromISR 1 +#define INCLUDE_xTimerPendFunctionCall 0 +#define INCLUDE_xTaskAbortDelay 0 +#define INCLUDE_xTaskGetHandle 0 +#define INCLUDE_xTaskResumeFromISR 1 + +#define vPortSVCHandler isr_svcall +#define xPortPendSVHandler isr_pendsv +#define xPortSysTickHandler isr_systick + + +/* A header file that defines trace macro can be included here. */ + +#endif /* FREERTOS_CONFIG_H */ \ No newline at end of file diff --git a/motor-control/firmware/lib/FreeRTOS-Kernel b/motor-control/firmware/lib/FreeRTOS-Kernel index 310ace5..0240cd5 160000 --- a/motor-control/firmware/lib/FreeRTOS-Kernel +++ b/motor-control/firmware/lib/FreeRTOS-Kernel @@ -1 +1 @@ -Subproject commit 310ace5dd0a7391124448d56a309e7e1b6926f10 +Subproject commit 0240cd55f20be7100439b606086709b74a105cb1 diff --git a/motor-control/firmware/lib/picotool b/motor-control/firmware/lib/picotool deleted file mode 160000 index bf33c6d..0000000 --- a/motor-control/firmware/lib/picotool +++ /dev/null @@ -1 +0,0 @@ -Subproject commit bf33c6ddd756c3961c7a13b000859b71e825ee74 diff --git a/motor-control/firmware/scripts/remote/flash.exp b/motor-control/firmware/scripts/remote/flash.exp index 13c4ea3..fe764c9 100755 --- a/motor-control/firmware/scripts/remote/flash.exp +++ b/motor-control/firmware/scripts/remote/flash.exp @@ -17,6 +17,4 @@ send "rp2040.core1 arp_reset assert 0\n" expect "> " send "rp2040.core0 arp_reset assert 0\n" expect "> " - -send "exit\n" exit \ No newline at end of file diff --git a/motor-control/firmware/scripts/remote/run_openocd.exp b/motor-control/firmware/scripts/remote/run_openocd.exp index a700b55..7b67ded 100755 --- a/motor-control/firmware/scripts/remote/run_openocd.exp +++ b/motor-control/firmware/scripts/remote/run_openocd.exp @@ -18,4 +18,4 @@ send "cd motor-control/firmware/scripts/remote\n" expect "$ " send "./run_openocd.sh\n" -expect eof +expect "Kill Me Please, this is should not be triggered!" diff --git a/motor-control/firmware/scripts/remote/run_openocd.sh b/motor-control/firmware/scripts/remote/run_openocd.sh index e8f523d..21c7ba7 100755 --- a/motor-control/firmware/scripts/remote/run_openocd.sh +++ b/motor-control/firmware/scripts/remote/run_openocd.sh @@ -1,3 +1,3 @@ #!/usr/bin/bash -openocd -f interface/cmsis-dap.cfg -c "bindto 0.0.0.0" -c "adapter speed 5000" -f target/rp2040.cfg -s tcl \ No newline at end of file +openocd -f interface/cmsis-dap.cfg -c "bindto 0.0.0.0" -c "adapter speed 100" -f target/rp2040.cfg -s tclErt645$ diff --git a/motor-control/firmware/src/main.cpp b/motor-control/firmware/src/main.cpp index 09be6ee..4beb970 100755 --- a/motor-control/firmware/src/main.cpp +++ b/motor-control/firmware/src/main.cpp @@ -9,16 +9,23 @@ * */ -#include "tmc2209/tmc2209.hpp" #include "pico/stdio.h" +#include "pico/stdlib.h" +#include "pico/time.h" #include "hardware/uart.h" +#include #include "pulser.pio.h" +#include "tmc2209/tmc2209.hpp" #include "tmc2209/step.hpp" +#include "FreeRTOSConfig.h" +#include "FreeRTOS.h" +#include "task.h" + #define LED_PIN 25 -#define RGB1_R_PIN 21 +#define RGB1_B_PIN 21 #define RGB1_G_PIN 22 #define RGB2_R_PIN 26 @@ -41,9 +48,112 @@ #define DIR0_PIN 8 #define DIR1_PIN 9 +enum SYSTEM_STATE { + STARTUP, + HOMEING_FIRST_RUN, + HOMEING_BACKOFF, + HOMEING_SECOND_RUN, + IDLE +}; + +SYSTEM_STATE system_state = STARTUP; + + +absolute_time_t debounce_cooldown_ticks= 5000; +absolute_time_t limit_debounce_tick = 0; + + +void isr_LimitSwitch(uint gpio, uint32_t event_mask) { + absolute_time_t current_tick = get_absolute_time(); + if((current_tick - limit_debounce_tick) < debounce_cooldown_ticks) { + return; + } else { + limit_debounce_tick = current_tick; + } + + switch (gpio) { + case LIMIT0_PIN: + break; + + default: + break; + } + +} + +void homeMotors0(TMC2209_step_dual * driver) { + + driver->set_conf0(10000, 1); + driver->pulse0(10000); + driver->wait0(0); + + driver->set_conf0(10000, 0); + driver->pulse0(100000); + while(!gpio_get(LIMIT2_PIN)) { + sleep_ms(1); + } + driver->pulse0(0); + sleep_ms(500); + + // Second Run + driver->set_conf0(5000, 1); + driver->pulse0(200); + driver->wait0(0); + + driver->set_conf0(500, 0); + driver->pulse0(5000); + while(!gpio_get(LIMIT2_PIN)) { + sleep_ms(1); + } + driver->pulse0(0); + sleep_ms(1); +} + +void homeMotors1(TMC2209_step_dual * driver) { + + driver->set_conf1(10000, 1); + driver->pulse1(5000); + driver->wait1(0); + + driver->set_conf1(10000, 0); + driver->pulse1(100000); + while(!gpio_get(LIMIT3_PIN)) { + sleep_ms(1); + } + driver->pulse1(0); + sleep_ms(500); + + // Second Run + driver->set_conf1(5000, 1); + driver->pulse1(200); + driver->wait1(0); + + driver->set_conf1(500, 0); + driver->pulse1(5000); + while(!gpio_get(LIMIT3_PIN)) { + sleep_ms(1); + } + driver->pulse1(0); + sleep_ms(1); +} + +void task_home0(void *pvParameters) { + TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters; + vTaskDelay(500); + homeMotors0(driver); +} + +void task_home1(void *pvParameters) { + TMC2209_step_dual * driver = (TMC2209_step_dual *)pvParameters; + homeMotors1(driver); +} + int main() { + sleep_ms(100); + stdio_usb_init(); + gpio_init(DIR0_PIN); - gpio_set_dir(DIR0_PIN, GPIO_OUT); + gpio_set_dir(DIR0_PIN, GPIO_OUT); gpio_put(DIR0_PIN, 0); gpio_init(DIR1_PIN); @@ -73,53 +183,125 @@ int main() { sleep_ms(5); - step_driver.set_conf1(10000, 0); + //step_driver.set_conf1(10000, 0); + //step_driver.set_conf0(10000, 0); + //step_driver.pulse1(1000); + //step_driver.pulse0(1000); + //step_driver.wait1(0); + //step_driver.wait0(0); gpio_init(LIMIT0_PIN); gpio_set_dir(LIMIT0_PIN, GPIO_IN); gpio_pull_up(LIMIT0_PIN); + gpio_set_input_hysteresis_enabled(LIMIT0_PIN, true); gpio_init(LIMIT1_PIN); gpio_set_dir(LIMIT1_PIN, GPIO_IN); gpio_pull_up(LIMIT1_PIN); - - step_driver.set_conf0(10000, 1); - step_driver.pulse0(10000); - step_driver.wait0(0); + gpio_set_input_hysteresis_enabled(LIMIT1_PIN, true); - step_driver.set_conf0(10000, 0); - step_driver.pulse0(100000); - while(!gpio_get(LIMIT1_PIN)) { - sleep_ms(1); - } - step_driver.pulse0(0); - sleep_ms(500); + gpio_init(LIMIT2_PIN); + gpio_set_dir(LIMIT2_PIN, GPIO_IN); + gpio_pull_up(LIMIT2_PIN); + gpio_set_input_hysteresis_enabled(LIMIT2_PIN, true); - step_driver.set_conf0(10000, 1); - step_driver.pulse0(200); - step_driver.wait0(0); + gpio_init(LIMIT3_PIN); + gpio_set_dir(LIMIT3_PIN, GPIO_IN); + gpio_pull_up(LIMIT3_PIN); + gpio_set_input_hysteresis_enabled(LIMIT3_PIN, true); - step_driver.set_conf0(500, 0); - step_driver.pulse0(5000); - while(!gpio_get(LIMIT1_PIN)) { - sleep_ms(1); - } - step_driver.pulse0(0); - sleep_ms(1); +/* + gpio_set_irq_enabled_with_callback( + LIMIT0_PIN, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + gpio_set_irq_enabled_with_callback( + LIMIT1_PIN, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + gpio_set_irq_enabled_with_callback( + LIMIT2_PIN, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + ); + gpio_set_irq_enabled_with_callback( + LIMIT3_PIN, + GPIO_IRQ_EDGE_RISE, + true, + isr_LimitSwitch + );*/ + gpio_init(RGB2_B_PIN); + gpio_init(RGB2_G_PIN); + gpio_init(RGB2_R_PIN); + gpio_set_dir(RGB2_B_PIN, GPIO_OUT); + gpio_set_dir(RGB2_G_PIN, GPIO_OUT); + gpio_set_dir(RGB2_R_PIN, GPIO_OUT); + + gpio_init(RGB1_B_PIN); + gpio_init(RGB1_G_PIN); + gpio_set_dir(RGB1_B_PIN, GPIO_OUT); + gpio_set_dir(RGB1_G_PIN, GPIO_OUT); + + gpio_put(RGB1_B_PIN, 1); + gpio_put(RGB1_G_PIN, 1); + gpio_put(RGB2_B_PIN, 1); + gpio_put(RGB2_G_PIN, 1); + gpio_put(RGB2_R_PIN, 1); + + /* gpio_init(LED_PIN); gpio_set_dir(LED_PIN, GPIO_OUT); - step_driver.set_conf0(10000, 0); - step_driver.pulse0(1000000); + //step_driver.set_conf0(10000, 0); + //step_driver.pulse0(1000000); + */ + homeMotors0(&step_driver); + printf("Home0 done\n"); + homeMotors1(&step_driver); + printf("Home1 done\n"); + //xTaskCreate(task_home0, "Home0", 1024, &step_driver, 5, NULL); + //xTaskCreate(task_home1, "Home1", 1024, &step_driver, 5, NULL); + vTaskStartScheduler(); + + /* while (true) { sleep_ms(50); gpio_put(LED_PIN, 0); sleep_ms(50); gpio_put(LED_PIN, 1); + + if(gpio_get(LIMIT1_PIN)) { + gpio_put(RGB1_B_PIN, 0); + } else { + gpio_put(RGB1_B_PIN, 1); + } + + if(gpio_get(LIMIT3_PIN)) { + gpio_put(RGB1_G_PIN, 0); + } else { + gpio_put(RGB1_G_PIN, 1); + } + + if(gpio_get(LIMIT0_PIN)) { + gpio_put(RGB2_B_PIN, 0); + } else { + gpio_put(RGB2_B_PIN, 1); + } + + if(gpio_get(LIMIT2_PIN)) { + gpio_put(RGB2_G_PIN, 0); + } else { + gpio_put(RGB2_G_PIN, 1); + } } - + */ return 0; } \ No newline at end of file