Got Free RTOS 2 work

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",

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)

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
+)

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 */

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

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!"

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
+openocd -f interface/cmsis-dap.cfg -c "bindto 0.0.0.0" -c "adapter speed 100" -f target/rp2040.cfg -s tclErt645$

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 <stdio.h>
 
 #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,7 +48,110 @@
 #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_put(DIR0_PIN, 0);
@@ -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);
+    gpio_set_input_hysteresis_enabled(LIMIT1_PIN, true);
 
-    step_driver.set_conf0(10000, 1);
-    step_driver.pulse0(10000);
-    step_driver.wait0(0);
+    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, 0);
-    step_driver.pulse0(100000);
-    while(!gpio_get(LIMIT1_PIN)) {
-        sleep_ms(1);
-    }
-    step_driver.pulse0(0);
-    sleep_ms(500);
+    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(10000, 1);
-    step_driver.pulse0(200);
-    step_driver.wait0(0);
+/*
+    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
+    );*/
 
-    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_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;
 }