added utils from different project

cspell.json

@@ -0,0 +1,17 @@
+{
+    "version": "0.2",
+    "ignorePaths": [],
+    "dictionaryDefinitions": [],
+    "dictionaries": [],
+    "words": [
+        "Gustice",
+        "LOGD",
+        "LOGI",
+        "LOGW",
+        "LOGE",
+        "GPIO",
+
+    ],
+    "ignoreWords": [],
+    "import": []
+}

test/CMakeLists.txt

@@ -0,0 +1,16 @@
+# This is the project CMakeLists.txt file for the test subproject
+cmake_minimum_required(VERSION 3.16)
+
+# Include the components directory of the main application:
+#
+set(EXTRA_COMPONENT_DIRS "../components ../managed_components")
+
+# Set the components to include the tests for.
+# This can be overriden from CMake cache:
+# - when invoking CMake directly: cmake -D TEST_COMPONENTS="xxxxx" ..
+# - when using idf.py: idf.py -T xxxxx build
+#
+set(TEST_COMPONENTS "testable freeRtosApi hwHal" CACHE STRING "List of components to test")
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(unit_test_test)

test/ReadMe.md

@@ -0,0 +1,35 @@
+# Unit Testing
+
+Unit Test based on unity-library [Unity](https://github.com/ThrowTheSwitch/Unity). 
+
+* _Assertions_ (`TEST_ASSERT` and similar) are used when writing the test cases. See [the reference](https://github.com/ThrowTheSwitch/Unity/blob/master/docs/UnityAssertionsReference.md) for more information about assertions.
+
+* `UNITY_BEGIN()` and `UNITY_END()` macros allow Unity to count the number of tests which have passed or failed, and display the summary.
+
+In addition to features of Unity, _test registration_ feature of ESP-IDF is used. This feature works when unit test functions are declared using `TEST_CASE` macro. Such functions from all the object files in the program are automatically collected into a list, and run-time facilities are provided to run functions from this list. 
+
+Note testable components have a *test* subdirectory which is included in the *test project*
+When the main application project is compiled, tests are not included. 
+Test project includes the tests by setting `TEST_COMPONENTS` variable in the project makefile.
+
+## Build and flash production code
+
+* Start an ESP-IDF-Terminal
+* Run `idf.py -p PORT flash monitor` in the current directory (`unit_test`).
+* Observe the output: a list of random numbers and their mean value.
+To exit the serial monitor, type ``Ctrl-]``.
+
+## Build and flash test code
+
+* Start an ESP-IDF-Terminal
+* Enter `test` directory (`unit_test/test`), and run `idf.py -p PORT flash monitor`.
+To exit the serial monitor, type ``Ctrl-]``.
+* Observe the output: results of test case execution.
+
+This is the test menu, which is launched by `unity_run_menu` function in the test project. It allows running the tests in a few ways:
+* Run specific test by its index: type the number and press ENTER.
+* Run a group of tests with a certain tag: type the tag, including square brackets, and press ENTER.
+* nRu all tests except the ones with a certain tag: press `!`, then type the tag, including the square brackets, then press ENTER.
+* Run test with a specific name: type the name of the test, including quotes, and press ENTER.
+* Run all the tests: press `*` and then ENTER.
+

test/dependencies.lock

@@ -0,0 +1,9 @@
+dependencies:
+  idf:
+    component_hash: null
+    source:
+      type: idf
+    version: 5.3.0
+manifest_hash: c4f4ec05313d427eb56c633f78ce213e553379725bf2af3c511665f930571beb
+target: esp32c6
+version: 1.0.0

test/main/CMakeLists.txt

@@ -0,0 +1,4 @@
+idf_component_register(
+    SRCS "unitTest.cpp"
+    INCLUDE_DIRS "."
+)

test/main/unitTest.cpp

@@ -0,0 +1,37 @@
+/**
+ * @file unitTest.cpp
+ * @author Gustice
+ * @brief Unit/Integration-Test frame for hardware bound components
+ *
+ * @copyright Copyright (c) 2024
+ */
+
+#include "unity.h"
+#include <stdio.h>
+#include <string.h>
+
+extern "C" { // This switch allows the ROS C-implementation to find this main
+void app_main(void);
+}
+
+void app_main(void) {
+    printf("\n#### Running all the registered tests #####\n\n");
+    UNITY_BEGIN();
+    unity_run_all_tests();
+    // Alternatively:
+    // "Executing one test by its name"
+    //      unity_run_test_by_name("Mean of an empty array is zero");
+    // Running tests with [mean] tag"
+    //      unity_run_tests_by_tag("[mean]", false);
+    // print_banner("Running tests without [fails] tag");
+    //      unity_run_tests_by_tag("[fails]", true);
+    UNITY_END();
+
+    printf("\n#### Starting interactive test menu: ####\n");
+    printf("# - run explicit #\n");
+    printf("- run all with *, \n");
+    printf("# - run tagged with [tag], \n");
+    printf("# - run all other than ![tag])\n");
+    printf("# Exit with 'Ctrl-]'\n");
+    unity_run_menu();
+}

test/sdkconfig.defaults

@@ -0,0 +1,3 @@
+CONFIG_COMPILER_CXX_EXCEPTIONS=y
+
+CONFIG_ESP_TASK_WDT_EN=n

tools/.gitignore

@@ -0,0 +1 @@
+__pycache__/

tools/trajectoryGen/trajectoryGen.py

@@ -0,0 +1,54 @@
+import math
+import matplotlib.pyplot as plt
+
+from utils.defines import *
+from utils.generator import *
+from matplotlib.ticker import (MultipleLocator, AutoMinorLocator)
+
+def main():
+    st = Stepper(1.8, 32)
+    print(f"Step = {st.Step}; Steps = {st.Steps}")
+
+    wv = QuadWave(64)
+    # plt.plot(wv.Sine, label="sin")
+    # plt.plot(wv.Cosine, label="cos")
+    # plt.legend()
+    # plt.show()
+
+    gen = Generator(st, 0.001, 1, 1)
+
+    curves = []
+    curves.append(gen.createSmoothStep(math.pi)) # 180 deg 
+    curves.append(gen.createSmoothStep(math.pi*1/6)) # 60 deg 
+    curves.append(gen.createBuildUpStep(math.pi)) # 180 deg 
+    curves.append(gen.createRapidStartSwingInEnd(math.pi)) # 180 deg 
+    curves.append(gen.createJumpInTarget(math.pi)) # 180 deg 
+
+    fig, axes = plt.subplots(len(curves), 1, layout='constrained')
+
+    for (i,ax) in enumerate(axes):
+        crv = curves[i]
+        ax.plot(crv["p"], label="ϕ")
+        ax.plot(crv["w"], label="ω", linestyle="--")
+
+        dax = ax.twinx()  # instantiate a second Axes that shares the same x-axis
+        dax.step(range(len(crv["o"])), crv["o"], label="o", linewidth=.5)
+        print(f"EndPos = {crv["pE"]}; (Delta = {math.pi - crv["pE"]}) in {crv["T"] * 1000} steps")
+
+    tMax = max(map(lambda x : x["T"], curves))
+    tEnd = math.ceil(tMax)*1000
+
+    for ax in axes:
+        ax.yaxis.set_ticks([0, math.pi/3, math.pi*2/3, math.pi],["Picto0", "Picto1", "Picto2", "Picto3"])
+        ax.xaxis.set_ticks(range(0,tEnd+1000,1000), range(0,math.ceil(tMax)+1,1))
+        ax.xaxis.set_ticks(range(0,tEnd+1000,100), minor = True)
+        ax.yaxis.grid(linestyle='dotted')
+        ax.set_xlabel('time (s)')
+        ax. set_ylabel('angle')
+        # ax.legend()
+
+    plt.show()
+
+    
+if __name__ == '__main__':
+    main()

tools/trajectoryGen/utils/defines.py

@@ -0,0 +1,33 @@
+if __name__ == "__main__": # If the Helper.py file is run directly
+    print("This is a modul and should be imported")
+
+import math
+
+class Stepper:
+    # step as degrees
+    Step : float
+    # step as radiant
+    StepRad : float
+    # steps for full resolution
+    Steps : int
+    # microsteps per step
+    Resolution : int
+
+    def __init__(self, step, res):
+        self.Step = step
+        self.Steps = 360 / step
+        self.StepRad = math.pi * step/180
+        self.Resolution = res
+
+class QuadWave:
+    Sine : []
+    Cosine : []
+    def __init__(self, resolution):
+        sine = []
+        cosine = []
+        s = 2 * math.pi / resolution
+        for i in range(resolution):
+            sine.append(math.sin(s * i))   
+            cosine.append(math.cos(s * i))   
+        self.Sine = sine
+        self.Cosine = cosine    

tools/trajectoryGen/utils/generator.py

@@ -0,0 +1,180 @@
+if __name__ == "__main__": # If the Helper.py file is run directly
+    print("This is a modul and should be imported")
+
+import math
+
+class Generator:
+    dT = 0 
+    aMax = 0
+    wMax = 0 # v = a * t
+    # s = a * t^2 / 2
+    # s = v * t
+
+    def __init__(self, stpr, dt, a, v):
+        self.stepper = stpr
+        self.dT = dt
+        self.aMax = a
+        self.wMax = v
+
+    def __initVars(self):
+        self.__pArr = [0]
+        self.__wArr = [0]
+        self.__tArr = [0]
+        self.__t = 0
+
+    def __appendPoint(self, p, w, t):
+        self.__wArr.append(w)
+        self.__pArr.append(p)
+        self.__tArr.append(t)
+
+    def __accelUntil(self, a, wInit, check):
+        t = 0
+        pX = self.__pArr[-1]
+        while True:
+            w = wInit + a * t
+            p = pX + wInit * t + a * t**2 / 2
+            if check(w, p):
+                break
+            self.__appendPoint(p, w, self.__t + t)
+            t = t + self.dT
+        self.__t = self.__t + t
+
+    def __getStopDistance(self, a, w):
+        tDec = w / a
+        pDec = a * tDec**2 / 2
+        return (pDec, tDec)
+
+    def __moveUntil(self, wM, pEnd): 
+        t = 0
+        pX = self.__pArr[-1]
+        while True:
+            p = pX + wM * t
+            if p > pEnd:
+                break
+            self.__appendPoint(p, wM, self.__t + t)
+            t = t + self.dT
+        self.__t = self.__t + t
+
+    def __buildUpSwing(self, a, vM, swings):
+        t = 0
+        pX = self.__pArr[-1]
+
+        TwMax = vM / a
+        tau = 2*math.pi
+        sTau = swings*tau
+
+        while True:
+            t = t + self.dT
+            env = a * t
+            p = math.sin(sTau*t/TwMax) * env * TwMax/sTau
+            w = (p-self.__pArr[-1])/self.dT
+            if w > self.wMax:
+                break 
+            self.__appendPoint(p, w, self.__t + t)
+
+        self.__t = self.__t + t
+
+    def __transformToStepper(self) :
+        stp = self.stepper.StepRad
+        res = stp / self.stepper.Resolution
+        oArr = map(lambda x : int((x % stp) / res), self.__pArr)
+        oArr = list(oArr)
+        return {"p": self.__pArr, "w": self.__wArr, "t": self.__tArr, "pE": self.__pArr[-1], "T": self.__t, "o": oArr}
+
+    def __smoothSubStep(self, delta, aM, wM):
+        (pDec,_) = self.__getStopDistance(aM, wM)
+
+        if (pDec < (delta - self.__pArr[-1])/2):
+            # t0..t1: acceleration to max velocity
+            self.__accelUntil(aM, 0, lambda w,p : w > wM)
+
+            # t1..t2: movement to until almost destination
+            self.__moveUntil(wM, delta - pDec)
+
+            # t2..r3: deceleration until destination
+            self.__accelUntil(-aM, wM, lambda w,p : w < 0)
+        else:
+            # t0..t1: acceleration until center is reached
+            pCen = ((delta - self.__pArr[-1]) /2 + self.__pArr[-1]) 
+            self.__accelUntil(aM, 0, lambda w,p : p > pCen)
+
+            # t2..r3: deceleration until destination
+            self.__accelUntil(-aM, self.__wArr[-1], lambda w,p : w < 0)
+
+    def createSmoothStep(self, delta):
+        self.__initVars()
+
+        (pDec,_) = self.__getStopDistance(self.aMax, self.wMax)
+
+        if (pDec < delta/2):
+            # t0..t1: acceleration to max velocity
+            self.__accelUntil(self.aMax, 0, lambda w,p : w > self.wMax)
+
+            # t1..t2: movement to until almost destination
+            self.__moveUntil(self.wMax, delta - pDec)
+
+            # t2..r3: deceleration until destination
+            self.__accelUntil(-self.aMax, self.wMax, lambda w,p : w < 0)
+        else:
+            # t0..t1: acceleration until center is reached
+            pCen = (self.__pArr[-1]) + delta /2
+            self.__accelUntil(self.aMax, 0, lambda w,p : p > pCen)
+
+            # t2..r3: deceleration until destination
+            self.__accelUntil(-self.aMax, self.__wArr[-1] , lambda w,p : w < 0)
+
+        return self.__transformToStepper()    
+    
+    def createBuildUpStep(self, delta): 
+        self.__initVars()
+
+        # t0..t1: increasing swing
+        self.__buildUpSwing(self.aMax/4, self.wMax, 3)
+        
+        (pDec,_) = self.__getStopDistance(self.aMax, self.wMax)
+
+        # t1..t2: movement to until almost destination
+        self.__moveUntil(self.wMax, delta - pDec)
+        
+        # t2..r3: deceleration until destination
+        self.__accelUntil(-self.aMax, self.wMax, lambda w,p : w < 0)
+        
+        return self.__transformToStepper()
+
+    def createRapidStartSwingInEnd(self, delta):
+        self.__initVars()
+
+        (pDec,_) = self.__getStopDistance(self.aMax, self.wMax)
+
+        # t0..t1: acceleration to max velocity
+        self.__accelUntil(self.aMax*2, 0, lambda w,p : w > self.wMax*2)
+        
+        # t1..r2: deceleration until moving speed
+        self.__accelUntil(-self.aMax, self.wMax*2, lambda w,p : w < self.wMax )
+
+        # t2..t3: movement to until destination (override)
+        self.__moveUntil(self.wMax, delta)
+
+        # t3..t4: deceleration
+        self.__accelUntil(-self.aMax, self.wMax, lambda w,p : w < 0)
+
+        pCen = delta + (self.__pArr[-1] - delta) /2
+        self.__accelUntil(-self.aMax, 0, lambda w,p : p < pCen )
+        self.__accelUntil(self.aMax, self.__wArr[-1], lambda w,p : w > 0)
+
+        return self.__transformToStepper()
+
+    def createJumpInTarget(self, delta):
+        self.__initVars()
+
+        jumpInRange = math.pi/3
+        deltaPre = delta - jumpInRange
+        
+        self.__smoothSubStep(deltaPre, self.aMax, self.wMax)
+        
+        jumpInSteps = [jumpInRange/2, jumpInRange/3, jumpInRange/6]
+        nextPoint = deltaPre
+        for step in jumpInSteps:
+            nextPoint = nextPoint + step
+            self.__smoothSubStep(nextPoint, self.aMax*10, self.wMax*2)
+        return self.__transformToStepper()

utest/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.16)
+
+
+set(DEF_UTESTDIR "uTest")
+set(DEF_SRCDIR "src")
+set(DEF_INCLUDEDIR "include")
+
+# use just host compilers for this one
+set(CMAKE_C_COMPILER "gcc")
+set(CMAKE_CXX_COMPILER "g++")
+set(CMAKE_GENERATOR "Ninja")
+set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE)
+set(CMAKE_VERBOSE_MAKEFILE TRUE)
+
+include(test_core.cmake)
+enable_testing()
+
+project(UnitTests)
+
+find_package(doctest REQUIRED PATHS "/mingw64/lib/cmake")
+# target_link_libraries(${PROJECT_NAME} PRIVATE doctest::doctest)
+
+addSubDirs_ofRoot("${CMAKE_CURRENT_LIST_DIR}/../components")
+
+
+

utest/ReadMe.md

@@ -0,0 +1,28 @@
+# Unit Testing - Software
+
+In contrast to *test*-directory this module demonstrates how unit tests can also be run on host.
+These unit tests aim to test freestanding software modules with more convenient methods and tools
+
+## Setup 
+
+make sure you have following packages installed on your host syste
+- gnu compiler
+- cmake
+- generator
+- doctest-dev
+
+## Build and test code
+
+* Start some terminal in *uTest*-directory that has access to all needed tools (i.e. on  windows a mingw environment or WSL)
+
+Run following commands
+```sh
+# run cmake configuration
+cmake -S . -B build
+# run cmake build
+cmake --build build
+# run tests
+./build/<testprog>
+# run all tests with 
+cd build && ctest
+```

utest/test_core.cmake

@@ -0,0 +1,43 @@
+function(addSubDirs_ofRoot anchorPath)
+    message("scanning: ${anchorPath}")
+    file(GLOB targetModules RELATIVE "${anchorPath}" "${anchorPath}/*")
+    foreach(module ${targetModules})
+        message("checking: ${module}")
+        get_filename_component(moduleName ${module} NAME)
+        set(modulePath "${anchorPath}/${moduleName}")
+        if (NOT IS_DIRECTORY "${modulePath}")
+            # skip files
+            continue()
+        endif()
+        if (${moduleName} MATCHES "^_.*")
+            # skip ignored component
+            continue()
+        endif()
+        
+        if(EXISTS "${modulePath}/${DEF_UTESTDIR}")
+            message("Found unit-test: ${moduleName}")
+            add_subdirectory("${modulePath}/${DEF_UTESTDIR}" ${moduleName})
+        endif()
+    endforeach()
+endfunction()
+
+macro(unitTestsFrame)
+    if (CMAKE_TESTING_ENABLED)
+        # get_filename_component(moduleName "${CMAKE_CURRENT_LIST_DIR}/.." NAME)
+        file(GLOB_RECURSE targetSources "../${DEF_SRCDIR}/*.cpp" "../${DEF_SRCDIR}/*.c")
+        add_library(${moduleName} ${targetSources})
+        target_include_directories(${moduleName} PUBLIC "../${DEF_INCLUDEDIR}")
+        
+        set(moduleTestName "${moduleName}Test")
+        file(GLOB_RECURSE testSource "*.cpp")
+        add_executable(${moduleTestName} ${testSource})
+        target_link_libraries(${moduleTestName} PRIVATE ${moduleName} doctest::doctest)
+        target_include_directories(${moduleTestName} PRIVATE "../${DEF_SRCDIR}")
+
+        add_test (TEST-${moduleName} ${moduleTestName})
+
+        # gtest_discover_tests(${moduleTestName}
+        #     PROPERTIES TIMEOUT 30 LABELS ${moduleName} LABELS "all"
+        # )
+    endif()
+endmacro()