From 48fe9366c906ddada7af95dcc96ff1949f66593f Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Mon, 24 Feb 2020 11:44:26 +0100 Subject: [PATCH 01/22] Updated Mosquittopp --- src/ux/mosquittopp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ux/mosquittopp b/src/ux/mosquittopp index f94927b..7f87022 160000 --- a/src/ux/mosquittopp +++ b/src/ux/mosquittopp @@ -1 +1 @@ -Subproject commit f94927b9cb5415fa448cc7d554b3bfabca14d45f +Subproject commit 7f87022e9cae9fd8e63be906adf847fe57fdfbee From e4a8a486799e2299bd9491e8b8f736aa381367b9 Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Tue, 2 Feb 2021 13:26:42 +0100 Subject: [PATCH 02/22] Use bcm2835 instead of wiringPi --- src/CMakeLists.txt | 2 +- src/components/internal/gpio/gpio.cpp | 20 ++++++++++---------- src/control/algo/myocontrol.cpp | 1 + src/sam/samanager.cpp | 4 ++-- src/ui/visual/ledstrip.cpp | 21 ++++++++++++--------- 5 files changed, 26 insertions(+), 22 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 84b62af..c27c15e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -23,6 +23,6 @@ target_link_libraries(sam ui utils ux - wiringPi) + bcm2835) install(TARGETS sam RUNTIME DESTINATION /opt/) 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/control/algo/myocontrol.cpp b/src/control/algo/myocontrol.cpp index d225f37..796b3d6 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/sam/samanager.cpp b/src/sam/samanager.cpp index cd5e32e..3063f07 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(); diff --git a/src/ui/visual/ledstrip.cpp b/src/ui/visual/ledstrip.cpp index f061ba5..42e34f4 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; @@ -12,13 +12,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) @@ -41,23 +44,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); } From ddba91d36e461becdfba60ef69c1deff7218e518 Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Wed, 10 Feb 2021 13:14:36 +0100 Subject: [PATCH 03/22] Update CMakeLists.txt --- src/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c27c15e..4d05102 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -7,7 +7,8 @@ set(SOURCES sam/samanager.cpp sam/system_monitor.cpp) -include_directories(.) +include_directories(. /opt/vc/include) +link_directories(/opt/vc/lib) add_subdirectory(components) add_subdirectory(control) From 544f8d971752eec174634712d25230da02ff88e7 Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Thu, 20 May 2021 11:03:53 +0200 Subject: [PATCH 04/22] Fix include --- src/utils/monitoring/vc_based_monitor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 { From 5beadfe31c54d013c9e73e12fdf3a055e15576a9 Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Thu, 20 May 2021 11:06:47 +0200 Subject: [PATCH 05/22] Use meson instead of CMake --- .gitignore | 4 + CMakeLists.txt | 17 ---- cross_file.ini | 23 +++++ meson.build | 91 +++++++++++++++++++ native.ini | 8 ++ src/CMakeLists.txt | 29 ------ src/components/CMakeLists.txt | 8 -- src/components/external/CMakeLists.txt | 9 -- .../external/myoband/CMakeLists.txt | 12 --- .../external/optitrack/CMakeLists.txt | 7 -- src/components/external/ximu/CMakeLists.txt | 7 -- src/components/internal/CMakeLists.txt | 11 --- .../internal/actuators/CMakeLists.txt | 17 ---- .../actuators/roboclaw/CMakeLists.txt | 10 -- src/components/internal/adc/CMakeLists.txt | 9 -- src/components/internal/dac/CMakeLists.txt | 7 -- src/components/internal/gpio/CMakeLists.txt | 7 -- src/components/internal/hand/CMakeLists.txt | 7 -- src/control/CMakeLists.txt | 19 ---- src/ui/CMakeLists.txt | 13 --- src/utils/CMakeLists.txt | 28 ------ src/ux/CMakeLists.txt | 11 --- 22 files changed, 126 insertions(+), 228 deletions(-) delete mode 100644 CMakeLists.txt create mode 100644 cross_file.ini create mode 100644 meson.build create mode 100644 native.ini delete mode 100644 src/CMakeLists.txt delete mode 100644 src/components/CMakeLists.txt delete mode 100644 src/components/external/CMakeLists.txt delete mode 100644 src/components/external/myoband/CMakeLists.txt delete mode 100644 src/components/external/optitrack/CMakeLists.txt delete mode 100644 src/components/external/ximu/CMakeLists.txt delete mode 100644 src/components/internal/CMakeLists.txt delete mode 100644 src/components/internal/actuators/CMakeLists.txt delete mode 100644 src/components/internal/actuators/roboclaw/CMakeLists.txt delete mode 100644 src/components/internal/adc/CMakeLists.txt delete mode 100644 src/components/internal/dac/CMakeLists.txt delete mode 100644 src/components/internal/gpio/CMakeLists.txt delete mode 100644 src/components/internal/hand/CMakeLists.txt delete mode 100644 src/control/CMakeLists.txt delete mode 100644 src/ui/CMakeLists.txt delete mode 100644 src/utils/CMakeLists.txt delete mode 100644 src/ux/CMakeLists.txt 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/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/cross_file.ini b/cross_file.ini new file mode 100644 index 0000000..d3dfd0b --- /dev/null +++ b/cross_file.ini @@ -0,0 +1,23 @@ +[host_machine] +system = 'linux' +cpu_family = 'arm' +cpu = 'armv7' +endian = 'little' + +[constants] +arch = '/usr/bin/arm-linux-gnueabihf-' +sysroot = '/home/florian/ISIR/sam-imager/mnt' +cpp_flags = ['--sysroot=' + sysroot, '-isystem' + sysroot + '/opt/vc/include', '-isystem' + sysroot + '/usr/local/include'] +link_flags = ['--sysroot=' + sysroot, '-L' + sysroot + '/opt/vc/lib','-L' + sysroot + '/usr/local/lib'] + +[built-in options] +cpp_args = cpp_flags +cpp_link_args = link_flags + +[binaries] +c = arch + 'gcc' +cpp = arch + 'g++' +c_ld = 'gold' +cpp_ld = 'gold' +ar = arch + 'ar' +strip = arch + 'strip' diff --git a/meson.build b/meson.build new file mode 100644 index 0000000..7801813 --- /dev/null +++ b/meson.build @@ -0,0 +1,91 @@ +project( + 'sam', + '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/optitrack/optitrack_listener.cpp', + 'src/components/external/ximu/ximu.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/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/adc/adafruit_ads1115.cpp', + 'src/components/internal/dac/mcp4728.cpp', + 'src/components/internal/gpio/gpio.cpp', + 'src/components/internal/hand/touch_bionics_hand.cpp', + 'src/control/algo/lawimu.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/demo.cpp', + 'src/control/general_formulation.cpp', + 'src/control/matlab_receiver.cpp', + 'src/control/remote_computer_control.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']) +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') + +sam_target = executable('sam', + sam_src, + include_directories:sam_public_headers, + cpp_args:['-DMOSQUITTO_SERVER_IP="127.0.0.1"','-DMOSQUITTO_SERVER_PORT=1883','-DDEFAULT_CPU_CORE=2','-DDEFAULT_THREAD_PRIO=20'], + dependencies : [mosquitto_dep, bcm2835_dep, cppfs_dep, thread_dep, i2c_dep, vc_dep] +) diff --git a/native.ini b/native.ini new file mode 100644 index 0000000..8f2602f --- /dev/null +++ b/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'] + +[built-in options] +cpp_args = cpp_flags +cpp_link_args = link_flags + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt deleted file mode 100644 index 4d05102..0000000 --- a/src/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -project(sam CXX) - -set(SOURCES - # cmake-format: sortable - main.cpp - sam/sam.cpp - sam/samanager.cpp - sam/system_monitor.cpp) - -include_directories(. /opt/vc/include) -link_directories(/opt/vc/lib) - -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 - bcm2835) - -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 a29e318..0000000 --- a/src/components/external/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -project(external CXX) - -add_subdirectory(myoband) -add_subdirectory(optitrack) -add_subdirectory(ximu) - -add_library(external INTERFACE) - -target_link_libraries(external INTERFACE myoband optitrack ximu) 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/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 a893dd8..0000000 --- a/src/components/internal/actuators/CMakeLists.txt +++ /dev/null @@ -1,17 +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) - -add_subdirectory(roboclaw) - -add_library(actuators ${SOURCES}) - -target_link_libraries(actuators roboclaw) 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/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/hand/CMakeLists.txt b/src/components/internal/hand/CMakeLists.txt deleted file mode 100644 index 6f4786f..0000000 --- a/src/components/internal/hand/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -project(hand CXX) - -set(SOURCES - # cmake-format: sortable - touch_bionics_hand.cpp) - -add_library(hand ${SOURCES}) diff --git a/src/control/CMakeLists.txt b/src/control/CMakeLists.txt deleted file mode 100644 index 1a8d59b..0000000 --- a/src/control/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -project(control CXX) - -set(SOURCES - # cmake-format: sortable - algo/lawimu.cpp - algo/lawjacobian.cpp - algo/lawopti.cpp - algo/myocontrol.cpp - compensation_imu.cpp - compensation_optitrack.cpp - demo.cpp - general_formulation.cpp - matlab_receiver.cpp - remote_computer_control.cpp - voluntary_control.cpp) - -add_library(control ${SOURCES}) - -target_link_libraries(control stdc++fs) 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/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/ux/CMakeLists.txt b/src/ux/CMakeLists.txt deleted file mode 100644 index 99ac3a5..0000000 --- a/src/ux/CMakeLists.txt +++ /dev/null @@ -1,11 +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_library(ux ${SOURCES}) - -target_link_libraries(ux mosquittopp) From c022a00a8cd2ed7e238dbcb1cb8a2c14bc9036a3 Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Thu, 20 May 2021 15:21:21 +0200 Subject: [PATCH 06/22] Update meson.build --- meson.build | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/meson.build b/meson.build index 7801813..7aa5b46 100644 --- a/meson.build +++ b/meson.build @@ -83,9 +83,13 @@ 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, - cpp_args:['-DMOSQUITTO_SERVER_IP="127.0.0.1"','-DMOSQUITTO_SERVER_PORT=1883','-DDEFAULT_CPU_CORE=2','-DDEFAULT_THREAD_PRIO=20'], - dependencies : [mosquitto_dep, bcm2835_dep, cppfs_dep, thread_dep, i2c_dep, vc_dep] + include_directories : sam_public_headers, + dependencies : [bcm2835_dep, cppfs_dep, i2c_dep, mosquitto_dep, thread_dep, vc_dep], ) From 9b5592a6b48fa845e96eea4726c8093b522f7b35 Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Thu, 20 May 2021 15:21:32 +0200 Subject: [PATCH 07/22] Move machine files to a subfolder --- cross_file.ini => meson/cross_file.ini | 2 +- native.ini => meson/native.ini | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) rename cross_file.ini => meson/cross_file.ini (96%) rename native.ini => meson/native.ini (90%) diff --git a/cross_file.ini b/meson/cross_file.ini similarity index 96% rename from cross_file.ini rename to meson/cross_file.ini index d3dfd0b..efc19b9 100644 --- a/cross_file.ini +++ b/meson/cross_file.ini @@ -10,7 +10,7 @@ sysroot = '/home/florian/ISIR/sam-imager/mnt' cpp_flags = ['--sysroot=' + sysroot, '-isystem' + sysroot + '/opt/vc/include', '-isystem' + sysroot + '/usr/local/include'] link_flags = ['--sysroot=' + sysroot, '-L' + sysroot + '/opt/vc/lib','-L' + sysroot + '/usr/local/lib'] -[built-in options] +[properties] cpp_args = cpp_flags cpp_link_args = link_flags diff --git a/native.ini b/meson/native.ini similarity index 90% rename from native.ini rename to meson/native.ini index 8f2602f..593f352 100644 --- a/native.ini +++ b/meson/native.ini @@ -2,7 +2,7 @@ cpp_flags = ['-isystem/opt/vc/include', '-isystem/usr/local/include'] link_flags = ['-L/opt/vc/lib','-L/usr/local/lib'] -[built-in options] +[properties] cpp_args = cpp_flags cpp_link_args = link_flags From dd263d20fd14dc0b88cee7cd7b12542567eaaa54 Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Mon, 31 May 2021 17:05:52 +0200 Subject: [PATCH 08/22] Update cross file for buildroot image --- meson/cross_file.ini | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/meson/cross_file.ini b/meson/cross_file.ini index efc19b9..504a6bc 100644 --- a/meson/cross_file.ini +++ b/meson/cross_file.ini @@ -5,10 +5,10 @@ cpu = 'armv7' endian = 'little' [constants] -arch = '/usr/bin/arm-linux-gnueabihf-' -sysroot = '/home/florian/ISIR/sam-imager/mnt' -cpp_flags = ['--sysroot=' + sysroot, '-isystem' + sysroot + '/opt/vc/include', '-isystem' + sysroot + '/usr/local/include'] -link_flags = ['--sysroot=' + sysroot, '-L' + sysroot + '/opt/vc/lib','-L' + sysroot + '/usr/local/lib'] +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 @@ -17,7 +17,5 @@ cpp_link_args = link_flags [binaries] c = arch + 'gcc' cpp = arch + 'g++' -c_ld = 'gold' -cpp_ld = 'gold' ar = arch + 'ar' strip = arch + 'strip' From 8b040ae01437a66ffa62368f96b66c105da37570 Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Mon, 31 May 2021 17:05:58 +0200 Subject: [PATCH 09/22] Fix missing include --- src/components/internal/actuators/roboclaw/factory.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 { From 6529796c3aac45ae6a1ee926cab6f65a0c62048a Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Mon, 31 May 2021 17:06:24 +0200 Subject: [PATCH 10/22] Add config files for vscode --- .vscode/c_cpp_properties.json | 17 ++++++ .vscode/extensions.json | 7 +++ .vscode/launch.json | 24 ++++++++ .vscode/settings.json | 7 +++ .vscode/tasks.json | 101 ++++++++++++++++++++++++++++++++++ 5 files changed, 156 insertions(+) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/extensions.json create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json create mode 100644 .vscode/tasks.json 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..86a4f68 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,7 @@ +{ + "files.associations": { + "exception": "cpp", + "stdexcept": "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..1e6dfd4 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,101 @@ +{ + "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": "Copy executable", + "type": "shell", + "command": "scp sam root@10.0.0.1:/opt/sam", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/builddir" + }, + "dependsOn": "Build" + }, + { + "label": "Deploy", + "group": "build", + "dependsOn": [ + "SAM: copy executable", + ], + "dependsOrder": "sequence", + "problemMatcher": [] + }, + { + "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": "SAM: kill remote gdbserver", + "presentation": { + "echo": true, + "reveal": "always", + "focus": true, + "panel": "dedicated", + "showReuseMessage": true, + "clear": true + } + } + ] +} \ No newline at end of file From 4356eb54bb6dde63180edf39ed03d95b1a9399cd Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Thu, 3 Jun 2021 15:55:41 +0200 Subject: [PATCH 11/22] Fix task names --- .vscode/tasks.json | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 1e6dfd4..7bf4965 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -57,7 +57,7 @@ } }, { - "label": "Copy executable", + "label": "Deploy", "type": "shell", "command": "scp sam root@10.0.0.1:/opt/sam", "group": "build", @@ -67,15 +67,6 @@ }, "dependsOn": "Build" }, - { - "label": "Deploy", - "group": "build", - "dependsOn": [ - "SAM: copy executable", - ], - "dependsOrder": "sequence", - "problemMatcher": [] - }, { "label": "Kill remote gdbserver", "type": "shell", @@ -87,7 +78,7 @@ "type": "shell", "command": "gnome-terminal -- ssh -t root@10.0.0.1 gdbserver localhost:4444 /opt/sam/sam", "group": "test", - "dependsOn": "SAM: kill remote gdbserver", + "dependsOn": "Kill remote gdbserver", "presentation": { "echo": true, "reveal": "always", From 997ac88cd8f09c598b177d159eeb83bd2d3ba87f Mon Sep 17 00:00:00 2001 From: Charlotte Date: Thu, 3 Jun 2021 17:11:33 +0200 Subject: [PATCH 12/22] Slight changes for NGIMUs --- src/sam/sam.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/sam/sam.cpp b/src/sam/sam.cpp index e178c1b..1063c17 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); From 6f19d2a935947c8e8793904f678f2d448a2494c9 Mon Sep 17 00:00:00 2001 From: Charlotte Date: Thu, 3 Jun 2021 17:13:42 +0200 Subject: [PATCH 13/22] Fix task names --- .vscode/tasks.json | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 1e6dfd4..7bf4965 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -57,7 +57,7 @@ } }, { - "label": "Copy executable", + "label": "Deploy", "type": "shell", "command": "scp sam root@10.0.0.1:/opt/sam", "group": "build", @@ -67,15 +67,6 @@ }, "dependsOn": "Build" }, - { - "label": "Deploy", - "group": "build", - "dependsOn": [ - "SAM: copy executable", - ], - "dependsOrder": "sequence", - "problemMatcher": [] - }, { "label": "Kill remote gdbserver", "type": "shell", @@ -87,7 +78,7 @@ "type": "shell", "command": "gnome-terminal -- ssh -t root@10.0.0.1 gdbserver localhost:4444 /opt/sam/sam", "group": "test", - "dependsOn": "SAM: kill remote gdbserver", + "dependsOn": "Kill remote gdbserver", "presentation": { "echo": true, "reveal": "always", From a3f59a89e82ee080662481f5f4abf719706b95d0 Mon Sep 17 00:00:00 2001 From: Charlotte Date: Thu, 3 Jun 2021 17:14:01 +0200 Subject: [PATCH 14/22] Changes in settings.json ? --- .vscode/settings.json | 56 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 55 insertions(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 86a4f68..760052e 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,7 +1,61 @@ { "files.associations": { "exception": "cpp", - "stdexcept": "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" }, "mesonbuild.configureOnOpen": true } \ No newline at end of file From 8d8255ebe4ddaaa17b6e295285795a18deef848f Mon Sep 17 00:00:00 2001 From: Florian Richer Date: Thu, 10 Jun 2021 17:58:33 +0200 Subject: [PATCH 15/22] Fix buzzer pin --- src/sam/sam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sam/sam.cpp b/src/sam/sam.cpp index e178c1b..bf81511 100644 --- a/src/sam/sam.cpp +++ b/src/sam/sam.cpp @@ -39,7 +39,7 @@ Sensors::Sensors() UserFeedback::UserFeedback() { - buzzer = std::make_unique(29); + buzzer = std::make_unique(21); buzzer->set_prio(90); buzzer->set_preferred_cpu(2); From bad53f34cf2f0a9859c6f1ded8d8d3789edb3852 Mon Sep 17 00:00:00 2001 From: Charlotte Date: Fri, 18 Jun 2021 09:12:14 +0200 Subject: [PATCH 16/22] Fix segmentation fault risk --- src/control/compensation_imu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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"); } From a2f44ac82a0c94c4ca7609d020d85aa8966bfa9a Mon Sep 17 00:00:00 2001 From: Charlotte Date: Fri, 18 Jun 2021 09:12:41 +0200 Subject: [PATCH 17/22] EPOS driver --- .../internal/actuators/epos/epos.cpp | 193 +++++++++++++++--- src/components/internal/actuators/epos/epos.h | 20 +- src/sam/sam.cpp | 2 +- src/sam/samanager.cpp | 2 + 4 files changed, 179 insertions(+), 38 deletions(-) diff --git a/src/components/internal/actuators/epos/epos.cpp b/src/components/internal/actuators/epos/epos.cpp index c96f521..088b83c 100644 --- a/src/components/internal/actuators/epos/epos.cpp +++ b/src/components/internal/actuators/epos/epos.cpp @@ -2,36 +2,40 @@ #include #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. + , _protocolStackName("MAXON_RS232") //MAXON_RS232, CANopen ou MAXON SERIAL V2 + , _interfaceName("RS232") //RS232, USB, CAN_ixx_usb, CAN_kvaser_usb 0, etc. + , _portName(portName) //COM1, USB0, CAN0, etc. , _baudrate(1000000) { _menu->set_description("EPOS4"); - _menu->set_code("pronosup"); - - OpenDevice(); + _menu->set_code("epos"); + if(OpenDevice() == 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) args = "0"; set_velocity(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(); }); } 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,18 +47,18 @@ 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) { if(_baudrate==lBaudrate) { @@ -66,7 +70,7 @@ int EPOS::OpenDevice() } else { - _pKeyHandle = nullptr; + _keyHandle = nullptr; } delete []pDeviceName; @@ -77,18 +81,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 +106,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 +124,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 +139,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 +158,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 +168,137 @@ 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; +} \ 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..29ab578 100644 --- a/src/components/internal/actuators/epos/epos.h +++ b/src/components/internal/actuators/epos/epos.h @@ -14,28 +14,30 @@ 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); 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/sam/sam.cpp b/src/sam/sam.cpp index 10aad44..147a41b 100644 --- a/src/sam/sam.cpp +++ b/src/sam/sam.cpp @@ -75,7 +75,7 @@ Joints::Joints() } } - // test_epos = Components::make_component("test_epos"); + test_epos = Components::make_component("test_epos", "epos", "/dev/ttyAMA0", 1); } Components::Components() diff --git a/src/sam/samanager.cpp b/src/sam/samanager.cpp index d7b1ff0..30de803 100644 --- a/src/sam/samanager.cpp +++ b/src/sam/samanager.cpp @@ -24,6 +24,7 @@ SAManager::~SAManager() _menu_mqtt_binding->show_message("Exited gracefully."); if (_robot->user_feedback.leds) _robot->user_feedback.leds->set(LedStrip::none, 11); + _robot->joints.test_epos->CloseDevice(); _robot->mosfet_gpio = false; // if (_robot->joints.elbow_flexion) { // _robot->joints.elbow_flexion->move_to(0, 20); @@ -97,6 +98,7 @@ 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.test_epos); _main_menu->activate(); } From d56e6e4748ae6769584b65ef073c7d3d63614627 Mon Sep 17 00:00:00 2001 From: Charlotte Date: Fri, 18 Jun 2021 09:14:45 +0200 Subject: [PATCH 18/22] Update pin number --- src/sam/sam.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/sam/sam.cpp b/src/sam/sam.cpp index 147a41b..483dd99 100644 --- a/src/sam/sam.cpp +++ b/src/sam/sam.cpp @@ -79,12 +79,12 @@ Joints::Joints() } 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) { } } From 96970aa9ce0f2796954b328d27a09a0938c73489 Mon Sep 17 00:00:00 2001 From: Charlotte Date: Fri, 18 Jun 2021 09:14:57 +0200 Subject: [PATCH 19/22] Small changes --- src/control/read_adc.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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"); From 79aa241bf78699961ecca5fed4a7b2143adf319c Mon Sep 17 00:00:00 2001 From: Charlotte Date: Fri, 18 Jun 2021 09:15:46 +0200 Subject: [PATCH 20/22] Changes in settings.json ? --- .vscode/settings.json | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 760052e..508d5ed 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -55,7 +55,11 @@ "streambuf": "cpp", "thread": "cpp", "cinttypes": "cpp", - "typeinfo": "cpp" + "typeinfo": "cpp", + "complex": "cpp", + "cstring": "cpp", + "map": "cpp", + "fstream": "cpp" }, "mesonbuild.configureOnOpen": true } \ No newline at end of file From 995b9f2eeb54132cea1d63e6f66297def5dcdc5a Mon Sep 17 00:00:00 2001 From: Charlotte Date: Tue, 20 Jul 2021 10:02:56 +0200 Subject: [PATCH 21/22] Test for RS232 --- src/control/test_imu.cpp | 162 +++++++-------------------------------- src/control/test_imu.h | 10 +-- 2 files changed, 32 insertions(+), 140 deletions(-) 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 From 85b979b5f3a7330d3cd7f17e8537ed5d35ba4e7a Mon Sep 17 00:00:00 2001 From: Charlotte Date: Tue, 20 Jul 2021 10:03:13 +0200 Subject: [PATCH 22/22] Tests for EPOS driver --- .../internal/actuators/epos/epos.cpp | 45 ++++++++++++++++--- src/components/internal/actuators/epos/epos.h | 1 + src/sam/sam.cpp | 4 +- src/sam/sam.h | 3 +- src/sam/samanager.cpp | 8 +++- 5 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/components/internal/actuators/epos/epos.cpp b/src/components/internal/actuators/epos/epos.cpp index 088b83c..39e300f 100644 --- a/src/components/internal/actuators/epos/epos.cpp +++ b/src/components/internal/actuators/epos/epos.cpp @@ -7,22 +7,26 @@ EPOS::EPOS(std::string name, std::string portName, unsigned short nodeId) , _name(name) , _usNodeId(nodeId) , _deviceName("EPOS4") - , _protocolStackName("MAXON_RS232") //MAXON_RS232, CANopen ou MAXON SERIAL V2 + , _protocolStackName("MAXON SERIAL V2") //MAXON_RS232, CANopen ou MAXON SERIAL V2 , _interfaceName("RS232") //RS232, USB, CAN_ixx_usb, CAN_kvaser_usb 0, etc. , _portName(portName) //COM1, USB0, CAN0, etc. - , _baudrate(1000000) + , _baudrate(115200) { - _menu->set_description("EPOS4"); - _menu->set_code("epos"); + _menu->set_description("EPOS4 "+ name); + _menu->set_code(name); if(OpenDevice() == 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)); }); + //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(){} @@ -60,6 +64,7 @@ int EPOS::OpenDevice() { if(VCS_GetProtocolStackSettings(_keyHandle, &lBaudrate, &lTimeout, &errorCode)!=0) { + std::cout << lBaudrate << std::endl; if(_baudrate==lBaudrate) { lResult = MMC_SUCCESS; @@ -301,4 +306,30 @@ int EPOS::PrepareDemo(unsigned int* errorCode) } } 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 29ab578..1a6a32e 100644 --- a/src/components/internal/actuators/epos/epos.h +++ b/src/components/internal/actuators/epos/epos.h @@ -25,6 +25,7 @@ class EPOS : public MenuUser { int PrintAvailablePorts(char* p_pInterfaceNameSel); int stop(); int PrepareDemo(unsigned int* eErrorCode); + void GetBaudRate(); protected: virtual void on_exit(){}; diff --git a/src/sam/sam.cpp b/src/sam/sam.cpp index 483dd99..c93900b 100644 --- a/src/sam/sam.cpp +++ b/src/sam/sam.cpp @@ -51,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"); @@ -75,7 +74,8 @@ Joints::Joints() } } - test_epos = Components::make_component("test_epos", "epos", "/dev/ttyAMA0", 1); + //epos1 = Components::make_component("epos1", "epos1", "/dev/ttyAMA0", 1); + //epos2 = Components::make_component("epos2", "epos2", "USB1", 1); } Components::Components() 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 30de803..d020bb5 100644 --- a/src/sam/samanager.cpp +++ b/src/sam/samanager.cpp @@ -24,7 +24,10 @@ SAManager::~SAManager() _menu_mqtt_binding->show_message("Exited gracefully."); if (_robot->user_feedback.leds) _robot->user_feedback.leds->set(LedStrip::none, 11); - _robot->joints.test_epos->CloseDevice(); + 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); @@ -98,7 +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.test_epos); + _main_menu->add_submenu_from_user(_robot->joints.epos1); + _main_menu->add_submenu_from_user(_robot->joints.epos2); _main_menu->activate(); }