diff --git a/.gitignore b/.gitignore index 1336ea3..3e1bbdd 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,7 @@ SAM.pro.user *.user Makefile + +build/ + +builddir/ diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..dc44a94 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,17 @@ +{ + "configurations": [ + { + "name": "RPI3", + "includePath": [ + "${workspaceFolder}/**" + ], + "defines": [], + "compilerPath": "/opt/sam-sdk/arm-buildroot-linux-gnueabihf_sdk-buildroot/bin/arm-none-linux-gnueabihf-g++", + "cStandard": "c17", + "cppStandard": "c++20", + "intelliSenseMode": "linux-gcc-arm", + "compileCommands": "${workspaceFolder}/builddir/compile_commands.json" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..7044bd3 --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + "recommendations": [ + "asabil.meson", + "ms-vscode.cpptools", + "webfreak.debug" + ] +} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..7b4fc77 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,24 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "remote gdb", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/builddir/sam", + "cwd": "${workspaceFolder}", + "MIMode": "gdb", + "miDebuggerPath": "/usr/bin/gdb-multiarch", + "miDebuggerArgs": "", + "miDebuggerServerAddress": "10.0.0.1:4444", + "logging": { + "engineLogging": true + }, + "externalConsole": true, + "preLaunchTask": "Launch remote gdbserver" + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..508d5ed --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,65 @@ +{ + "files.associations": { + "exception": "cpp", + "stdexcept": "cpp", + "*.tcc": "cpp", + "unordered_map": "cpp", + "numeric": "cpp", + "ostream": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "cctype": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "vector": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ranges": "cpp", + "sstream": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp", + "complex": "cpp", + "cstring": "cpp", + "map": "cpp", + "fstream": "cpp" + }, + "mesonbuild.configureOnOpen": true +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..7bf4965 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,92 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "Clear build dir", + "type": "shell", + "command": "rm -r builddir", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + }, + "presentation": { + "echo": true, + "reveal": "always", + "focus": false, + "panel": "shared", + "showReuseMessage": true, + "clear": true + } + }, + { + "label": "Setup meson", + "type": "shell", + "command": "meson . builddir --cross-file meson/cross_file.ini", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + }, + "presentation": { + "echo": true, + "reveal": "always", + "focus": false, + "panel": "shared", + "showReuseMessage": true, + "clear": true + }, + "dependsOn": "Clear build dir" + }, + { + "label": "Build", + "type": "shell", + "command": "ninja", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/builddir" + }, + "presentation": { + "echo": true, + "reveal": "always", + "focus": false, + "panel": "shared", + "showReuseMessage": true, + "clear": true + } + }, + { + "label": "Deploy", + "type": "shell", + "command": "scp sam root@10.0.0.1:/opt/sam", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/builddir" + }, + "dependsOn": "Build" + }, + { + "label": "Kill remote gdbserver", + "type": "shell", + "command": "ssh root@10.0.0.1 \"killall -q gdbserver; exit 0;\"", + "group": "test" + }, + { + "label": "Launch remote gdbserver", + "type": "shell", + "command": "gnome-terminal -- ssh -t root@10.0.0.1 gdbserver localhost:4444 /opt/sam/sam", + "group": "test", + "dependsOn": "Kill remote gdbserver", + "presentation": { + "echo": true, + "reveal": "always", + "focus": true, + "panel": "dedicated", + "showReuseMessage": true, + "clear": true + } + } + ] +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index dea2504..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -project(sam CXX) - -cmake_minimum_required(VERSION 3.0) - -set(CMAKE_VERBOSE_MAKEFILE "ON") -set(CMAKE_CXX_STANDARD 20) - -add_definitions(-DMOSQUITTO_SERVER_IP="127.0.0.1" - -DMOSQUITTO_SERVER_PORT=1883 - -DDEFAULT_CPU_CORE=2 - -DDEFAULT_THREAD_PRIO=20) - -add_compile_options(-O2 -mcpu=cortex-a53 -mfloat-abi=hard) - -add_link_options(-mfloat-abi=hard) - -add_subdirectory(src) diff --git a/meson.build b/meson.build new file mode 100644 index 0000000..d96b794 --- /dev/null +++ b/meson.build @@ -0,0 +1,121 @@ +project( + 'sam', + ['c','cpp'], + version : '1.0.0', + default_options : ['cpp_std=c++17'] +) + +sam_public_headers = include_directories(['src']) + +sam_src = [ + 'src/main.cpp', + 'src/components/external/myoband/myoLinux/gattclient.cpp', + 'src/components/external/myoband/myoLinux/myoclient.cpp', + 'src/components/external/myoband/myoLinux/serial.cpp', + 'src/components/external/myoband/myoband.cpp', + 'src/components/external/ngimu/Osc99/OscAddress.c', + 'src/components/external/ngimu/Osc99/OscBundle.cpp', + 'src/components/external/ngimu/Osc99/OscCommon.c', + 'src/components/external/ngimu/Osc99/OscError.c', + 'src/components/external/ngimu/Osc99/OscMessage.cpp', + 'src/components/external/ngimu/Osc99/OscPacket.cpp', + 'src/components/external/ngimu/Osc99/OscSlip.cpp', + 'src/components/external/ngimu/ngimu.cpp', + 'src/components/external/optitrack/optitrack_listener.cpp', + 'src/components/external/ximu/ximu.cpp', + 'src/components/internal/actuators/epos/epos.cpp', + 'src/components/internal/actuators/roboclaw/answer.cpp', + 'src/components/internal/actuators/roboclaw/factory.cpp', + 'src/components/internal/actuators/roboclaw/message.cpp', + 'src/components/internal/actuators/roboclaw/roboclaw.cpp', + 'src/components/internal/actuators/actuator.cpp', + 'src/components/internal/actuators/custom_elbow.cpp', + 'src/components/internal/actuators/elbow_cybathlon.cpp', + 'src/components/internal/actuators/osmer_elbow.cpp', + 'src/components/internal/actuators/pronosupination.cpp', + 'src/components/internal/actuators/shoulder_rotator.cpp', + 'src/components/internal/actuators/wrist_flexor.cpp', + 'src/components/internal/actuators/wrist_rotator.cpp', + 'src/components/internal/actuators/wrist_cybathlon.cpp', + 'src/components/internal/adc/adafruit_ads1115.cpp', + 'src/components/internal/dac/mcp4728.cpp', + 'src/components/internal/gpio/gpio.cpp', + 'src/components/internal/hand/quantum_hand.cpp', + 'src/components/internal/hand/touch_bionics_hand.cpp', + 'src/control/algo/lawimu_we.cpp', + 'src/control/algo/lawimu_wrist.cpp', + 'src/control/algo/lawjacobian.cpp', + 'src/control/algo/lawopti.cpp', + 'src/control/algo/myocontrol.cpp', + 'src/control/compensation_imu.cpp', + 'src/control/compensation_optitrack.cpp', + 'src/control/controle_bretelles.cpp', + 'src/control/cybathlon.cpp', + 'src/control/demo.cpp', + 'src/control/demo_imu.cpp', + 'src/control/jf_opti.cpp', + 'src/control/jfimu_sk.cpp', + 'src/control/jfimu_v3.cpp', + 'src/control/jfimu_v4.cpp', + 'src/control/jfoptiorientation.cpp', + 'src/control/matlab_receiver.cpp', + 'src/control/myo_2electrodes.cpp', + 'src/control/pushbuttons.cpp', + 'src/control/read_adc.cpp', + 'src/control/recorddata.cpp', + 'src/control/remote_computer_control.cpp', + 'src/control/test_imu.cpp', + 'src/control/voluntary_control.cpp', + 'src/sam/sam.cpp', + 'src/sam/samanager.cpp', + 'src/sam/system_monitor.cpp', + 'src/ui/menu/menu_console.cpp', + 'src/ui/menu/menu_frontend.cpp', + 'src/ui/menu/menu_mqtt.cpp', + 'src/ui/sound/buzzer.cpp', + 'src/ui/visual/ledstrip.cpp', + 'src/utils/interfaces/menu_user.cpp', + 'src/utils/interfaces/mqtt_user.cpp', + 'src/utils/log/logger.cpp', + 'src/utils/log/safe_stream.cpp', + 'src/utils/monitoring/abstract_monitor.cpp', + 'src/utils/monitoring/cpu_freq_monitor.cpp', + 'src/utils/monitoring/cpu_load_monitor.cpp', + 'src/utils/monitoring/cpu_temp_monitor.cpp', + 'src/utils/monitoring/vc_based_monitor.cpp', + 'src/utils/named_object.cpp', + 'src/utils/param.cpp', + 'src/utils/serial_port.cpp', + 'src/utils/socket.cpp', + 'src/utils/threaded_loop.cpp', + 'src/utils/watchdog.cpp', + 'src/utils/worker.cpp', + 'src/ux/menu/menu_backend.cpp', + 'src/ux/menu/menu_broker.cpp', + 'src/ux/menu/menu_item.cpp', + 'src/ux/mosquittopp/client.cpp', + 'src/ux/mosquittopp/connect_factory.cpp', + 'src/ux/mosquittopp/connect_helper.cpp', + 'src/ux/mosquittopp/message.cpp', + 'src/ux/mosquittopp/subscription_factory.cpp', + 'src/ux/mosquittopp/subscription.cpp', +] + +i2c_dep = declare_dependency(link_args : ['-li2c']) +eposcmd_dep = declare_dependency(link_args : ['-lEposCmd']) +mosquitto_dep = declare_dependency(link_args : ['-lmosquitto']) +bcm2835_dep = declare_dependency(link_args : ['-lbcm2835']) +vc_dep = declare_dependency(link_args : ['-lvcos', '-lvchiq_arm', '-lvchostif']) +cppfs_dep = declare_dependency(link_args: ['-lstdc++fs']) +thread_dep = dependency('threads') + +add_project_arguments('-DMOSQUITTO_SERVER_IP="127.0.0.1"', language : 'cpp') +add_project_arguments('-DMOSQUITTO_SERVER_PORT=1883', language : 'cpp') +add_project_arguments('-DDEFAULT_CPU_CORE=2', language : 'cpp') +add_project_arguments('-DDEFAULT_THREAD_PRIO=20', language : 'cpp') + +sam_target = executable('sam', + sam_src, + include_directories : sam_public_headers, + dependencies : [bcm2835_dep, cppfs_dep, eposcmd_dep, i2c_dep, mosquitto_dep, thread_dep, vc_dep], +) diff --git a/meson/cross_file.ini b/meson/cross_file.ini new file mode 100644 index 0000000..504a6bc --- /dev/null +++ b/meson/cross_file.ini @@ -0,0 +1,21 @@ +[host_machine] +system = 'linux' +cpu_family = 'arm' +cpu = 'armv7' +endian = 'little' + +[constants] +arch = '/opt/sam-sdk/arm-buildroot-linux-gnueabihf_sdk-buildroot/bin/arm-none-linux-gnueabihf-' +sysroot = '/opt/sam-sdk/arm-buildroot-linux-gnueabihf_sdk-buildroot/arm-buildroot-linux-gnueabihf/sysroot/' +cpp_flags = ['--sysroot=' + sysroot, '-isystem' + sysroot + '/opt/vc/include', '-isystem' + sysroot + '/usr/local/include', '-isystem' + sysroot + '/usr/include/arm-linux-gnueabihf'] +link_flags = ['-L' + sysroot + '/usr/lib/arm-linux-gnueabihf', '-L' + sysroot + '/opt/vc/lib','-L' + sysroot + '/usr/local/lib'] + +[properties] +cpp_args = cpp_flags +cpp_link_args = link_flags + +[binaries] +c = arch + 'gcc' +cpp = arch + 'g++' +ar = arch + 'ar' +strip = arch + 'strip' diff --git a/meson/native.ini b/meson/native.ini new file mode 100644 index 0000000..593f352 --- /dev/null +++ b/meson/native.ini @@ -0,0 +1,8 @@ +[constants] +cpp_flags = ['-isystem/opt/vc/include', '-isystem/usr/local/include'] +link_flags = ['-L/opt/vc/lib','-L/usr/local/lib'] + +[properties] +cpp_args = cpp_flags +cpp_link_args = link_flags + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt deleted file mode 100644 index 84b62af..0000000 --- a/src/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -project(sam CXX) - -set(SOURCES - # cmake-format: sortable - main.cpp - sam/sam.cpp - sam/samanager.cpp - sam/system_monitor.cpp) - -include_directories(.) - -add_subdirectory(components) -add_subdirectory(control) -add_subdirectory(ui) -add_subdirectory(utils) -add_subdirectory(ux) - -add_executable(sam ${SOURCES}) - -target_link_libraries(sam - components - control - ui - utils - ux - wiringPi) - -install(TARGETS sam RUNTIME DESTINATION /opt/) diff --git a/src/components/CMakeLists.txt b/src/components/CMakeLists.txt deleted file mode 100644 index dcb1b9d..0000000 --- a/src/components/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -project(components CXX) - -add_subdirectory(external) -add_subdirectory(internal) - -add_library(components INTERFACE) - -target_link_libraries(components INTERFACE external internal) diff --git a/src/components/external/CMakeLists.txt b/src/components/external/CMakeLists.txt deleted file mode 100644 index c38024e..0000000 --- a/src/components/external/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -project(external CXX) - -add_subdirectory(myoband) -add_subdirectory(optitrack) -add_subdirectory(ximu) -add_subdirectory(ngimu) - -add_library(external INTERFACE) - -target_link_libraries(external INTERFACE myoband optitrack ximu ngimu) diff --git a/src/components/external/myoband/CMakeLists.txt b/src/components/external/myoband/CMakeLists.txt deleted file mode 100644 index 0cb4bc4..0000000 --- a/src/components/external/myoband/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -project(myoband CXX) - -set(SOURCES - # cmake-format: sortable - myoLinux/gattclient.cpp - myoLinux/myoclient.cpp - myoLinux/serial.cpp - myoband.cpp) - -add_library(myoband ${SOURCES}) - -target_link_libraries(myoband utils) diff --git a/src/components/external/ngimu/CMakeLists.txt b/src/components/external/ngimu/CMakeLists.txt deleted file mode 100644 index 79163f7..0000000 --- a/src/components/external/ngimu/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -project(ngimu CXX) - -set(SOURCES - # cmake-format: sortable - ngimu.cpp) - -add_library(ngimu ${SOURCES}) - -target_link_libraries(ngimu Osc99) diff --git a/src/ux/Osc99/LICENSE b/src/components/external/ngimu/Osc99/LICENSE similarity index 100% rename from src/ux/Osc99/LICENSE rename to src/components/external/ngimu/Osc99/LICENSE diff --git a/src/ux/Osc99/Osc99.h b/src/components/external/ngimu/Osc99/Osc99.h similarity index 100% rename from src/ux/Osc99/Osc99.h rename to src/components/external/ngimu/Osc99/Osc99.h diff --git a/src/ux/Osc99/OscAddress.c b/src/components/external/ngimu/Osc99/OscAddress.c similarity index 100% rename from src/ux/Osc99/OscAddress.c rename to src/components/external/ngimu/Osc99/OscAddress.c diff --git a/src/ux/Osc99/OscAddress.h b/src/components/external/ngimu/Osc99/OscAddress.h similarity index 100% rename from src/ux/Osc99/OscAddress.h rename to src/components/external/ngimu/Osc99/OscAddress.h diff --git a/src/ux/Osc99/OscBundle.cpp b/src/components/external/ngimu/Osc99/OscBundle.cpp similarity index 100% rename from src/ux/Osc99/OscBundle.cpp rename to src/components/external/ngimu/Osc99/OscBundle.cpp diff --git a/src/ux/Osc99/OscBundle.h b/src/components/external/ngimu/Osc99/OscBundle.h similarity index 100% rename from src/ux/Osc99/OscBundle.h rename to src/components/external/ngimu/Osc99/OscBundle.h diff --git a/src/ux/Osc99/OscCommon.c b/src/components/external/ngimu/Osc99/OscCommon.c similarity index 100% rename from src/ux/Osc99/OscCommon.c rename to src/components/external/ngimu/Osc99/OscCommon.c diff --git a/src/ux/Osc99/OscCommon.h b/src/components/external/ngimu/Osc99/OscCommon.h similarity index 100% rename from src/ux/Osc99/OscCommon.h rename to src/components/external/ngimu/Osc99/OscCommon.h diff --git a/src/ux/Osc99/OscError.c b/src/components/external/ngimu/Osc99/OscError.c similarity index 100% rename from src/ux/Osc99/OscError.c rename to src/components/external/ngimu/Osc99/OscError.c diff --git a/src/ux/Osc99/OscError.h b/src/components/external/ngimu/Osc99/OscError.h similarity index 100% rename from src/ux/Osc99/OscError.h rename to src/components/external/ngimu/Osc99/OscError.h diff --git a/src/ux/Osc99/OscMessage.cpp b/src/components/external/ngimu/Osc99/OscMessage.cpp similarity index 100% rename from src/ux/Osc99/OscMessage.cpp rename to src/components/external/ngimu/Osc99/OscMessage.cpp diff --git a/src/ux/Osc99/OscMessage.h b/src/components/external/ngimu/Osc99/OscMessage.h similarity index 100% rename from src/ux/Osc99/OscMessage.h rename to src/components/external/ngimu/Osc99/OscMessage.h diff --git a/src/ux/Osc99/OscPacket.cpp b/src/components/external/ngimu/Osc99/OscPacket.cpp similarity index 100% rename from src/ux/Osc99/OscPacket.cpp rename to src/components/external/ngimu/Osc99/OscPacket.cpp diff --git a/src/ux/Osc99/OscPacket.h b/src/components/external/ngimu/Osc99/OscPacket.h similarity index 100% rename from src/ux/Osc99/OscPacket.h rename to src/components/external/ngimu/Osc99/OscPacket.h diff --git a/src/ux/Osc99/OscSlip.cpp b/src/components/external/ngimu/Osc99/OscSlip.cpp similarity index 100% rename from src/ux/Osc99/OscSlip.cpp rename to src/components/external/ngimu/Osc99/OscSlip.cpp diff --git a/src/ux/Osc99/OscSlip.h b/src/components/external/ngimu/Osc99/OscSlip.h similarity index 100% rename from src/ux/Osc99/OscSlip.h rename to src/components/external/ngimu/Osc99/OscSlip.h diff --git a/src/ux/Osc99/README.md b/src/components/external/ngimu/Osc99/README.md similarity index 100% rename from src/ux/Osc99/README.md rename to src/components/external/ngimu/Osc99/README.md diff --git a/src/components/external/ngimu/ngimu.h b/src/components/external/ngimu/ngimu.h index 717c9f7..37f81c7 100644 --- a/src/components/external/ngimu/ngimu.h +++ b/src/components/external/ngimu/ngimu.h @@ -6,7 +6,7 @@ #include "utils/serial_port.h" #include "utils/threaded_loop.h" #include -#include "ux/Osc99/Osc99.h" +#include "Osc99/Osc99.h" #include #define NGIMU_BAUDRATE B115200 diff --git a/src/components/external/optitrack/CMakeLists.txt b/src/components/external/optitrack/CMakeLists.txt deleted file mode 100644 index 45a8749..0000000 --- a/src/components/external/optitrack/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -project(optitrack CXX) - -set(SOURCES - # cmake-format: sortable - optitrack_listener.cpp) - -add_library(optitrack ${SOURCES}) diff --git a/src/components/external/ximu/CMakeLists.txt b/src/components/external/ximu/CMakeLists.txt deleted file mode 100644 index 80c1a3f..0000000 --- a/src/components/external/ximu/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -project(ximu CXX) - -set(SOURCES - # cmake-format: sortable - ximu.cpp) - -add_library(ximu ${SOURCES}) diff --git a/src/components/internal/CMakeLists.txt b/src/components/internal/CMakeLists.txt deleted file mode 100644 index faea05f..0000000 --- a/src/components/internal/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -project(internal CXX) - -add_subdirectory(actuators) -add_subdirectory(adc) -add_subdirectory(dac) -add_subdirectory(gpio) -add_subdirectory(hand) - -add_library(internal INTERFACE) - -target_link_libraries(internal INTERFACE actuators adc dac gpio hand) diff --git a/src/components/internal/actuators/CMakeLists.txt b/src/components/internal/actuators/CMakeLists.txt deleted file mode 100644 index 4e8bc5f..0000000 --- a/src/components/internal/actuators/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -project(actuators CXX) - -set(SOURCES - # cmake-format: sortable - actuator.cpp - custom_elbow.cpp - osmer_elbow.cpp - pronosupination.cpp - shoulder_rotator.cpp - wrist_flexor.cpp - wrist_rotator.cpp - wrist_cybathlon.cpp - elbow_cybathlon.cpp) - -add_subdirectory(roboclaw) -add_subdirectory(epos) - -add_library(actuators ${SOURCES}) - -target_link_libraries(actuators roboclaw epos) diff --git a/src/components/internal/actuators/epos/CMakeLists.txt b/src/components/internal/actuators/epos/CMakeLists.txt deleted file mode 100644 index 6113d87..0000000 --- a/src/components/internal/actuators/epos/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -project(epos CXX) - -set(SOURCES - # cmake-format: sortable - epos.cpp) - -add_library(epos ${SOURCES}) - -#target_link_libraries(epos PUBLIC -# /home/charlotte/EPOS_Linux_Library/lib/arm/v7/libEposCmd.so.6.6.2.0 -# /home/charlotte/EPOS_Linux_Library/lib/arm/v7/libftd2xx.so.1.4.8 -#) diff --git a/src/components/internal/actuators/epos/Definitions.h b/src/components/internal/actuators/epos/Definitions.h deleted file mode 100644 index 7d92e85..0000000 --- a/src/components/internal/actuators/epos/Definitions.h +++ /dev/null @@ -1,525 +0,0 @@ -/************************************************************************************************************************************* -** maxon motor ag, CH-6072 Sachseln -************************************************************************************************************************************** -** -** FILE: Definitions.h -** -** Summary: Functions for Linux shared library -** -** Date: 11.2018 -** Target: x86, x86_64, arm (sf,hf,aarch64) -** Devices: EPOS, EPOS2, EPOS4 -** Written by: maxon motor ag, CH-6072 Sachseln -** -*************************************************************************************************************************************/ - -#ifndef _H_LINUX_EPOSCMD_ -#define _H_LINUX_EPOSCMD_ - -//Communication - int CreateCommunication(); - int DeleteCommunication(); - -// INITIALISATION FUNCTIONS - #define Initialisation_DllExport extern "C" - #define HelpFunctions_DllExport extern "C" -// CONFIGURATION FUNCTIONS - #define Configuration_DllExport extern "C" -// OPERATION FUNCTIONS - #define Status_DllExport extern "C" - #define StateMachine_DllExport extern "C" - #define ErrorHandling_DllExport extern "C" - #define MotionInfo_DllExport extern "C" - #define ProfilePositionMode_DllExport extern "C" - #define ProfileVelocityMode_DllExport extern "C" - #define HomingMode_DllExport extern "C" - #define InterpolatedPositionMode_DllExport extern "C" - #define PositionMode_DllExport extern "C" - #define VelocityMode_DllExport extern "C" - #define CurrentMode_DllExport extern "C" - #define MasterEncoderMode_DllExport extern "C" - #define StepDirectionMode_DllExport extern "C" - #define InputsOutputs_DllExport extern "C" -// LOW LAYER FUNCTIONS - #define CanLayer_DllExport extern "C" - -/************************************************************************************************************************************* -* INITIALISATION FUNCTIONS -*************************************************************************************************************************************/ - -//Communication - Initialisation_DllExport void* VCS_OpenDevice(char* DeviceName, char* ProtocolStackName, char* InterfaceName, char* PortName, unsigned int* pErrorCode); - Initialisation_DllExport int VCS_SetProtocolStackSettings(void* KeyHandle, unsigned int Baudrate, unsigned int Timeout, unsigned int* pErrorCode); - Initialisation_DllExport int VCS_GetProtocolStackSettings(void* KeyHandle, unsigned int* pBaudrate, unsigned int* pTimeout, unsigned int* pErrorCode); - Initialisation_DllExport int VCS_CloseDevice(void* KeyHandle, unsigned int* pErrorCode); - Initialisation_DllExport int VCS_CloseAllDevices(unsigned int* pErrorCode); - -//Gateway - Initialisation_DllExport int VCS_SetGatewaySettings(void* KeyHandle, unsigned int Baudrate, unsigned int* pErrorCode); - Initialisation_DllExport int VCS_GetGatewaySettings(void* KeyHandle, unsigned int* pBaudrate, unsigned int* pErrorCode); - -//Sub device - Initialisation_DllExport void* VCS_OpenSubDevice(void* DeviceHandle, char* DeviceName, char* ProtocolStackName, unsigned int* pErrorCode); - Initialisation_DllExport int VCS_CloseSubDevice(void* KeyHandle, unsigned int* pErrorCode); - Initialisation_DllExport int VCS_CloseAllSubDevices(void* DeviceHandle, unsigned int* pErrorCode); - -//Info - HelpFunctions_DllExport int VCS_GetDriverInfo(char* p_pszLibraryName, unsigned short p_usMaxLibraryNameStrSize,char* p_pszLibraryVersion, unsigned short p_usMaxLibraryVersionStrSize, unsigned int* p_pErrorCode); - HelpFunctions_DllExport int VCS_GetVersion(void* KeyHandle, unsigned short NodeId, unsigned short* pHardwareVersion, unsigned short* pSoftwareVersion, unsigned short* pApplicationNumber, unsigned short* pApplicationVersion, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetErrorInfo(unsigned int ErrorCodeValue, char* pErrorInfo, unsigned short MaxStrSize); - -//Advanced Functions - HelpFunctions_DllExport int VCS_GetDeviceNameSelection(int StartOfSelection, char* pDeviceNameSel, unsigned short MaxStrSize, int* pEndOfSelection, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetProtocolStackNameSelection(char* DeviceName, int StartOfSelection, char* pProtocolStackNameSel, unsigned short MaxStrSize, int* pEndOfSelection, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetInterfaceNameSelection(char* DeviceName, char* ProtocolStackName, int StartOfSelection, char* pInterfaceNameSel, unsigned short MaxStrSize, int* pEndOfSelection, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetPortNameSelection(char* DeviceName, char* ProtocolStackName, char* InterfaceName, int StartOfSelection, char* pPortSel, unsigned short MaxStrSize, int* pEndOfSelection, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_ResetPortNameSelection(char* DeviceName, char* ProtocolStackName, char* InterfaceName, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetBaudrateSelection(char* DeviceName, char* ProtocolStackName, char* InterfaceName, char* PortName, int StartOfSelection, unsigned int* pBaudrateSel, int* pEndOfSelection, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetKeyHandle(char* DeviceName, char* ProtocolStackName, char* InterfaceName, char* PortName, void** pKeyHandle, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetDeviceName(void* KeyHandle, char* pDeviceName, unsigned short MaxStrSize, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetProtocolStackName(void* KeyHandle, char* pProtocolStackName, unsigned short MaxStrSize, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetInterfaceName(void* KeyHandle, char* pInterfaceName, unsigned short MaxStrSize, unsigned int* pErrorCode); - HelpFunctions_DllExport int VCS_GetPortName(void* KeyHandle, char* pPortName, unsigned short MaxStrSize, unsigned int* pErrorCode); - -/************************************************************************************************************************************* -* CONFIGURATION FUNCTIONS -*************************************************************************************************************************************/ - -//General - Configuration_DllExport int VCS_SetObject(void* KeyHandle, unsigned short NodeId, unsigned short ObjectIndex, unsigned char ObjectSubIndex, void* pData, unsigned int NbOfBytesToWrite, unsigned int* pNbOfBytesWritten, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetObject(void* KeyHandle, unsigned short NodeId, unsigned short ObjectIndex, unsigned char ObjectSubIndex, void* pData, unsigned int NbOfBytesToRead, unsigned int* pNbOfBytesRead, unsigned int* pErrorCode); - Configuration_DllExport int VCS_Restore(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - Configuration_DllExport int VCS_Store(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - -//Advanced Functions - //Motor - Configuration_DllExport int VCS_SetMotorType(void* KeyHandle, unsigned short NodeId, unsigned short MotorType, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetDcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short NominalCurrent, unsigned short MaxOutputCurrent, unsigned short ThermalTimeConstant, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetEcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short NominalCurrent, unsigned short MaxOutputCurrent, unsigned short ThermalTimeConstant, unsigned char NbOfPolePairs, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetMotorType(void* KeyHandle, unsigned short NodeId, unsigned short* pMotorType, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetDcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pNominalCurrent, unsigned short* pMaxOutputCurrent, unsigned short* pThermalTimeConstant, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetEcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pNominalCurrent, unsigned short* pMaxOutputCurrent, unsigned short* pThermalTimeConstant, unsigned char* pNbOfPolePairs, unsigned int* pErrorCode); - - //Sensor - Configuration_DllExport int VCS_SetSensorType(void* KeyHandle, unsigned short NodeId, unsigned short SensorType, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetIncEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned int EncoderResolution, int InvertedPolarity, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetHallSensorParameter(void* KeyHandle, unsigned short NodeId, int InvertedPolarity, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetSsiAbsEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short DataRate, unsigned short NbOfMultiTurnDataBits, unsigned short NbOfSingleTurnDataBits, int InvertedPolarity, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetSsiAbsEncoderParameterEx(void* KeyHandle, unsigned short NodeId, unsigned short DataRate, unsigned short NbOfMultiTurnDataBits, unsigned short NbOfSingleTurnDataBits, unsigned short NbOfSpecialDataBits, int InvertedPolarity, unsigned short Timeout, unsigned short PowerupTime, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetSensorType(void* KeyHandle, unsigned short NodeId, unsigned short* pSensorType, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetIncEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned int* pEncoderResolution, int* pInvertedPolarity, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetHallSensorParameter(void* KeyHandle, unsigned short NodeId, int* pInvertedPolarity, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetSsiAbsEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pDataRate, unsigned short* pNbOfMultiTurnDataBits, unsigned short* pNbOfSingleTurnDataBits, int* pInvertedPolarity, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetSsiAbsEncoderParameterEx(void* KeyHandle, unsigned short NodeId, unsigned short* pDataRate, unsigned short* pNbOfMultiTurnDataBits, unsigned short* pNbOfSingleTurnDataBits, unsigned short* pNbOfSpecialDataBits, int* pInvertedPolarity, unsigned short* pTimeout, unsigned short* pPowerupTime, unsigned int* pErrorCode); - - //Safety - Configuration_DllExport int VCS_SetMaxFollowingError(void* KeyHandle, unsigned short NodeId, unsigned int MaxFollowingError, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetMaxFollowingError(void* KeyHandle, unsigned short NodeId, unsigned int* pMaxFollowingError, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetMaxProfileVelocity(void* KeyHandle, unsigned short NodeId, unsigned int MaxProfileVelocity, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetMaxProfileVelocity(void* KeyHandle, unsigned short NodeId, unsigned int* pMaxProfileVelocity, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetMaxAcceleration(void* KeyHandle, unsigned short NodeId, unsigned int MaxAcceleration, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetMaxAcceleration(void* KeyHandle, unsigned short NodeId, unsigned int* pMaxAcceleration, unsigned int* pErrorCode); - - //Controller Gains - Configuration_DllExport int VCS_SetControllerGain(void* KeyHandle, unsigned short NodeId, unsigned short EController, unsigned short EGain, unsigned long long Value, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetControllerGain(void* KeyHandle, unsigned short NodeId, unsigned short EController, unsigned short EGain, unsigned long long* pValue, unsigned int* pErrorCode); - - //Inputs/Outputs - Configuration_DllExport int VCS_DigitalInputConfiguration(void* KeyHandle, unsigned short NodeId, unsigned short DigitalInputNb, unsigned short Configuration, int Mask, int Polarity, int ExecutionMask, unsigned int* pErrorCode); - Configuration_DllExport int VCS_DigitalOutputConfiguration(void* KeyHandle, unsigned short NodeId, unsigned short DigitalOutputNb, unsigned short Configuration, int State, int Mask, int Polarity, unsigned int* pErrorCode); - Configuration_DllExport int VCS_AnalogInputConfiguration(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNb, unsigned short Configuration, int ExecutionMask, unsigned int* pErrorCode); - Configuration_DllExport int VCS_AnalogOutputConfiguration(void* KeyHandle, unsigned short NodeId, unsigned short AnalogOutputNb, unsigned short Configuration, unsigned int* pErrorCode); - - //Units - Configuration_DllExport int VCS_SetVelocityUnits(void* KeyHandle, unsigned short NodeId, unsigned char VelDimension, signed char VelNotation, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetVelocityUnits(void* KeyHandle, unsigned short NodeId, unsigned char* pVelDimension, char* pVelNotation, unsigned int* pErrorCode); - - //Compatibility Functions (do not use) - Configuration_DllExport int VCS_SetPositionRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short P, unsigned short I, unsigned short D, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetPositionRegulatorFeedForward(void* KeyHandle, unsigned short NodeId, unsigned short VelocityFeedForward, unsigned short AccelerationFeedForward, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetPositionRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short* pP, unsigned short* pI, unsigned short* pD, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetPositionRegulatorFeedForward(void* KeyHandle, unsigned short NodeId, unsigned short* pVelocityFeedForward, unsigned short* pAccelerationFeedForward, unsigned int* pErrorCode); - - Configuration_DllExport int VCS_SetVelocityRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short P, unsigned short I, unsigned int* pErrorCode); - Configuration_DllExport int VCS_SetVelocityRegulatorFeedForward(void* KeyHandle, unsigned short NodeId, unsigned short VelocityFeedForward, unsigned short AccelerationFeedForward, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetVelocityRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short* pP, unsigned short* pI, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetVelocityRegulatorFeedForward(void* KeyHandle, unsigned short NodeId, unsigned short* pVelocityFeedForward, unsigned short* pAccelerationFeedForward, unsigned int* pErrorCode); - - Configuration_DllExport int VCS_SetCurrentRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short P, unsigned short I, unsigned int* pErrorCode); - Configuration_DllExport int VCS_GetCurrentRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short* pP, unsigned short* pI, unsigned int* pErrorCode); - -/************************************************************************************************************************************* -* OPERATION FUNCTIONS -*************************************************************************************************************************************/ - -//OperationMode - Status_DllExport int VCS_SetOperationMode(void* KeyHandle, unsigned short NodeId, char OperationMode, unsigned int* pErrorCode); - Status_DllExport int VCS_GetOperationMode(void* KeyHandle, unsigned short NodeId, char* pOperationMode, unsigned int* pErrorCode); - -//StateMachine - StateMachine_DllExport int VCS_ResetDevice(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_SetState(void* KeyHandle, unsigned short NodeId, unsigned short State, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_SetEnableState(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_SetDisableState(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_SetQuickStopState(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_ClearFault(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_GetState(void* KeyHandle, unsigned short NodeId, unsigned short* pState, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_GetEnableState(void* KeyHandle, unsigned short NodeId, int* pIsEnabled, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_GetDisableState(void* KeyHandle, unsigned short NodeId, int* pIsDisabled, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_GetQuickStopState(void* KeyHandle, unsigned short NodeId, int* pIsQuickStopped, unsigned int* pErrorCode); - StateMachine_DllExport int VCS_GetFaultState(void* KeyHandle, unsigned short NodeId, int* pIsInFault, unsigned int* pErrorCode); - -//ErrorHandling - ErrorHandling_DllExport int VCS_GetNbOfDeviceError(void* KeyHandle, unsigned short NodeId, unsigned char *pNbDeviceError, unsigned int *pErrorCode); - ErrorHandling_DllExport int VCS_GetDeviceErrorCode(void* KeyHandle, unsigned short NodeId, unsigned char DeviceErrorNumber, unsigned int *pDeviceErrorCode, unsigned int *pErrorCode); - -//Motion Info - MotionInfo_DllExport int VCS_GetMovementState(void* KeyHandle, unsigned short NodeId, int* pTargetReached, unsigned int* pErrorCode); - MotionInfo_DllExport int VCS_GetPositionIs(void* KeyHandle, unsigned short NodeId, int* pPositionIs, unsigned int* pErrorCode); - MotionInfo_DllExport int VCS_GetVelocityIs(void* KeyHandle, unsigned short NodeId, int* pVelocityIs, unsigned int* pErrorCode); - MotionInfo_DllExport int VCS_GetVelocityIsAveraged(void* KeyHandle, unsigned short NodeId, int* pVelocityIsAveraged, unsigned int* pErrorCode); - MotionInfo_DllExport int VCS_GetCurrentIs(void* KeyHandle, unsigned short NodeId, short* pCurrentIs, unsigned int* pErrorCode); - MotionInfo_DllExport int VCS_GetCurrentIsAveraged(void* KeyHandle, unsigned short NodeId, short* pCurrentIsAveraged, unsigned int* pErrorCode); - MotionInfo_DllExport int VCS_WaitForTargetReached(void* KeyHandle, unsigned short NodeId, unsigned int Timeout, unsigned int* pErrorCode); - -//Profile Position Mode - ProfilePositionMode_DllExport int VCS_ActivateProfilePositionMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - ProfilePositionMode_DllExport int VCS_SetPositionProfile(void* KeyHandle, unsigned short NodeId, unsigned int ProfileVelocity, unsigned int ProfileAcceleration, unsigned int ProfileDeceleration, unsigned int* pErrorCode); - ProfilePositionMode_DllExport int VCS_GetPositionProfile(void* KeyHandle, unsigned short NodeId, unsigned int* pProfileVelocity, unsigned int* pProfileAcceleration, unsigned int* pProfileDeceleration, unsigned int* pErrorCode); - ProfilePositionMode_DllExport int VCS_MoveToPosition(void* KeyHandle, unsigned short NodeId, long TargetPosition, int Absolute, int Immediately, unsigned int* pErrorCode); - ProfilePositionMode_DllExport int VCS_GetTargetPosition(void* KeyHandle, unsigned short NodeId, long* pTargetPosition, unsigned int* pErrorCode); - ProfilePositionMode_DllExport int VCS_HaltPositionMovement(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - - //Advanced Functions - ProfilePositionMode_DllExport int VCS_EnablePositionWindow(void* KeyHandle, unsigned short NodeId, unsigned int PositionWindow, unsigned short PositionWindowTime, unsigned int* pErrorCode); - ProfilePositionMode_DllExport int VCS_DisablePositionWindow(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - -//Profile Velocity Mode - ProfileVelocityMode_DllExport int VCS_ActivateProfileVelocityMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - ProfileVelocityMode_DllExport int VCS_SetVelocityProfile(void* KeyHandle, unsigned short NodeId, unsigned int ProfileAcceleration, unsigned int ProfileDeceleration, unsigned int* pErrorCode); - ProfileVelocityMode_DllExport int VCS_GetVelocityProfile(void* KeyHandle, unsigned short NodeId, unsigned int* pProfileAcceleration, unsigned int* pProfileDeceleration, unsigned int* pErrorCode); - ProfileVelocityMode_DllExport int VCS_MoveWithVelocity(void* KeyHandle, unsigned short NodeId, long TargetVelocity, unsigned int* pErrorCode); - ProfileVelocityMode_DllExport int VCS_GetTargetVelocity(void* KeyHandle, unsigned short NodeId, long* pTargetVelocity, unsigned int* pErrorCode); - ProfileVelocityMode_DllExport int VCS_HaltVelocityMovement(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - - //Advanced Functions - ProfileVelocityMode_DllExport int VCS_EnableVelocityWindow(void* KeyHandle, unsigned short NodeId, unsigned int VelocityWindow, unsigned short VelocityWindowTime, unsigned int* pErrorCode); - ProfileVelocityMode_DllExport int VCS_DisableVelocityWindow(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - -//Homing Mode - HomingMode_DllExport int VCS_ActivateHomingMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - HomingMode_DllExport int VCS_SetHomingParameter(void* KeyHandle, unsigned short NodeId, unsigned int HomingAcceleration, unsigned int SpeedSwitch, unsigned int SpeedIndex, int HomeOffset, unsigned short CurrentThreshold, int HomePosition, unsigned int* pErrorCode); - HomingMode_DllExport int VCS_GetHomingParameter(void* KeyHandle, unsigned short NodeId, unsigned int* pHomingAcceleration, unsigned int* pSpeedSwitch, unsigned int* pSpeedIndex, int* pHomeOffset, unsigned short* pCurrentThreshold, int* pHomePosition, unsigned int* pErrorCode); - HomingMode_DllExport int VCS_FindHome(void* KeyHandle, unsigned short NodeId, signed char HomingMethod, unsigned int* pErrorCode); - HomingMode_DllExport int VCS_StopHoming(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - HomingMode_DllExport int VCS_DefinePosition(void* KeyHandle, unsigned short NodeId, int HomePosition, unsigned int* pErrorCode); - HomingMode_DllExport int VCS_WaitForHomingAttained(void* KeyHandle, unsigned short NodeId, int Timeout, unsigned int* pErrorCode); - HomingMode_DllExport int VCS_GetHomingState(void* KeyHandle, unsigned short NodeId, int* pHomingAttained, int* pHomingError, unsigned int* pErrorCode); - -//Interpolated Position Mode - InterpolatedPositionMode_DllExport int VCS_ActivateInterpolatedPositionMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - InterpolatedPositionMode_DllExport int VCS_SetIpmBufferParameter(void* KeyHandle, unsigned short NodeId, unsigned short UnderflowWarningLimit, unsigned short OverflowWarningLimit, unsigned int* pErrorCode); - InterpolatedPositionMode_DllExport int VCS_GetIpmBufferParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pUnderflowWarningLimit, unsigned short* pOverflowWarningLimit, unsigned int* pMaxBufferSize, unsigned int* pErrorCode); - InterpolatedPositionMode_DllExport int VCS_ClearIpmBuffer(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - InterpolatedPositionMode_DllExport int VCS_GetFreeIpmBufferSize(void* KeyHandle, unsigned short NodeId, unsigned int* pBufferSize, unsigned int* pErrorCode); - InterpolatedPositionMode_DllExport int VCS_AddPvtValueToIpmBuffer(void* KeyHandle, unsigned short NodeId, long Position, long Velocity, unsigned char Time, unsigned int* pErrorCode); - InterpolatedPositionMode_DllExport int VCS_StartIpmTrajectory(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - InterpolatedPositionMode_DllExport int VCS_StopIpmTrajectory(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - InterpolatedPositionMode_DllExport int VCS_GetIpmStatus(void* KeyHandle, unsigned short NodeId, int* pTrajectoryRunning, int* pIsUnderflowWarning, int* pIsOverflowWarning, int* pIsVelocityWarning, int* pIsAccelerationWarning, int* pIsUnderflowError, int* pIsOverflowError, int* pIsVelocityError, int* pIsAccelerationError, unsigned int* pErrorCode); - -//Position Mode - PositionMode_DllExport int VCS_ActivatePositionMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - PositionMode_DllExport int VCS_SetPositionMust(void* KeyHandle, unsigned short NodeId, long PositionMust, unsigned int* pErrorCode); - PositionMode_DllExport int VCS_GetPositionMust(void* KeyHandle, unsigned short NodeId, long* pPositionMust, unsigned int* pErrorCode); - - //Advanced Functions - PositionMode_DllExport int VCS_ActivateAnalogPositionSetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, float Scaling, long Offset, unsigned int* pErrorCode); - PositionMode_DllExport int VCS_DeactivateAnalogPositionSetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, unsigned int* pErrorCode); - PositionMode_DllExport int VCS_EnableAnalogPositionSetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - PositionMode_DllExport int VCS_DisableAnalogPositionSetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - -//Velocity Mode - VelocityMode_DllExport int VCS_ActivateVelocityMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - VelocityMode_DllExport int VCS_SetVelocityMust(void* KeyHandle, unsigned short NodeId, long VelocityMust, unsigned int* pErrorCode); - VelocityMode_DllExport int VCS_GetVelocityMust(void* KeyHandle, unsigned short NodeId, long* pVelocityMust, unsigned int* pErrorCode); - - //Advanced Functions - VelocityMode_DllExport int VCS_ActivateAnalogVelocitySetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, float Scaling, long Offset, unsigned int* pErrorCode); - VelocityMode_DllExport int VCS_DeactivateAnalogVelocitySetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, unsigned int* pErrorCode); - VelocityMode_DllExport int VCS_EnableAnalogVelocitySetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - VelocityMode_DllExport int VCS_DisableAnalogVelocitySetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - -//Current Mode - CurrentMode_DllExport int VCS_ActivateCurrentMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - CurrentMode_DllExport int VCS_SetCurrentMust(void* KeyHandle, unsigned short NodeId, short CurrentMust, unsigned int* pErrorCode); - CurrentMode_DllExport int VCS_GetCurrentMust(void* KeyHandle, unsigned short NodeId, short* pCurrentMust, unsigned int* pErrorCode); - - //Advanced Functions - CurrentMode_DllExport int VCS_ActivateAnalogCurrentSetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, float Scaling, short Offset, unsigned int* pErrorCode); - CurrentMode_DllExport int VCS_DeactivateAnalogCurrentSetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, unsigned int* pErrorCode); - CurrentMode_DllExport int VCS_EnableAnalogCurrentSetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - CurrentMode_DllExport int VCS_DisableAnalogCurrentSetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - -//MasterEncoder Mode - MasterEncoderMode_DllExport int VCS_ActivateMasterEncoderMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - MasterEncoderMode_DllExport int VCS_SetMasterEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short ScalingNumerator, unsigned short ScalingDenominator, unsigned char Polarity, unsigned int MaxVelocity, unsigned int MaxAcceleration, unsigned int* pErrorCode); - MasterEncoderMode_DllExport int VCS_GetMasterEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pScalingNumerator, unsigned short* pScalingDenominator, unsigned char* pPolarity, unsigned int* pMaxVelocity, unsigned int* pMaxAcceleration, unsigned int* pErrorCode); - -//StepDirection Mode - StepDirectionMode_DllExport int VCS_ActivateStepDirectionMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - StepDirectionMode_DllExport int VCS_SetStepDirectionParameter(void* KeyHandle, unsigned short NodeId, unsigned short ScalingNumerator, unsigned short ScalingDenominator, unsigned char Polarity, unsigned int MaxVelocity, unsigned int MaxAcceleration, unsigned int* pErrorCode); - StepDirectionMode_DllExport int VCS_GetStepDirectionParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pScalingNumerator, unsigned short* pScalingDenominator, unsigned char* pPolarity, unsigned int* pMaxVelocity, unsigned int* pMaxAcceleration, unsigned int* pErrorCode); - -//Inputs Outputs - //General - InputsOutputs_DllExport int VCS_GetAllDigitalInputs(void* KeyHandle, unsigned short NodeId, unsigned short* pInputs, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_GetAllDigitalOutputs(void* KeyHandle, unsigned short NodeId, unsigned short* pOutputs, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_SetAllDigitalOutputs(void* KeyHandle, unsigned short NodeId, unsigned short Outputs, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_GetAnalogInput(void* KeyHandle, unsigned short NodeId, unsigned short InputNumber, unsigned short* pAnalogValue, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_GetAnalogInputVoltage(void* KeyHandle, unsigned short NodeId, unsigned short InputNumber, long* pVoltageValue, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_GetAnalogInputState(void* KeyHandle, unsigned short NodeId, unsigned short Configuration, long* pStateValue, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_SetAnalogOutput(void* KeyHandle, unsigned short NodeId, unsigned short OutputNumber, unsigned short AnalogValue, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_SetAnalogOutputVoltage(void* KeyHandle, unsigned short NodeId, unsigned short OutputNumber, long VoltageValue, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_SetAnalogOutputState(void* KeyHandle, unsigned short NodeId, unsigned short Configuration, long StateValue, unsigned int* pErrorCode); - - //Position Compare - InputsOutputs_DllExport int VCS_SetPositionCompareParameter(void* KeyHandle, unsigned short NodeId, unsigned char OperationalMode, unsigned char IntervalMode, unsigned char DirectionDependency, unsigned short IntervalWidth, unsigned short IntervalRepetitions, unsigned short PulseWidth, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_GetPositionCompareParameter(void* KeyHandle, unsigned short NodeId, unsigned char* pOperationalMode, unsigned char* pIntervalMode, unsigned char* pDirectionDependency, unsigned short* pIntervalWidth, unsigned short* pIntervalRepetitions, unsigned short* pPulseWidth, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_ActivatePositionCompare(void* KeyHandle, unsigned short NodeId, unsigned short DigitalOutputNumber, int Polarity, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_DeactivatePositionCompare(void* KeyHandle, unsigned short NodeId, unsigned short DigitalOutputNumber, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_EnablePositionCompare(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_DisablePositionCompare(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_SetPositionCompareReferencePosition(void* KeyHandle, unsigned short NodeId, long ReferencePosition, unsigned int* pErrorCode); - - //Position Marker - InputsOutputs_DllExport int VCS_SetPositionMarkerParameter(void* KeyHandle, unsigned short NodeId, unsigned char PositionMarkerEdgeType, unsigned char PositionMarkerMode, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_GetPositionMarkerParameter(void* KeyHandle, unsigned short NodeId, unsigned char* pPositionMarkerEdgeType, unsigned char* pPositionMarkerMode, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_ActivatePositionMarker(void* KeyHandle, unsigned short NodeId, unsigned short DigitalInputNumber, int Polarity, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_DeactivatePositionMarker(void* KeyHandle, unsigned short NodeId, unsigned short DigitalInputNumber, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_ReadPositionMarkerCounter(void* KeyHandle, unsigned short NodeId, unsigned short* pCount, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_ReadPositionMarkerCapturedPosition(void* KeyHandle, unsigned short NodeId, unsigned short CounterIndex, long* pCapturedPosition, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_ResetPositionMarkerCounter(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); - -/************************************************************************************************************************************* -* LOW LAYER FUNCTIONS -*************************************************************************************************************************************/ - -//CanLayer Functions - CanLayer_DllExport int VCS_SendCANFrame(void* KeyHandle, unsigned short CobID, unsigned short Length, void* pData, unsigned int* pErrorCode); - CanLayer_DllExport int VCS_ReadCANFrame(void* KeyHandle, unsigned short CobID, unsigned short Length, void* pData, unsigned int Timeout, unsigned int* pErrorCode); - CanLayer_DllExport int VCS_RequestCANFrame(void* KeyHandle, unsigned short CobID, unsigned short Length, void* pData, unsigned int* pErrorCode); - CanLayer_DllExport int VCS_SendNMTService(void* KeyHandle, unsigned short NodeId, unsigned short CommandSpecifier, unsigned int* pErrorCode); - -/************************************************************************************************************************************* -* TYPE DEFINITIONS -*************************************************************************************************************************************/ -//Communication - //Dialog Mode - const int DM_PROGRESS_DLG = 0; - const int DM_PROGRESS_AND_CONFIRM_DLG = 1; - const int DM_CONFIRM_DLG = 2; - const int DM_NO_DLG = 3; - -//Configuration - //MotorType - const unsigned short MT_DC_MOTOR = 1; - const unsigned short MT_EC_SINUS_COMMUTATED_MOTOR = 10; - const unsigned short MT_EC_BLOCK_COMMUTATED_MOTOR = 11; - - //SensorType - const unsigned short ST_UNKNOWN = 0; - const unsigned short ST_INC_ENCODER_3CHANNEL = 1; - const unsigned short ST_INC_ENCODER_2CHANNEL = 2; - const unsigned short ST_HALL_SENSORS = 3; - const unsigned short ST_SSI_ABS_ENCODER_BINARY = 4; - const unsigned short ST_SSI_ABS_ENCODER_GREY = 5; - const unsigned short ST_INC_ENCODER2_3CHANNEL = 6; - const unsigned short ST_INC_ENCODER2_2CHANNEL = 7; - const unsigned short ST_ANALOG_INC_ENCODER_3CHANNEL = 8; - const unsigned short ST_ANALOG_INC_ENCODER_2CHANNEL = 9; - -//In- and outputs - //Digital input configuration - const unsigned short DIC_NEGATIVE_LIMIT_SWITCH = 0; - const unsigned short DIC_POSITIVE_LIMIT_SWITCH = 1; - const unsigned short DIC_HOME_SWITCH = 2; - const unsigned short DIC_POSITION_MARKER = 3; - const unsigned short DIC_DRIVE_ENABLE = 4; - const unsigned short DIC_QUICK_STOP = 5; - const unsigned short DIC_GENERAL_PURPOSE_J = 6; - const unsigned short DIC_GENERAL_PURPOSE_I = 7; - const unsigned short DIC_GENERAL_PURPOSE_H = 8; - const unsigned short DIC_GENERAL_PURPOSE_G = 9; - const unsigned short DIC_GENERAL_PURPOSE_F = 10; - const unsigned short DIC_GENERAL_PURPOSE_E = 11; - const unsigned short DIC_GENERAL_PURPOSE_D = 12; - const unsigned short DIC_GENERAL_PURPOSE_C = 13; - const unsigned short DIC_GENERAL_PURPOSE_B = 14; - const unsigned short DIC_GENERAL_PURPOSE_A = 15; - - //Digital output configuration - const unsigned short DOC_READY_FAULT = 0; - const unsigned short DOC_POSITION_COMPARE = 1; - const unsigned short DOC_GENERAL_PURPOSE_H = 8; - const unsigned short DOC_GENERAL_PURPOSE_G = 9; - const unsigned short DOC_GENERAL_PURPOSE_F = 10; - const unsigned short DOC_GENERAL_PURPOSE_E = 11; - const unsigned short DOC_GENERAL_PURPOSE_D = 12; - const unsigned short DOC_GENERAL_PURPOSE_C = 13; - const unsigned short DOC_GENERAL_PURPOSE_B = 14; - const unsigned short DOC_GENERAL_PURPOSE_A = 15; - - //Analog input configuration - const unsigned short AIC_ANALOG_CURRENT_SETPOINT = 0; - const unsigned short AIC_ANALOG_VELOCITY_SETPOINT = 1; - const unsigned short AIC_ANALOG_POSITION_SETPOINT = 2; - const unsigned short AIC_GENERAL_PURPOSE_H = 8; - const unsigned short AIC_GENERAL_PURPOSE_G = 9; - const unsigned short AIC_GENERAL_PURPOSE_F = 10; - const unsigned short AIC_GENERAL_PURPOSE_E = 11; - const unsigned short AIC_GENERAL_PURPOSE_D = 12; - const unsigned short AIC_GENERAL_PURPOSE_C = 13; - const unsigned short AIC_GENERAL_PURPOSE_B = 14; - const unsigned short AIC_GENERAL_PURPOSE_A = 15; - - //Analog output configuration - const unsigned short AOC_GENERAL_PURPOSE_A = 0; - const unsigned short AOC_GENERAL_PURPOSE_B = 1; - -//Units - //VelocityDimension - const unsigned char VD_RPM = 0xA4; - - //VelocityNotation - const signed char VN_STANDARD = 0; - const signed char VN_DECI = -1; - const signed char VN_CENTI = -2; - const signed char VN_MILLI = -3; - -//Operational mode - const signed char OMD_PROFILE_POSITION_MODE = 1; - const signed char OMD_PROFILE_VELOCITY_MODE = 3; - const signed char OMD_HOMING_MODE = 6; - const signed char OMD_INTERPOLATED_POSITION_MODE = 7; - const signed char OMD_POSITION_MODE = -1; - const signed char OMD_VELOCITY_MODE = -2; - const signed char OMD_CURRENT_MODE = -3; - const signed char OMD_MASTER_ENCODER_MODE = -5; - const signed char OMD_STEP_DIRECTION_MODE = -6; - -//States - const unsigned short ST_DISABLED = 0; - const unsigned short ST_ENABLED = 1; - const unsigned short ST_QUICKSTOP = 2; - const unsigned short ST_FAULT = 3; - -//Homing mode - //Homing method - const char HM_ACTUAL_POSITION = 35; - const char HM_NEGATIVE_LIMIT_SWITCH = 17; - const char HM_NEGATIVE_LIMIT_SWITCH_AND_INDEX = 1; - const char HM_POSITIVE_LIMIT_SWITCH = 18; - const char HM_POSITIVE_LIMIT_SWITCH_AND_INDEX = 2; - const char HM_HOME_SWITCH_POSITIVE_SPEED = 23; - const char HM_HOME_SWITCH_POSITIVE_SPEED_AND_INDEX = 7; - const char HM_HOME_SWITCH_NEGATIVE_SPEED = 27; - const char HM_HOME_SWITCH_NEGATIVE_SPEED_AND_INDEX = 11; - const char HM_CURRENT_THRESHOLD_POSITIVE_SPEED = -3; - const char HM_CURRENT_THRESHOLD_POSITIVE_SPEED_AND_INDEX = -1; - const char HM_CURRENT_THRESHOLD_NEGATIVE_SPEED = -4; - const char HM_CURRENT_THRESHOLD_NEGATIVE_SPEED_AND_INDEX = -2; - const char HM_INDEX_POSITIVE_SPEED = 34; - const char HM_INDEX_NEGATIVE_SPEED = 33; - -//Input Output PositionMarker - //PositionMarkerEdgeType - const unsigned char PET_BOTH_EDGES = 0; - const unsigned char PET_RISING_EDGE = 1; - const unsigned char PET_FALLING_EDGE = 2; - - //PositionMarkerMode - const unsigned char PM_CONTINUOUS = 0; - const unsigned char PM_SINGLE = 1; - const unsigned char PM_MULTIPLE = 2; - -//Input Output PositionCompare - //PositionCompareOperationalMode - const unsigned char PCO_SINGLE_POSITION_MODE = 0; - const unsigned char PCO_POSITION_SEQUENCE_MODE = 1; - - //PositionCompareIntervalMode - const unsigned char PCI_NEGATIVE_DIR_TO_REFPOS = 0; - const unsigned char PCI_POSITIVE_DIR_TO_REFPOS = 1; - const unsigned char PCI_BOTH_DIR_TO_REFPOS = 2; - - //PositionCompareDirectionDependency - const unsigned char PCD_MOTOR_DIRECTION_NEGATIVE = 0; - const unsigned char PCD_MOTOR_DIRECTION_POSITIVE = 1; - const unsigned char PCD_MOTOR_DIRECTION_BOTH = 2; - -//Data recorder - //Trigger type - const unsigned short DR_MOVEMENT_START_TRIGGER = 1; //bit 1 - const unsigned short DR_ERROR_TRIGGER = 2; //bit 2 - const unsigned short DR_DIGITAL_INPUT_TRIGGER = 4; //bit 3 - const unsigned short DR_MOVEMENT_END_TRIGGER = 8; //bit 4 - -//CanLayer Functions - const unsigned short NCS_START_REMOTE_NODE = 1; - const unsigned short NCS_STOP_REMOTE_NODE = 2; - const unsigned short NCS_ENTER_PRE_OPERATIONAL = 128; - const unsigned short NCS_RESET_NODE = 129; - const unsigned short NCS_RESET_COMMUNICATION = 130; - -// Controller Gains - // EController - const unsigned short EC_PI_CURRENT_CONTROLLER = 1; - const unsigned short EC_PI_VELOCITY_CONTROLLER = 10; - const unsigned short EC_PI_VELOCITY_CONTROLLER_WITH_OBSERVER = 11; - const unsigned short EC_PID_POSITION_CONTROLLER = 20; - const unsigned short EC_DUAL_LOOP_POSITION_CONTROLLER = 21; - - // EGain (EC_PI_CURRENT_CONTROLLER) - const unsigned short EG_PICC_P_GAIN = 1; - const unsigned short EG_PICC_I_GAIN = 2; - - // EGain (EC_PI_VELOCITY_CONTROLLER) - const unsigned short EG_PIVC_P_GAIN = 1; - const unsigned short EG_PIVC_I_GAIN = 2; - const unsigned short EG_PIVC_FEED_FORWARD_VELOCITY_GAIN = 10; - const unsigned short EG_PIVC_FEED_FORWARD_ACCELERATION_GAIN = 11; - - // EGain (EC_PI_VELOCITY_CONTROLLER_WITH_OBSERVER) - const unsigned short EG_PIVCWO_P_GAIN = 1; - const unsigned short EG_PIVCWO_I_GAIN = 2; - const unsigned short EG_PIVCWO_FEED_FORWARD_VELOCITY_GAIN = 10; - const unsigned short EG_PIVCWO_FEED_FORWARD_ACCELERATION_GAIN = 11; - const unsigned short EG_PIVCWO_OBSERVER_THETA_GAIN = 20; - const unsigned short EG_PIVCWO_OBSERVER_OMEGA_GAIN = 21; - const unsigned short EG_PIVCWO_OBSERVER_TAU_GAIN = 22; - - // EGain (EC_PID_POSITION_CONTROLLER) - const unsigned short EG_PIDPC_P_GAIN = 1; - const unsigned short EG_PIDPC_I_GAIN = 2; - const unsigned short EG_PIDPC_D_GAIN = 3; - const unsigned short EG_PIDPC_FEED_FORWARD_VELOCITY_GAIN = 10; - const unsigned short EG_PIDPC_FEED_FORWARD_ACCELERATION_GAIN = 11; - - // EGain (EC_DUAL_LOOP_POSITION_CONTROLLER) - const unsigned short EG_DLPC_AUXILIARY_LOOP_P_GAIN = 1; - const unsigned short EG_DLPC_AUXILIARY_LOOP_I_GAIN = 2; - const unsigned short EG_DLPC_AUXILIARY_LOOP_FEED_FORWARD_VELOCITY_GAIN = 10; - const unsigned short EG_DLPC_AUXILIARY_LOOP_FEED_FORWARD_ACCELERATION_GAIN = 11; - const unsigned short EG_DLPC_AUXILIARY_LOOP_OBSERVER_THETA_GAIN = 20; - const unsigned short EG_DLPC_AUXILIARY_LOOP_OBSERVER_OMEGA_GAIN = 21; - const unsigned short EG_DLPC_AUXILIARY_LOOP_OBSERVER_TAU_GAIN = 22; - const unsigned short EG_DLPC_MAIN_LOOP_P_GAIN_LOW = 101; - const unsigned short EG_DLPC_MAIN_LOOP_P_GAIN_HIGH = 102; - const unsigned short EG_DLPC_MAIN_LOOP_GAIN_SCHEDULING_WEIGHT = 110; - const unsigned short EG_DLPC_MAIN_LOOP_FILTER_COEFFICIENT_A = 120; - const unsigned short EG_DLPC_MAIN_LOOP_FILTER_COEFFICIENT_B = 121; - const unsigned short EG_DLPC_MAIN_LOOP_FILTER_COEFFICIENT_C = 122; - const unsigned short EG_DLPC_MAIN_LOOP_FILTER_COEFFICIENT_D = 123; - const unsigned short EG_DLPC_MAIN_LOOP_FILTER_COEFFICIENT_E = 124; - -#endif //_H_LINUX_EPOSCMD_ diff --git a/src/components/internal/actuators/epos/epos.cpp b/src/components/internal/actuators/epos/epos.cpp index 04247e2..39e300f 100644 --- a/src/components/internal/actuators/epos/epos.cpp +++ b/src/components/internal/actuators/epos/epos.cpp @@ -1,37 +1,45 @@ #include "epos.h" #include -#include "Definitions.h" +#include -EPOS::EPOS() +EPOS::EPOS(std::string name, std::string portName, unsigned short nodeId) : MenuUser("", "", [this]() { on_exit(); }) - , _usNodeId(1) + , _name(name) + , _usNodeId(nodeId) , _deviceName("EPOS4") , _protocolStackName("MAXON SERIAL V2") //MAXON_RS232, CANopen ou MAXON SERIAL V2 - , _interfaceName("USB") //RS232, USB, CAN_ixx_usb, CAN_kvaser_usb 0, etc. - , _portName("USB0") //COM1, USB0, CAN0, etc. - , _baudrate(1000000) + , _interfaceName("RS232") //RS232, USB, CAN_ixx_usb, CAN_kvaser_usb 0, etc. + , _portName(portName) //COM1, USB0, CAN0, etc. + , _baudrate(115200) { - _menu->set_description("EPOS4"); - _menu->set_code("pronosup"); + _menu->set_description("EPOS4 "+ name); + _menu->set_code(name); - OpenDevice(); - - _menu->add_item("s", "Stop", [this](std::string) { stop(); }); - _menu->add_item("g", "Go to", [this](std::string args) {if (args.length() > 0) move_to(std::stol(args)); }); - _menu->add_item("v", "Set velocity (deg/s)", [this](std::string args) { if (args.length() > 0) args = "0"; set_velocity(std::stol(args)); }); + if(OpenDevice() == MMC_SUCCESS) { + //unsigned int errorCode; + //if(PrepareDemo(&errorCode) == MMC_SUCCESS ) { + _menu->add_item("s", "Stop", [this](std::string) { stop(); }); + _menu->add_item("g", "Go to", [this](std::string args) {if (args.length() > 0) move_to(std::stol(args)); }); + _menu->add_item("v", "Set velocity (deg/s)", [this](std::string args) {if (args.length() > 0) set_velocity(std::stol(args)); }); + //} + } else { + std::cout<<"Unable to open EPOS device"<< std::endl; + } + _menu->add_item("l", "List available interfaces", [this](std::string) { PrintAvailableInterfaces(); }); + _menu->add_item("b", "Get available baudrates", [this](std::string) {GetBaudRate();}); } EPOS::~EPOS(){} void EPOS::LogError(std::string functionName, int p_lResult, unsigned int p_ulErrorCode) { - std::cerr << _programName << ": " << functionName << " failed (result=" << p_lResult << ", errorCode=0x" << std::hex << p_ulErrorCode << ")"<< std::endl; + std::cerr << _name << ": " << functionName << " failed (result=" << p_lResult << ", errorCode=0x" << std::hex << p_ulErrorCode << ")"<< std::endl; } int EPOS::OpenDevice() { int lResult = MMC_FAILED; - unsigned int p_pErrorCode; + unsigned int errorCode; char* pDeviceName = new char[255]; char* pProtocolStackName = new char[255]; @@ -43,19 +51,20 @@ int EPOS::OpenDevice() strcpy(pInterfaceName, _interfaceName.c_str()); strcpy(pPortName, _portName.c_str()); - _pKeyHandle = VCS_OpenDevice(pDeviceName, pProtocolStackName, pInterfaceName, pPortName, &p_pErrorCode); + _keyHandle = VCS_OpenDevice(pDeviceName, pProtocolStackName, pInterfaceName, pPortName, &errorCode); - if(_pKeyHandle!=nullptr && p_pErrorCode == 0) + if(_keyHandle!=nullptr && errorCode == 0) { unsigned int lBaudrate = 0; unsigned int lTimeout = 0; - if(VCS_GetProtocolStackSettings(_pKeyHandle, &lBaudrate, &lTimeout, &p_pErrorCode)!=0) + if(VCS_GetProtocolStackSettings(_keyHandle, &lBaudrate, &lTimeout, &errorCode)!=0) { - if(VCS_SetProtocolStackSettings(_pKeyHandle, _baudrate, lTimeout, &p_pErrorCode)!=0) + if(VCS_SetProtocolStackSettings(_keyHandle, _baudrate, lTimeout, &errorCode)!=0) { - if(VCS_GetProtocolStackSettings(_pKeyHandle, &lBaudrate, &lTimeout, &p_pErrorCode)!=0) + if(VCS_GetProtocolStackSettings(_keyHandle, &lBaudrate, &lTimeout, &errorCode)!=0) { + std::cout << lBaudrate << std::endl; if(_baudrate==lBaudrate) { lResult = MMC_SUCCESS; @@ -66,7 +75,7 @@ int EPOS::OpenDevice() } else { - _pKeyHandle = nullptr; + _keyHandle = nullptr; } delete []pDeviceName; @@ -77,18 +86,18 @@ int EPOS::OpenDevice() return lResult; } -int EPOS::CloseDevice(unsigned int* p_pErrorCode) +int EPOS::CloseDevice() { int lResult = MMC_SUCCESS; - *p_pErrorCode = 0; + unsigned int errorCode = 0; - if(VCS_SetDisableState(_pKeyHandle, _usNodeId, p_pErrorCode) == 0) + if(VCS_SetDisableState(_keyHandle, _usNodeId, &errorCode) == 0) { - LogError("VCS_SetDisableState", lResult, *p_pErrorCode); + LogError("VCS_SetDisableState", lResult, errorCode); lResult = MMC_FAILED; } else { - if(VCS_CloseDevice(_pKeyHandle, p_pErrorCode)==0 || *p_pErrorCode != 0) + if(VCS_CloseDevice(_keyHandle, &errorCode)==0 || errorCode != 0) { lResult = MMC_FAILED; } @@ -102,9 +111,15 @@ int EPOS::move_to(long targetPosition, bool absolute, bool immediately) int lResult = MMC_SUCCESS; unsigned int p_rlErrorCode=0; + if((lResult = PrepareDemo(&p_rlErrorCode))!=MMC_SUCCESS) + { + LogError("PrepareDemo", lResult, p_rlErrorCode); + return lResult; + } + std::cout << "set profile position mode, node = " << _usNodeId << std::endl; - if(VCS_ActivateProfilePositionMode(_pKeyHandle, _usNodeId, &p_rlErrorCode) == 0) + if(VCS_ActivateProfilePositionMode(_keyHandle, _usNodeId, &p_rlErrorCode) == 0) { LogError("VCS_ActivateProfilePositionMode", lResult, p_rlErrorCode); lResult = MMC_FAILED; @@ -114,7 +129,7 @@ int EPOS::move_to(long targetPosition, bool absolute, bool immediately) if (targetPosition != -1) { std::cout << "Moving to position = " << targetPosition << std::endl; - if(VCS_MoveToPosition(_pKeyHandle, _usNodeId, targetPosition, absolute, immediately, &p_rlErrorCode) == 0) + if(VCS_MoveToPosition(_keyHandle, _usNodeId, targetPosition, absolute, immediately, &p_rlErrorCode) == 0) { LogError("VCS_MoveToPosition", lResult, p_rlErrorCode); lResult = MMC_FAILED; @@ -129,11 +144,17 @@ int EPOS::move_to(long targetPosition, bool absolute, bool immediately) int EPOS::set_velocity(long targetVelocity) { int lResult = MMC_SUCCESS; - unsigned int p_rlErrorCode; + unsigned int p_rlErrorCode = 0; + + if((lResult = PrepareDemo(&p_rlErrorCode))!=MMC_SUCCESS) + { + LogError("PrepareDemo", lResult, p_rlErrorCode); + return lResult; + } std::cout << "set profile velocity mode, node = " << _usNodeId << std::endl; - if(VCS_ActivateProfileVelocityMode(_pKeyHandle, _usNodeId, &p_rlErrorCode) == 0) + if(VCS_ActivateProfileVelocityMode(_keyHandle, _usNodeId, &p_rlErrorCode) == 0) { LogError("VCS_ActivateProfileVelocityMode", lResult, p_rlErrorCode); lResult = MMC_FAILED; @@ -142,7 +163,7 @@ int EPOS::set_velocity(long targetVelocity) { std::cout << "Moving with target velocity = " << targetVelocity << std::endl; - if(VCS_MoveWithVelocity(_pKeyHandle, _usNodeId, targetVelocity, &p_rlErrorCode) == 0) + if(VCS_MoveWithVelocity(_keyHandle, _usNodeId, targetVelocity, &p_rlErrorCode) == 0) { lResult = MMC_FAILED; LogError("VCS_MoveWithVelocity", lResult, p_rlErrorCode); @@ -152,16 +173,163 @@ int EPOS::set_velocity(long targetVelocity) return lResult; } +int EPOS::PrintAvailableInterfaces() +{ + int lResult = MMC_FAILED; + int lStartOfSelection = 1; + int lMaxStrSize = 255; + char* pInterfaceNameSel = new char[lMaxStrSize]; + int lEndOfSelection = 0; + unsigned int ulErrorCode = 0; + + do + { + if(!VCS_GetInterfaceNameSelection((char*)_deviceName.c_str(), (char*)_protocolStackName.c_str(), lStartOfSelection, pInterfaceNameSel, lMaxStrSize, &lEndOfSelection, &ulErrorCode)) + { + lResult = MMC_FAILED; + LogError("GetInterfaceNameSelection", lResult, ulErrorCode); + break; + } + else + { + lResult = MMC_SUCCESS; + + printf("interface = %s\n", pInterfaceNameSel); + + PrintAvailablePorts(pInterfaceNameSel); + } + + lStartOfSelection = 0; + } + while(lEndOfSelection == 0); + + for(int i=0; i<65; i++) + { + std::cout << "-"; + } + std::cout << std::endl; + + delete[] pInterfaceNameSel; + + return lResult; +} + +int EPOS::PrintAvailablePorts(char* p_pInterfaceNameSel) +{ + int lResult = MMC_FAILED; + int lStartOfSelection = 1; + int lMaxStrSize = 255; + char* pPortNameSel = new char[lMaxStrSize]; + int lEndOfSelection = 0; + unsigned int ulErrorCode = 0; + + do + { + if(!VCS_GetPortNameSelection((char*)_deviceName.c_str(), (char*)_protocolStackName.c_str(), p_pInterfaceNameSel, lStartOfSelection, pPortNameSel, lMaxStrSize, &lEndOfSelection, &ulErrorCode)) + { + lResult = MMC_FAILED; + LogError("GetPortNameSelection", lResult, ulErrorCode); + break; + } + else + { + lResult = MMC_SUCCESS; + printf(" port = %s\n", pPortNameSel); + } + + lStartOfSelection = 0; + } + while(lEndOfSelection == 0); + + return lResult; +} + int EPOS::stop() { int lResult = MMC_SUCCESS; unsigned int p_rlErrorCode; std::cout << "Halt velocity movement" << std::endl; - if(VCS_HaltVelocityMovement(_pKeyHandle, _usNodeId, &p_rlErrorCode) == 0) + if(VCS_HaltVelocityMovement(_keyHandle, _usNodeId, &p_rlErrorCode) == 0) { lResult = MMC_FAILED; LogError("VCS_HaltVelocityMovement", lResult, p_rlErrorCode); } return lResult; } + +int EPOS::PrepareDemo(unsigned int* errorCode) +{ + int lResult = MMC_SUCCESS; + int oIsFault = 0; + + if(VCS_GetFaultState(_keyHandle, _usNodeId, &oIsFault, errorCode) == 0) + { + LogError("VCS_GetFaultState", lResult, *errorCode); + lResult = MMC_FAILED; + } + + if(lResult==0) + { + if(oIsFault) + { + std::cout << "clear fault, node = '" << _usNodeId << std::endl;; + + if(VCS_ClearFault(_keyHandle, _usNodeId, errorCode) == 0) + { + LogError("VCS_ClearFault", lResult, *errorCode); + lResult = MMC_FAILED; + } + } + + if(lResult==0) + { + int oIsEnabled = 0; + + if(VCS_GetEnableState(_keyHandle, _usNodeId, &oIsEnabled, errorCode) == 0) + { + LogError("VCS_GetEnableState", lResult, *errorCode); + lResult = MMC_FAILED; + } + + if(lResult==0) + { + if(!oIsEnabled) + { + if(VCS_SetEnableState(_keyHandle, _usNodeId, errorCode) == 0) + { + LogError("VCS_SetEnableState", lResult, *errorCode); + lResult = MMC_FAILED; + } + } + } + } + } + return lResult; +} + +void EPOS::GetBaudRate() +{ + char* pDeviceName = new char[255]; + char* pProtocolStackName = new char[255]; + char* pInterfaceName = new char[255]; + char* pPortName = new char[255]; + + strcpy(pDeviceName, _deviceName.c_str()); + strcpy(pProtocolStackName, _protocolStackName.c_str()); + strcpy(pInterfaceName, _interfaceName.c_str()); + strcpy(pPortName, _portName.c_str()); + + unsigned int baudrateSel; + int endOfSelection = false; + unsigned int errorCode = 0; + + if(VCS_GetBaudrateSelection(pDeviceName, pProtocolStackName, pInterfaceName, pPortName, true, &baudrateSel, &endOfSelection, &errorCode)) + { + while(!endOfSelection) + { + VCS_GetBaudrateSelection(pDeviceName, pProtocolStackName, pInterfaceName, pPortName, false, &baudrateSel, &endOfSelection, &errorCode); + std::cout << baudrateSel << std::endl; + } + } +} \ No newline at end of file diff --git a/src/components/internal/actuators/epos/epos.h b/src/components/internal/actuators/epos/epos.h index 9f456d5..1a6a32e 100644 --- a/src/components/internal/actuators/epos/epos.h +++ b/src/components/internal/actuators/epos/epos.h @@ -14,28 +14,31 @@ class EPOS : public MenuUser { public: - EPOS(); + EPOS(std::string name, std::string portName, unsigned short nodeId); virtual ~EPOS(); void LogError(std::string functionName, int p_lResult, unsigned int p_ulErrorCode); - int OpenDevice(); - int CloseDevice(unsigned int* p_pErrorCode); - int move_to(long targetPosition, bool absolute=false, bool immediately=true); - int set_velocity(long targetVelocity); - int stop(); + int OpenDevice(); + int CloseDevice(); + int move_to(long targetPosition, bool absolute=false, bool immediately=true); + int set_velocity(long targetVelocity); + int PrintAvailableInterfaces(); + int PrintAvailablePorts(char* p_pInterfaceNameSel); + int stop(); + int PrepareDemo(unsigned int* eErrorCode); + void GetBaudRate(); protected: virtual void on_exit(){}; private: -// std::string _name; - void* _pKeyHandle = 0; + std::string _name; + void* _keyHandle = 0; unsigned short _usNodeId = 1; std::string _deviceName; std::string _protocolStackName; std::string _interfaceName; std::string _portName; unsigned int _baudrate = 0; - const std::string _programName = "LinuxEposCmd"; }; #endif // EPOS_H diff --git a/src/components/internal/actuators/epos/libEposCmd.so.6.6.2.0 b/src/components/internal/actuators/epos/libEposCmd.so.6.6.2.0 deleted file mode 100644 index b52f95f..0000000 Binary files a/src/components/internal/actuators/epos/libEposCmd.so.6.6.2.0 and /dev/null differ diff --git a/src/components/internal/actuators/epos/libftd2xx.so.1.4.8 b/src/components/internal/actuators/epos/libftd2xx.so.1.4.8 deleted file mode 100644 index 4e9703d..0000000 Binary files a/src/components/internal/actuators/epos/libftd2xx.so.1.4.8 and /dev/null differ diff --git a/src/components/internal/actuators/roboclaw/CMakeLists.txt b/src/components/internal/actuators/roboclaw/CMakeLists.txt deleted file mode 100644 index 23cea8e..0000000 --- a/src/components/internal/actuators/roboclaw/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -project(roboclaw CXX) - -set(SOURCES - # cmake-format: sortable - answer.cpp - factory.cpp - message.cpp - roboclaw.cpp) - -add_library(roboclaw ${SOURCES}) diff --git a/src/components/internal/actuators/roboclaw/factory.h b/src/components/internal/actuators/roboclaw/factory.h index f50301b..cbe81fa 100644 --- a/src/components/internal/actuators/roboclaw/factory.h +++ b/src/components/internal/actuators/roboclaw/factory.h @@ -3,7 +3,7 @@ #include #include - +#include #include "utils/serial_port.h" namespace RC { diff --git a/src/components/internal/adc/CMakeLists.txt b/src/components/internal/adc/CMakeLists.txt deleted file mode 100644 index 47ef3ff..0000000 --- a/src/components/internal/adc/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -project(adc CXX) - -set(SOURCES - # cmake-format: sortable - adafruit_ads1115.cpp) - -add_library(adc ${SOURCES}) - -target_link_libraries(adc i2c) diff --git a/src/components/internal/dac/CMakeLists.txt b/src/components/internal/dac/CMakeLists.txt deleted file mode 100644 index 15082bf..0000000 --- a/src/components/internal/dac/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -project(dac CXX) - -set(SOURCES - # cmake-format: sortable - mcp4728.cpp) - -add_library(dac ${SOURCES}) diff --git a/src/components/internal/gpio/CMakeLists.txt b/src/components/internal/gpio/CMakeLists.txt deleted file mode 100644 index 1ed552e..0000000 --- a/src/components/internal/gpio/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -project(gpio CXX) - -set(SOURCES - # cmake-format: sortable - gpio.cpp) - -add_library(gpio ${SOURCES}) diff --git a/src/components/internal/gpio/gpio.cpp b/src/components/internal/gpio/gpio.cpp index 21311ab..f9b2772 100644 --- a/src/components/internal/gpio/gpio.cpp +++ b/src/components/internal/gpio/gpio.cpp @@ -1,5 +1,5 @@ #include "gpio.h" -#include "wiringPi.h" +#include GPIO::GPIO(int pin, Direction dir, Pull pull) : _pin(pin) @@ -8,22 +8,22 @@ GPIO::GPIO(int pin, Direction dir, Pull pull) { switch (dir) { case DIR_INPUT: - pinMode(_pin, INPUT); + bcm2835_gpio_fsel(_pin, BCM2835_GPIO_FSEL_INPT); break; case DIR_OUTPUT: - pinMode(_pin, OUTPUT); + bcm2835_gpio_fsel(_pin, BCM2835_GPIO_FSEL_OUTP); break; } switch (pull) { case PULL_UP: - pullUpDnControl(_pin, PUD_UP); + bcm2835_gpio_set_pud(_pin, BCM2835_GPIO_PUD_UP); break; case PULL_DOWN: - pullUpDnControl(_pin, PUD_DOWN); + bcm2835_gpio_set_pud(_pin, BCM2835_GPIO_PUD_DOWN); break; case PULL_NONE: - pullUpDnControl(_pin, PUD_OFF); + bcm2835_gpio_set_pud(_pin, BCM2835_GPIO_PUD_OFF); break; } } @@ -31,7 +31,7 @@ GPIO::GPIO(int pin, Direction dir, Pull pull) GPIO& GPIO::operator=(int v) { if (_dir == DIR_OUTPUT) { - digitalWrite(_pin, v); + bcm2835_gpio_write(_pin, v); } return *this; } @@ -39,7 +39,7 @@ GPIO& GPIO::operator=(int v) GPIO& GPIO::operator=(bool v) { if (_dir == DIR_OUTPUT) { - digitalWrite(_pin, v ? 1 : 0); + bcm2835_gpio_write(_pin, v ? HIGH : LOW); } return *this; } @@ -47,7 +47,7 @@ GPIO& GPIO::operator=(bool v) GPIO::operator int() { if (_dir == DIR_INPUT) { - return digitalRead(_pin); + return bcm2835_gpio_lev(_pin); } else { return 0; } @@ -56,7 +56,7 @@ GPIO::operator int() GPIO::operator bool() { if (_dir == DIR_INPUT) { - return digitalRead(_pin) == HIGH; + return bcm2835_gpio_lev(_pin) == HIGH; } else { return 0; } diff --git a/src/components/internal/hand/CMakeLists.txt b/src/components/internal/hand/CMakeLists.txt deleted file mode 100644 index 0cb6dee..0000000 --- a/src/components/internal/hand/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -project(hand CXX) - -set(SOURCES - # cmake-format: sortable - touch_bionics_hand.cpp - quantum_hand.cpp) - -add_library(hand ${SOURCES}) - -target_link_libraries(hand dac) - diff --git a/src/control/CMakeLists.txt b/src/control/CMakeLists.txt deleted file mode 100644 index 2d41642..0000000 --- a/src/control/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -project(control CXX) - -set(SOURCES - # cmake-format: sortable - algo/lawimu_wrist.cpp - algo/lawjacobian.cpp - algo/lawopti.cpp - algo/lawimu_we.cpp - algo/myocontrol.cpp - compensation_imu.cpp - compensation_optitrack.cpp - cybathlon.cpp - demo.cpp - demo_imu.cpp - jf_opti.cpp - jfoptiorientation.cpp - jfimu_sk.cpp - jfimu_v3.cpp - jfimu_v4.cpp - matlab_receiver.cpp - read_adc.cpp - remote_computer_control.cpp - recorddata.cpp - test_imu.cpp - voluntary_control.cpp - pushbuttons.cpp - myo_2electrodes.cpp - controle_bretelles.cpp) - -add_library(control ${SOURCES}) - -target_link_libraries(control stdc++fs) diff --git a/src/control/algo/myocontrol.cpp b/src/control/algo/myocontrol.cpp index a39dcb2..def53cb 100644 --- a/src/control/algo/myocontrol.cpp +++ b/src/control/algo/myocontrol.cpp @@ -1,4 +1,5 @@ #include "myocontrol.h" +#include namespace MyoControl { diff --git a/src/control/compensation_imu.cpp b/src/control/compensation_imu.cpp index 084ff13..462eb86 100644 --- a/src/control/compensation_imu.cpp +++ b/src/control/compensation_imu.cpp @@ -19,7 +19,7 @@ CompensationIMU::CompensationIMU(std::shared_ptr robot) , _lfa("_lfa(cm)", BaseParam::ReadWrite, this, 35) , _lsh("_lsh(cm)", BaseParam::ReadWrite, this, -20) { - if (!check_ptr(_robot->sensors.yellow_imu)) { + if (!check_ptr(_robot->sensors.yellow_imu, _robot->joints.elbow_flexion, _robot->joints.wrist_pronation, _robot->joints.hand)) { throw std::runtime_error("Compensation IMU Control is missing components"); } diff --git a/src/control/controle_bretelles.cpp b/src/control/controle_bretelles.cpp index be3a29e..f711b32 100644 --- a/src/control/controle_bretelles.cpp +++ b/src/control/controle_bretelles.cpp @@ -3,7 +3,6 @@ #include "utils/log/log.h" #include "algo/myocontrol.h" #include "ui/visual/ledstrip.h" -#include "wiringPi.h" #include #include diff --git a/src/control/demo_imu.cpp b/src/control/demo_imu.cpp index 7e2cbbe..40dad7b 100644 --- a/src/control/demo_imu.cpp +++ b/src/control/demo_imu.cpp @@ -1,7 +1,6 @@ #include "demo_imu.h" #include "utils/check_ptr.h" #include "utils/log/log.h" -#include "wiringPi.h" #include DemoIMU::DemoIMU(std::shared_ptr robot) @@ -9,11 +8,11 @@ DemoIMU::DemoIMU(std::shared_ptr robot) , _robot(robot) , _lambdaW(5) , _thresholdW(4. * M_PI / 180.) - , _pin_up(23) - , _pin_down(22) - , _pin_mode1(25) - , _pin_mode2(26) - , _pin_status(24) + , _pin_up(23,GPIO::DIR_INPUT,GPIO::PULL_UP) + , _pin_down(22,GPIO::DIR_INPUT,GPIO::PULL_UP) + , _pin_mode1(25,GPIO::DIR_INPUT,GPIO::PULL_UP) + , _pin_mode2(26,GPIO::DIR_INPUT,GPIO::PULL_UP) + , _pin_status(24,GPIO::DIR_INPUT,GPIO::PULL_UP) { if (!check_ptr(_robot->joints.wrist_pronation, _robot->sensors.white_imu)) { throw std::runtime_error("Compensation IMU Control is missing components"); @@ -26,12 +25,6 @@ DemoIMU::DemoIMU(std::shared_ptr robot) _menu->add_item(_robot->joints.wrist_pronation->menu()); if (_robot->joints.hand) _menu->add_item(_robot->joints.hand->menu()); - - pullUpDnControl(_pin_up, PUD_UP); - pullUpDnControl(_pin_down, PUD_UP); - pullUpDnControl(_pin_mode1, PUD_UP); - pullUpDnControl(_pin_mode2, PUD_UP); - pullUpDnControl(_pin_status, PUD_UP); } DemoIMU::~DemoIMU() @@ -52,11 +45,11 @@ void DemoIMU::tare_IMU() void DemoIMU::displayPin() { - int pin_down_value = digitalRead(_pin_down); - int pin_up_value = digitalRead(_pin_up); - int pin_mode_value1 = digitalRead(_pin_mode1); - int pin_mode_value2 = digitalRead(_pin_mode2); - int pin_status_value = digitalRead(_pin_status); + int pin_down_value = _pin_down; + int pin_up_value = _pin_up; + int pin_mode_value1 = _pin_mode1; + int pin_mode_value2 = _pin_mode2; + int pin_status_value = _pin_status; debug() << "PinUp: " << pin_up_value; debug() << "PinDown: " << pin_down_value; debug() << "PinMode1: " << pin_mode_value1; @@ -79,7 +72,7 @@ void DemoIMU::loop(double dt, clock::time_point time) double timeWithDelta = (time - _start_time).count(); // Read pin for status static int prev_pin_status_value = 0; - int pin_status_value = digitalRead(_pin_status); + int pin_status_value = _pin_status; if (_start == false) { action_loop_pb(); // do nothing by default @@ -108,7 +101,7 @@ void DemoIMU::action_loop_imu() int init_cnt = 10; // Read pin for mode - int pin_mode_value2 = digitalRead(_pin_mode2); + int pin_mode_value2 = _pin_mode2; if (pin_mode_value2 == 1) { /// ERGONOMIC MODE @@ -145,13 +138,13 @@ void DemoIMU::action_loop_imu() void DemoIMU::action_loop_pb() { // Read pin for mode - int pin_mode_value1 = digitalRead(_pin_mode1); + int pin_mode_value1 = _pin_mode1; if (pin_mode_value1 == 1) { /// OPEN-LOOP WITH PUSH BUTTONS static int prev_pin_up_value = 0, prev_pin_down_value = 0; - int pin_down_value = digitalRead(_pin_down); - int pin_up_value = digitalRead(_pin_up); + int pin_down_value = _pin_down; + int pin_up_value = _pin_up; /// WRIST if (pin_down_value == 1 && prev_pin_down_value == 0) { diff --git a/src/control/demo_imu.h b/src/control/demo_imu.h index 10539cf..592cf5e 100644 --- a/src/control/demo_imu.h +++ b/src/control/demo_imu.h @@ -6,6 +6,7 @@ #include "utils/socket.h" #include "utils/threaded_loop.h" #include +#include class DemoIMU : public ThreadedLoop { public: @@ -30,11 +31,11 @@ class DemoIMU : public ThreadedLoop { int _cnt; double _lambdaW; double _thresholdW; - int _pin_up; // pin buttons for open-loop - int _pin_down; // pin buttons for open-loop - int _pin_mode1; // 0 = open-loop with buttons; 1 = ergonomic with IMU - int _pin_mode2; // 0 = open-loop with buttons; 1 = ergonomic with IMU - int _pin_status; // 0 -> change status: start or stop + GPIO _pin_up; // pin buttons for open-loop + GPIO _pin_down; // pin buttons for open-loop + GPIO _pin_mode1; // 0 = open-loop with buttons; 1 = ergonomic with IMU + GPIO _pin_mode2; // 0 = open-loop with buttons; 1 = ergonomic with IMU + GPIO _pin_status; // 0 -> change status: start or stop bool _start; // indicates weither the loop is working }; diff --git a/src/control/jfimu_sk.cpp b/src/control/jfimu_sk.cpp index 467dc50..28df21d 100644 --- a/src/control/jfimu_sk.cpp +++ b/src/control/jfimu_sk.cpp @@ -2,7 +2,6 @@ #include "algo/myocontrol.h" #include "utils/check_ptr.h" #include "utils/log/log.h" -#include "wiringPi.h" #include #include @@ -15,8 +14,8 @@ JacobianFormulationIMU_sk::JacobianFormulationIMU_sk(std::string name, std::stri , _k("k", BaseParam::ReadWrite, this, 5) , _lt("lt(cm)", BaseParam::ReadWrite, this, 30) , _lsh("lsh(cm)", BaseParam::ReadWrite, this, 20) - , _pin_up(24) - , _pin_down(22) + , _pin_up(24,GPIO::DIR_INPUT,GPIO::PULL_UP) + , _pin_down(22,GPIO::DIR_INPUT,GPIO::PULL_UP) , _lua("lua(cm)", BaseParam::ReadWrite, this, 30) , _lfa("lfa(cm)", BaseParam::ReadWrite, this, 28) , _lwrist("lwrist(cm)", BaseParam::ReadWrite, this, 10) @@ -52,9 +51,6 @@ JacobianFormulationIMU_sk::JacobianFormulationIMU_sk(std::string name, std::stri debug() << "nDOF: " << _nbDOF; } - pullUpDnControl(_pin_up, PUD_UP); - pullUpDnControl(_pin_down, PUD_UP); - _filename = filename; // threshold EMG @@ -138,8 +134,8 @@ void JacobianFormulationIMU_sk::calibrations() void JacobianFormulationIMU_sk::displayPin() { - int pin_down_value = digitalRead(_pin_down); - int pin_up_value = digitalRead(_pin_up); + int pin_down_value = _pin_down; + int pin_up_value = _pin_up; debug() << "PinUp: " << pin_up_value; debug() << "PinDown: " << pin_down_value; } @@ -413,8 +409,8 @@ void JacobianFormulationIMU_sk::loop(double, clock::time_point time) } /// PIN PUSH-BUTTONS CONTROL - int pin_down_value = digitalRead(_pin_down); - int pin_up_value = digitalRead(_pin_up); + int pin_down_value = _pin_down; + int pin_up_value = _pin_up; /// CONTROL LOOP if (_cnt == 0) { @@ -532,7 +528,7 @@ void JacobianFormulationIMU_sk::listenArduino() auto data = _receiverArduino.receive(); std::string buf; std::transform(data.begin(), data.end(), buf.begin(), [](std::byte b) { return static_cast(b); }); - _pinArduino = std::stoi(buf); - debug() << "Pin Arduino: " << _pinArduino; + // _pinArduino = std::stoi(buf); + // debug() << "Pin Arduino: " << _pinArduino; } } diff --git a/src/control/jfimu_sk.h b/src/control/jfimu_sk.h index e3165ff..8c762b5 100644 --- a/src/control/jfimu_sk.h +++ b/src/control/jfimu_sk.h @@ -7,6 +7,7 @@ #include "utils/socket.h" #include "utils/threaded_loop.h" #include +#include class JacobianFormulationIMU_sk : public ThreadedLoop { public: @@ -54,14 +55,14 @@ class JacobianFormulationIMU_sk : public ThreadedLoop { int _init_cnt = 10; int _cnt; double _lambda[nbLinks]; - int _pin_up; - int _pin_down; + GPIO _pin_up; + GPIO _pin_down; double _theta[nbLinks]; double _threshold[nbLinks]; int _l[nbLinks]; Socket _receiverArduino; - int _pinArduino; + // GPIO _pinArduino; unsigned int _infoSent; uint16_t _emg[2]; diff --git a/src/control/jfimu_v1.cpp b/src/control/jfimu_v1.cpp index c7c675e..020667e 100644 --- a/src/control/jfimu_v1.cpp +++ b/src/control/jfimu_v1.cpp @@ -1,7 +1,6 @@ #include "jfimu_v1.h" #include "utils/check_ptr.h" #include "utils/log/log.h" -#include "wiringPi.h" #include #include diff --git a/src/control/jfimu_v3.cpp b/src/control/jfimu_v3.cpp index 1351507..681950f 100644 --- a/src/control/jfimu_v3.cpp +++ b/src/control/jfimu_v3.cpp @@ -1,7 +1,6 @@ #include "jfimu_v3.h" #include "utils/check_ptr.h" #include "utils/log/log.h" -#include "wiringPi.h" #include #include diff --git a/src/control/jfimu_v4.cpp b/src/control/jfimu_v4.cpp index 527b3d6..438e410 100644 --- a/src/control/jfimu_v4.cpp +++ b/src/control/jfimu_v4.cpp @@ -1,7 +1,6 @@ #include "jfimu_v4.h" #include "utils/check_ptr.h" #include "utils/log/log.h" -#include "wiringPi.h" #include #include diff --git a/src/control/myo_2electrodes.cpp b/src/control/myo_2electrodes.cpp index 34aba0b..2e13d47 100644 --- a/src/control/myo_2electrodes.cpp +++ b/src/control/myo_2electrodes.cpp @@ -5,8 +5,6 @@ #include "utils/log/log.h" #include -#include "wiringPi.h" - // indicate if optitrack is on #define OPTITRACK 1 diff --git a/src/control/pushbuttons.cpp b/src/control/pushbuttons.cpp index 9805a50..2d42579 100644 --- a/src/control/pushbuttons.cpp +++ b/src/control/pushbuttons.cpp @@ -5,8 +5,6 @@ #include "utils/log/log.h" #include -#include "wiringPi.h" - // indicate if optitrack is on #define OPTITRACK 1 diff --git a/src/control/read_adc.cpp b/src/control/read_adc.cpp index 69b6028..40931de 100644 --- a/src/control/read_adc.cpp +++ b/src/control/read_adc.cpp @@ -13,10 +13,10 @@ ReadADC::ReadADC(std::shared_ptr robot) : ThreadedLoop("Read ADC", .025) , _robot(robot) { - if (!check_ptr(_robot->joints.elbow_flexion, _robot->joints.wrist_pronation, _robot->joints.hand_quantum)) { - throw std::runtime_error("Demo is missing components"); - } - +// if (!check_ptr(_robot->joints.elbow_flexion, _robot->joints.wrist_pronation, _robot->joints.hand_quantum)) { + // throw std::runtime_error("Demo is missing components"); + // } + _menu->set_description("Read ADC"); _menu->set_code("adc"); diff --git a/src/control/recorddata.cpp b/src/control/recorddata.cpp index 85ba82a..4ede153 100644 --- a/src/control/recorddata.cpp +++ b/src/control/recorddata.cpp @@ -1,7 +1,6 @@ #include "recorddata.h" #include "utils/check_ptr.h" #include "utils/log/log.h" -#include "wiringPi.h" #include #include diff --git a/src/control/test_imu.cpp b/src/control/test_imu.cpp index db48a7c..77c0c14 100644 --- a/src/control/test_imu.cpp +++ b/src/control/test_imu.cpp @@ -5,21 +5,21 @@ #include "ui/visual/ledstrip.h" #include "utils/log/log.h" #include +#include "components/internal/actuators/roboclaw/factory.h" TestIMU::TestIMU(std::shared_ptr robot) - : ThreadedLoop("Test IMU", 0.025) + : ThreadedLoop("Test RS232", 0.1) , _robot(robot) { - if (!check_ptr(_robot->sensors.white_imu, _robot->sensors.ng_imu, _robot->joints.elbow_flexion, _robot->joints.wrist_pronation, _robot->joints.hand_quantum)) { - throw std::runtime_error("TestIMU is missing components"); + _menu->set_description("Test RS232"); + _menu->set_code("test"); /* + try { + _serial_port = RC::Factory::get("/dev/ttyAMA0", B115200); + critical() << "Create test RS232"; + } catch (std::exception& e) { + critical() << "Couldn't create test RS232 : (" << e.what() << ")"; } - _menu->set_description("Test IMU"); - _menu->set_code("testimu"); - _menu->add_item("tare", "Tare IMUs", [this](std::string) { this->tare_IMU(); }); - _menu->add_item(_robot->joints.elbow_flexion->menu()); - _menu->add_item(_robot->joints.wrist_pronation->menu()); - _menu->add_item(_robot->joints.hand_quantum->menu()); - +*/ } TestIMU::~TestIMU() @@ -27,141 +27,35 @@ TestIMU::~TestIMU() stop_and_join(); } -void TestIMU::tare_IMU() -{ - if (_robot->sensors.white_imu) - _robot->sensors.white_imu->send_command_algorithm_init_then_tare(); - if (_robot->sensors.ng_imu) - _robot->sensors.ng_imu->send_command_algorithm_init(); - - debug("Wait ..."); - - usleep(6 * 1000000); - _robot->user_feedback.buzzer->makeNoise(Buzzer::TRIPLE_BUZZ); -} - bool TestIMU::setup() { - if (_robot->joints.elbow_flexion->is_calibrated() == false) - _robot->joints.elbow_flexion->calibrate(); - - _robot->joints.wrist_pronation->set_encoder_position(0); - - _robot->user_feedback.leds->set(LedStrip::white, 11); - - // Record IMU data in txt file - std::string filename("comparaison_IMU"); - std::string suffix; - int cnt = 0; - std::string extension(".txt"); - do { - ++cnt; - suffix = "_" + std::to_string(cnt); - } while (std::filesystem::exists(filename + suffix + extension)); - _file = std::ofstream(filename + suffix + extension); - if (!_file.good()) { - critical() << "Failed to open" << (filename + suffix + extension); - return false; - } else { - /// WRITE FILE HEADERS - _file << "quatximu.w \t quatximu.x \t quatximu.y \t quatximu.z \t"; - _file << "gyroximu.x \t gyroximu.y \t gyroximu.z \t"; - _file << "accximu.x \t accximu.y \t accximu.z \t"; - _file << "quatngimu.w \t quatngimu.x \t quatngimu.y \t quatngimu.z \t"; - _file << "gyrongimu.x \t gyrongimu.y \t gyrongimu.z \t"; - _file << "accngimu.x \t accngimu.y \t accngimu.z \t"; - _file << std::endl; - } - - _start_time = clock::now(); + _serial_port.open("/dev/ttyAMA0",B115200); + std::cout << "bonjour" << std::endl; return true; } void TestIMU::loop(double dt, clock::time_point time) { - //Myo demo control - static std::unique_ptr myocontrol; - static const unsigned int counts_after_mode_change = 10; - static const unsigned int counts_cocontraction = 5; - static const unsigned int counts_before_bubble = 5; - static const unsigned int counts_after_bubble = 5; - static const MyoControl::EMGThresholds thresholds(15, 8, 15, 15, 8, 15); - auto robot = _robot; - MyoControl::Action elbow( - "Elbow", [robot]() { robot->joints.elbow_flexion->set_velocity_safe(-25); }, [robot]() { robot->joints.elbow_flexion->set_velocity_safe(25); }, [robot]() { robot->joints.elbow_flexion->set_velocity_safe(0); }); - MyoControl::Action wrist_pronosup( - "Wrist rotation", [robot]() { robot->joints.wrist_pronation->set_velocity_safe(40); }, [robot]() { robot->joints.wrist_pronation->set_velocity_safe(-40); }, [robot]() { robot->joints.wrist_pronation->set_velocity_safe(0); }); - MyoControl::Action hand( - "Hand", [robot]() { robot->joints.hand_quantum->makeContraction(QuantumHand::SHORT_CONTRACTION,1,4); }, [robot]() { robot->joints.hand_quantum->makeContraction(QuantumHand::SHORT_CONTRACTION,2,4); }, [robot]() { robot->joints.wrist_pronation->set_velocity_safe(0); }); - static LedStrip::color current_color = LedStrip::none; - std::vector s1 { hand, wrist_pronosup, elbow }; - - int emg[2]; - static bool first = true; - if (first) { - current_color = LedStrip::green; - myocontrol = std::make_unique(s1, thresholds, counts_after_mode_change, counts_cocontraction, counts_before_bubble, counts_after_bubble); - first = false; - } - emg[0] = 0; - emg[1] = 0; - if (!_robot->btn1) { - emg[0] = 80; - } - if (!_robot->btn2) { - emg[1] = 80; - } - myocontrol->process(emg[0], emg[1]); - - std::vector colors(11, current_color); - switch (myocontrol->current_index()) { - case 0: - colors[4] = LedStrip::color(80, 30, 0, 1); - break; - case 1: - colors[4] = LedStrip::color(0, 50, 50, 1); - break; - case 2: - colors[4] = LedStrip::color(50, 0, 50, 1); - break; - case 3: - colors[4] = LedStrip::white; - break; - default: - break; - } - _robot->user_feedback.leds->set(colors); - + std::cout << "test" << std::endl; + send(); +} - //Get IMU data - double quatx[4], quatng[4], gyrox[3], accx[3], magx[3], sensorsng[10]; - _robot->sensors.white_imu->get_quat(quatx); - _robot->sensors.white_imu->get_cal(gyrox, accx, magx); - _robot->sensors.ng_imu->get_quat(quatng); - _robot->sensors.ng_imu->get_sensors(sensorsng); +void TestIMU::cleanup() +{ + std::cout << "au revoir" << std::endl; +} - //Publish IMU values with MQTT - static bool publish = 1; - publish = !publish; - if (publish) { - for (uint16_t i = 0; i < 3; i++) { - _mqtt.publish("sam/emg/time/" + std::to_string(i), std::to_string(quatx[i])); - } - for (uint16_t i = 0; i < 3; i++) { - _mqtt.publish("sam/emg/time/" + std::to_string(i+3), std::to_string(quatng[i])); - } +void TestIMU::send() +{ + //_serial_port->take_ownership(); + auto data = _serial_port.read_all(); + for(auto c:data) { + std::cout << (char)c << " (" << (int)c << ") "; } + std::cout << std::endl; + _serial_port.write("testjjhgkjB"); - _file << quatx[0] << "\t" << quatx[1] << "\t" << quatx[2] << "\t" << quatx[3] << "\t" << gyrox[0] << "\t" << gyrox[1] << "\t" << gyrox[2] << "\t" << accx[0] << "\t" << accx[1] << "\t" << accx[2] << "\t"; - _file << quatng[0] << "\t" << quatng[1] << "\t" << quatng[2] << "\t" << quatng[3] << "\t" << sensorsng[0] << "\t" << sensorsng[1] << "\t" << sensorsng[2] << "\t" << sensorsng[3] << "\t" << sensorsng[4] << "\t" << sensorsng[5] ; - _file << std::endl; -} + //_serial_port->release_ownership(); -void TestIMU::cleanup() -{ - _file.close(); - _robot->joints.wrist_pronation->set_velocity_safe(0); - _robot->joints.elbow_flexion->set_velocity_safe(0); - _robot->user_feedback.leds->set(LedStrip::white, 11); } diff --git a/src/control/test_imu.h b/src/control/test_imu.h index 64c92ff..fadead4 100644 --- a/src/control/test_imu.h +++ b/src/control/test_imu.h @@ -1,12 +1,12 @@ #ifndef TESTIMU_H #define TESTIMU_H -#include "algo/lawjacobian.h" #include "sam/sam.h" #include "utils/socket.h" #include "utils/interfaces/mqtt_user.h" #include "utils/threaded_loop.h" #include +#include "utils/serial_port.h" class TestIMU : public ThreadedLoop, public MqttUser { public: @@ -14,17 +14,15 @@ class TestIMU : public ThreadedLoop, public MqttUser { ~TestIMU() override; private: - void tare_IMU(); bool setup() override; void loop(double dt, clock::time_point time) override; void cleanup() override; std::shared_ptr _robot; - Socket _receiver; - std::ofstream _file; - int _cnt; - clock::time_point _start_time; + void send(); + + SerialPort _serial_port; }; #endif // TESTIMU_H diff --git a/src/sam/sam.cpp b/src/sam/sam.cpp index e178c1b..c93900b 100644 --- a/src/sam/sam.cpp +++ b/src/sam/sam.cpp @@ -15,12 +15,15 @@ Sensors::Sensors() int cnt = 0; do { + //auto ng_imu = Components::make_component("ng_imu", "/dev/ngimu"+std::to_string(cnt), B115200); ng_imu = Components::make_component("ng_imu", "/dev/ngimu"+std::to_string(cnt), B115200); if (ng_imu) { while(!ng_imu->is_serialnumber_available()) ng_imu->send_command_serial_number(); - if (ng_imu->get_serialnumber()=="0035F6E2") + if (ng_imu->get_serialnumber()=="0035F6E2") { + //red_ngimu = std::move(ng_imu); info() << "red_ngimu"; + } } ++cnt; } while (!ng_imu && cnt<5); @@ -39,7 +42,7 @@ Sensors::Sensors() UserFeedback::UserFeedback() { - buzzer = std::make_unique(29); + buzzer = std::make_unique(21); buzzer->set_prio(90); buzzer->set_preferred_cpu(2); @@ -48,7 +51,6 @@ UserFeedback::UserFeedback() Joints::Joints() { - shoulder_medial_rotation = Components::make_component("shoulder_medial_rotation"); elbow_flexion = Components::make_component("elbow_v2"); wrist_pronation = Components::make_component("wrist_pronation_v2"); @@ -72,16 +74,17 @@ Joints::Joints() } } - // test_epos = Components::make_component("test_epos"); + //epos1 = Components::make_component("epos1", "epos1", "/dev/ttyAMA0", 1); + //epos2 = Components::make_component("epos2", "epos2", "USB1", 1); } Components::Components() - : mosfet_gpio(4, GPIO::DIR_OUTPUT, GPIO::PULL_DOWN) - , demo_gpio(28, GPIO::DIR_INPUT, GPIO::PULL_UP) - , adc_gpio(27, GPIO::DIR_INPUT, GPIO::PULL_UP) - , btn1(24, GPIO::DIR_INPUT, GPIO::PULL_UP) - , btn2(22, GPIO::DIR_INPUT, GPIO::PULL_UP) - , btn3(25, GPIO::DIR_INPUT, GPIO::PULL_UP) + : mosfet_gpio(23, GPIO::DIR_OUTPUT, GPIO::PULL_DOWN) + , demo_gpio(20, GPIO::DIR_INPUT, GPIO::PULL_UP) + , adc_gpio(16, GPIO::DIR_INPUT, GPIO::PULL_UP) + , btn1(19, GPIO::DIR_INPUT, GPIO::PULL_UP) + , btn2(6, GPIO::DIR_INPUT, GPIO::PULL_UP) + , btn3(26, GPIO::DIR_INPUT, GPIO::PULL_UP) { } } diff --git a/src/sam/sam.h b/src/sam/sam.h index 2b0d881..5bbf7b9 100644 --- a/src/sam/sam.h +++ b/src/sam/sam.h @@ -82,7 +82,8 @@ class Joints { std::unique_ptr hand; std::unique_ptr hand_quantum; - std::unique_ptr test_epos; + std::unique_ptr epos1; + std::unique_ptr epos2; }; class Components { diff --git a/src/sam/samanager.cpp b/src/sam/samanager.cpp index 0bad96d..d020bb5 100644 --- a/src/sam/samanager.cpp +++ b/src/sam/samanager.cpp @@ -1,12 +1,12 @@ #include "samanager.h" #include "utils/log/log.h" #include -#include +#include SAManager::SAManager() : _main_menu(std::make_unique("main", "Main menu", [this] { _cv.notify_one(); })) { - wiringPiSetup(); + bcm2835_init(); if (isatty(fileno(stdin))) { _menu_console_binding = std::make_unique(); @@ -24,6 +24,10 @@ SAManager::~SAManager() _menu_mqtt_binding->show_message("Exited gracefully."); if (_robot->user_feedback.leds) _robot->user_feedback.leds->set(LedStrip::none, 11); + if (_robot->joints.epos1) + _robot->joints.epos1->CloseDevice(); + if (_robot->joints.epos2) + _robot->joints.epos2->CloseDevice(); _robot->mosfet_gpio = false; // if (_robot->joints.elbow_flexion) { // _robot->joints.elbow_flexion->move_to(0, 20); @@ -97,6 +101,8 @@ void SAManager::fill_menus() // _main_menu->add_submenu_from_user(_jfIMU4); // _main_menu->add_submenu_from_user(_recordData); _main_menu->add_submenu_from_user(_bretelles); + _main_menu->add_submenu_from_user(_robot->joints.epos1); + _main_menu->add_submenu_from_user(_robot->joints.epos2); _main_menu->activate(); } diff --git a/src/ui/CMakeLists.txt b/src/ui/CMakeLists.txt deleted file mode 100644 index be2af53..0000000 --- a/src/ui/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -project(ui CXX) - -set(SOURCES - # cmake-format: sortable - menu/menu_console.cpp - menu/menu_frontend.cpp - menu/menu_mqtt.cpp - sound/buzzer.cpp - visual/ledstrip.cpp) - -add_library(ui ${SOURCES}) - -target_link_libraries(ui espeak) diff --git a/src/ui/visual/ledstrip.cpp b/src/ui/visual/ledstrip.cpp index 0caa165..cad450a 100644 --- a/src/ui/visual/ledstrip.cpp +++ b/src/ui/visual/ledstrip.cpp @@ -1,6 +1,6 @@ #include "ledstrip.h" #include "utils/log/log.h" -#include "wiringPiSPI.h" +#include static const uint8_t led_value = 50; @@ -14,13 +14,16 @@ LedStrip::color LedStrip::none(0, 0, 0, 0); LedStrip::LedStrip() { - if (wiringPiSPISetup(0, 500000) < 0) { - critical() << "wiringPiSPISetup failed"; + if (bcm2835_spi_begin() < 0) { + critical() << "bcm2835_spi_begin failed"; } + bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_512); + bcm2835_spi_chipSelect(BCM2835_SPI_CS0); } LedStrip::~LedStrip() { + bcm2835_spi_end(); } void LedStrip::set(std::vector colors) @@ -43,23 +46,23 @@ void LedStrip::set(color c, unsigned int n) void LedStrip::send_opening_bytes() { - uint8_t buf[4] = { 0x00, 0x00, 0x00, 0x00 }; - wiringPiSPIDataRW(0, reinterpret_cast(&buf), 4); + char buf[4] = { 0x00, 0x00, 0x00, 0x00 }; + bcm2835_spi_writenb(reinterpret_cast(&buf), 4); } void LedStrip::send_closing_bytes() { - uint8_t buf[4] = { 0xff, 0xff, 0xff, 0xff }; - wiringPiSPIDataRW(0, reinterpret_cast(&buf), 4); + char buf[4] = { 0xff, 0xff, 0xff, 0xff }; + bcm2835_spi_writenb(reinterpret_cast(&buf), 4); } void LedStrip::send_color_bytes(color c) { - uint8_t led_frame[4]; + char led_frame[4]; led_frame[0] = 0b11100000 | (0b00011111 & c.brightness); led_frame[1] = c.b; led_frame[2] = c.g; led_frame[3] = c.r; - wiringPiSPIDataRW(0, led_frame, 4); + bcm2835_spi_writenb(reinterpret_cast(&led_frame), 4); } diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt deleted file mode 100644 index a56f778..0000000 --- a/src/utils/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -project(utils CXX) - -set(SOURCES - # cmake-format: sortable - interfaces/menu_user.cpp - interfaces/mqtt_user.cpp - log/logger.cpp - log/safe_stream.cpp - monitoring/abstract_monitor.cpp - monitoring/cpu_freq_monitor.cpp - monitoring/cpu_load_monitor.cpp - monitoring/cpu_temp_monitor.cpp - monitoring/vc_based_monitor.cpp - named_object.cpp - param.cpp - serial_port.cpp - socket.cpp - threaded_loop.cpp - watchdog.cpp - worker.cpp) - -add_library(utils ${SOURCES}) - -target_link_libraries(utils - pthread - vcos - vchiq_arm - vchostif) diff --git a/src/utils/monitoring/vc_based_monitor.h b/src/utils/monitoring/vc_based_monitor.h index 630a082..88527d6 100644 --- a/src/utils/monitoring/vc_based_monitor.h +++ b/src/utils/monitoring/vc_based_monitor.h @@ -4,7 +4,7 @@ #include "abstract_monitor.h" extern "C" { -#include "interface/vmcs_host/vc_vchi_gencmd.h" +#include } namespace Monitoring { diff --git a/src/ux/CMakeLists.txt b/src/ux/CMakeLists.txt deleted file mode 100644 index 0a60535..0000000 --- a/src/ux/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -project(ux CXX) - -set(SOURCES - # cmake-format: sortable - menu/menu_backend.cpp menu/menu_broker.cpp menu/menu_item.cpp) - -add_subdirectory(mosquittopp) -add_subdirectory(Osc99) - -add_library(ux ${SOURCES}) - -target_link_libraries(ux mosquittopp) diff --git a/src/ux/Osc99/CMakeLists.txt b/src/ux/Osc99/CMakeLists.txt deleted file mode 100644 index 4d2a16b..0000000 --- a/src/ux/Osc99/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -project(Osc99 C CXX) - -set(SOURCES - # cmake-format: sortable - OscAddress.c - OscCommon.c - OscError.c - OscBundle.cpp - OscMessage.cpp - OscPacket.cpp - OscSlip.cpp) - -add_library(Osc99 ${SOURCES})