From 37c79947e60d33e8406e59047e60e740ba20e476 Mon Sep 17 00:00:00 2001 From: Valentina Gaggero Date: Tue, 17 Oct 2023 16:49:08 +0200 Subject: [PATCH] Parametrized Wrist decoupler and aea3 support in AMC board (#424) Co-authored-by: Alessandro Scalzo --- .../amc/application/v1/bin/.placeholder.txt | 1 + .../application/v1/cfg/amc-template-appl.sct | 28 + .../amc/application/v1/cfg/ipal_cfg2.cpp | 154 ++ .../board/amc/application/v1/cfg/ipal_cfg2.h | 10 + .../v1/cfg/overridden/overridden_appl.cpp | 428 ++++ .../overridden/overridden_configurator.cpp | 134 + .../v1/cfg/overridden/overridden_controller.c | 192 ++ .../v1/cfg/overridden/overridden_error.c | 95 + .../v1/cfg/overridden/overridden_runner.cpp | 241 ++ .../v1/cfg/overridden/overridden_sm.cpp | 233 ++ .../removed.overridden_backdoortransceiver.c | 109 + .../v1/cfg/protocol/EoProtocolAS_fun_amc.c | 261 ++ .../v1/cfg/protocol/EoProtocolAS_fun_ems4rd.c | 307 +++ .../protocol/EoProtocolAS_overridden_fun.h | 72 + .../v1/cfg/protocol/EoProtocolMC_fun_amc.c | 583 +++++ .../v1/cfg/protocol/EoProtocolMC_fun_ems4rd.c | 1753 +++++++++++++ .../protocol/EoProtocolMC_overridden_fun.h | 72 + .../v1/cfg/protocol/EoProtocolMN_fun_amc.c | 1002 ++++++++ .../v1/cfg/protocol/EoProtocolMN_fun_ems4rd.c | 1297 ++++++++++ .../protocol/EoProtocolMN_overridden_fun.h | 72 + .../v1/cfg/protocol/EoProtocolSK_fun_ems4rd.c | 129 + .../protocol/EoProtocolSK_overridden_fun.h | 72 + .../protocol/can/EoCANprotASperiodic_amc.c | 239 ++ .../cfg/protocol/can/EoCANprotASpolling_amc.c | 434 ++++ .../protocol/can/EoCANprotBSperiodic_amc.c | 115 + .../protocol/can/EoCANprotISperiodic_amc.c | 126 + .../protocol/can/EoCANprotMCperiodic_amc.c | 710 ++++++ .../cfg/protocol/can/EoCANprotMCpolling_amc.c | 1289 ++++++++++ .../protocol/can/EoCANprotSKperiodic_amc.c | 89 + .../v1/cfg/startup_stm32h745xx_CM7.s | 621 +++++ .../v1/cfg/theApplication_config.cpp | 284 +++ .../v1/cfg/theApplication_config.h | 90 + .../v1/cfg/theEnvironment_config.h | 28 + .../v1/cfg/theErrorManager_config.h | 43 + .../application/v1/cfg/theHandler_config.h | 157 ++ .../amc/application/v1/cfg/theIPnet_config.h | 40 + .../v1/proj/appl-01.uvoptx} | 20 +- .../v1/proj/appl-01.uvprojx} | 44 +- .../amc/application/v1/src/amc-appl-01.cpp | 55 + .../appl-01/cfg/theApplication_config.h | 20 +- .../amc/examples/appl-01/proj/appl-01.uvoptx | 1063 +++----- .../amc/examples/appl-01/proj/appl-01.uvprojx | 491 ++-- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 10 +- .../v2/proj/ems4rd.diagnostic2ready.uvoptx | 2 +- .../v2/proj/ems4rd.diagnostic2ready.uvprojx | 12 +- .../appl/v2/proj/ems4rd.wristmk2.uvoptx | 138 +- .../appl/v2/proj/ems4rd.wristmk2.uvprojx | 34 +- .../v2/src/eoappservices/EOtheServices_hid.h | 18 +- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 8 +- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 8 +- .../arch-arm/embobj/plus/mc/Calibrators.h | 10 +- .../arch-arm/embobj/plus/mc/Controller.c | 124 +- emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c | 74 +- .../eBcode/arch-arm/embobj/plus/mc/JointSet.c | 571 +++-- .../eBcode/arch-arm/embobj/plus/mc/JointSet.h | 54 +- .../arch-arm/embobj/plus/mc/Joint_hid.h | 2 + .../eBcode/arch-arm/embobj/plus/mc/rtwtypes.h | 10 +- .../embobj/plus/mc/wrist_decoupler.cpp | 2230 ++++++++++------- .../arch-arm/embobj/plus/mc/wrist_decoupler.h | 165 +- .../embobj/plus/mc/wrist_decoupler_data.cpp | 39 - .../eth/embot_app_eth_theEncoderReader.cpp | 40 + .../arch-arm/embot/hw/embot_hw_chip_MA730.cpp | 23 +- .../arch-arm/embot/hw/embot_hw_chip_MA730.h | 9 + .../arch-arm/embot/hw/embot_hw_encoder.cpp | 59 +- 64 files changed, 14289 insertions(+), 2554 deletions(-) create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/bin/.placeholder.txt create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/amc-template-appl.sct create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/ipal_cfg2.cpp create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/ipal_cfg2.h create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_appl.cpp create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_configurator.cpp create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_controller.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_error.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_runner.cpp create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_sm.cpp create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/removed.overridden_backdoortransceiver.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_fun_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_fun_ems4rd.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_overridden_fun.h create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_fun_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_fun_ems4rd.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_overridden_fun.h create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_fun_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_fun_ems4rd.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_overridden_fun.h create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolSK_fun_ems4rd.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolSK_overridden_fun.h create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotASperiodic_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotASpolling_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotBSperiodic_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotISperiodic_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotMCperiodic_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotMCpolling_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotSKperiodic_amc.c create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/startup_stm32h745xx_CM7.s create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.cpp create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theEnvironment_config.h create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theErrorManager_config.h create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theHandler_config.h create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theIPnet_config.h rename emBODY/eBcode/arch-arm/board/amc/{examples/appl-01/proj/appl-01-wrist.uvoptx => application/v1/proj/appl-01.uvoptx} (99%) rename emBODY/eBcode/arch-arm/board/amc/{examples/appl-01/proj/appl-01-wrist.uvprojx => application/v1/proj/appl-01.uvprojx} (99%) create mode 100644 emBODY/eBcode/arch-arm/board/amc/application/v1/src/amc-appl-01.cpp delete mode 100644 emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler_data.cpp diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/bin/.placeholder.txt b/emBODY/eBcode/arch-arm/board/amc/application/v1/bin/.placeholder.txt new file mode 100644 index 0000000000..b0eb8200ba --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/bin/.placeholder.txt @@ -0,0 +1 @@ +.placeholder diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/amc-template-appl.sct b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/amc-template-appl.sct new file mode 100644 index 0000000000..06d66a0c8d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/amc-template-appl.sct @@ -0,0 +1,28 @@ +; ************************************************************* +; *** Scatter-Loading Description File generated by uVision *** +; ************************************************************* + +LR_IROM1 0x08060000 0x00080000 { ; load region size_region + ER_IROM1 0x08060000 0x00080000 { ; load address = execution address + *.o (RESET, +First) + *(InRoot$$Sections) + .ANY (+RO) + .ANY (+XO) + } + RX_DESCR 0x30040000 0x00000060 { + eth_4.o(.RxDecripSection) + } + TX_DESCR 0x30040060 0x000001A0 { + eth_4.o(.TxDecripSection) + } + RX_ARRAY 0x30040200 0x00001800 { + eth_4.o(.RxArraySection) + } + RW_IRAM1 0x38000000 0x00010000 { ; RW data + .ANY (+RW +ZI) + } + RW_IRAM2 0x24000000 0x0007FFC0 { + .ANY (+RW +ZI) + } +} + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/ipal_cfg2.cpp b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/ipal_cfg2.cpp new file mode 100644 index 0000000000..38277b602d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/ipal_cfg2.cpp @@ -0,0 +1,154 @@ + +/* + * Copyright (C) 2021 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +#include "ipal_cfg2.h" + +// external dependencies +#include "embot_hw_eth.h" + +#include "embot_core.h" + + +static void ipal_app_on_fatal_error(ipal_fatalerror_t errorcode, const char * errormsg) +{ + /* Avoid compiler warnings*/ + errorcode = errorcode; + errormsg = errormsg; + volatile uint32_t n = 0; + volatile const char * m = errormsg; + m = errormsg; + +// itm_puts(errormsg); + + for(;;) + { + n = errorcode; + m = errormsg; + } +} + +/* =============================================================================================== + * IPAL CONFIGURATION + * =============================================================================================== */ + + +constexpr ipal_cfg2_support_t suppcfg2 = +{ + .arch_ipstack = ipal_ipstack_iitmod_lwip, + .memorymodel = ipal_memmodel_static, + .supportedmodules = ipal_cfg2_supportedflag_eth | ipal_cfg2_supportedflag_arp | + ipal_cfg2_supportedflag_udp | ipal_cfg2_supportedflag_igmp | + ipal_cfg2_supportedflag_extfn2, + .filler = {0, 0, 0, 0} +}; + +constexpr ipal_cfg2_system_t syscfg2 = +{ + .sys_timetick = 10*embot::core::time1millisec, + .sys_mempoolsize = 0, + .filler = {0, 0} +}; + +constexpr ipal_cfg2_eth_t ethcfg2 = +{ + .eth_mac = IPAL_mac48addr(0x1E,0x30,0x6C,0xA2,0x45,0x5E), + .eth_ip = IPAL_ipv4addr(10, 0, 1, 99), + .eth_mask = IPAL_ipv4addr(255, 255, 255, 0), + .eth_isrpriority = 0, + .filler = {0, 0, 0, 0, 0, 0, 0}, + .hal_eth_init = embot::hw::eth::init, + .hal_eth_enable = embot::hw::eth::enable, + .hal_eth_disable = embot::hw::eth::disable, + .hal_eth_sendframe = embot::hw::eth::sendframe, + .get_frame_size = embot::hw::eth::get_frame_size, + .get_frame = embot::hw::eth::get_frame +}; + +constexpr ipal_cfg2_arp_t arpcfg2 = +{ + .arp_cachetimeout = 255*embot::core::time1second, + .arp_retrytimeout = 0, + .arp_cachesize = 32, + .arp_retrymaxnum = 5, + .arp_autonotify = 0, + .filler = {0, 0, 0, 0, 0} +}; + +constexpr ipal_cfg2_udp_t udpcfg2 = +{ + .udp_socketnum = 5, + .filler = {0, 0, 0, 0} +}; + +constexpr ipal_cfg2_igmp_t igmpcfg2 = +{ + .igmp_groupsnum = 1, + .filler = {0, 0, 0, 0, 0, 0, 0} +}; + +void ipal_app_usr_on_rx_frame(void) +{ + static volatile uint32_t rxnum = 0; + rxnum++; + embot::core::print("received an ETH frame"); +} + +void ipal_app_usr_on_tx_frame(void) +{ + static volatile uint32_t txnum = 0; + txnum++; +} + + +// osal_* can also be nullptr +void * osal_mutex_new(void) +{ + static uint32_t value = 0; + return &value; +} + +ipal_result_t osal_mutex_take(void *mutex, ipal_reltime_t tout) +{ + return ipal_res_OK; +} + +ipal_result_t osal_mutex_release(void *mutex) +{ + return ipal_res_OK; +} + +constexpr uint32_t tout = 10; + +constexpr ipal_cfg2_extfn_t extfncfg2 = +{ + .usr_on_fatal_error = ipal_app_on_fatal_error, + .signal_rx_frame = ipal_app_usr_on_rx_frame, + .signal_tx_frame = ipal_app_usr_on_tx_frame, + .osal_mutex_new = osal_mutex_new, + .osal_mutex_take = osal_mutex_take, + .osal_mutex_release = osal_mutex_release, + .osal_param_tout_forever = 10, + .dummy = 0 +}; + +ipal_cfg2_t ipal_cfg2 = +{ + .support = (ipal_cfg2_support_t*) &suppcfg2, + .system = (ipal_cfg2_system_t*) &syscfg2, + .eth = (ipal_cfg2_eth_t*) ðcfg2, + .arp = (ipal_cfg2_arp_t*) &arpcfg2, + .udp = (ipal_cfg2_udp_t*) &udpcfg2, + .igmp = (ipal_cfg2_igmp_t*) &igmpcfg2, + .dhcp = NULL, + .extfn2 = (ipal_cfg2_extfn_t*) &extfncfg2 +}; + + +// eof - keep a line after that + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/ipal_cfg2.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/ipal_cfg2.h new file mode 100644 index 0000000000..5e20646df3 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/ipal_cfg2.h @@ -0,0 +1,10 @@ + +#ifndef __IPAL_CFG2_H_ +#define __IPAL_CFG2_H_ + +#include "ipal.h" + + +extern ipal_cfg2_t ipal_cfg2; + +#endif diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_appl.cpp b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_appl.cpp new file mode 100644 index 0000000000..9e306e4845 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_appl.cpp @@ -0,0 +1,428 @@ +/* + * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "hal_led.h" +//#include "EOtheLEDpulser.h" +#include "EOtheErrorManager.h" +#include "EoError.h" + +#include "EOVtheSystem.h" +#include "EOMtheEMSconfigurator.h" + +//#include "EOMtheEMSappl_hid.h" + +//#include "EOMtheEMSapplCfg.h" + +#include "EOaction.h" +#include "EOtimer.h" + +//#include "EOMtheEMSapplCfg_cfg.h" + +#include "EOtheCANservice.h" + +//#include "EOtheServices.h" +//#include "EOtheMotionController.h" +//#include "EOtheSTRAIN.h" +//#include "EOtheMAIS.h" +//#include "EOtheSKIN.h" +//#include "EOtheInertials2.h" +//#include "EOtheInertials3.h" +//#include "EOtheTemperatures.h" +//#include "EOthePSC.h" +//#include "EOthePOS.h" +//#include "EOtheETHmonitor.h" +#include "EOVtheCallbackManager.h" + +#include "embot_app_eth_theFTservice.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- + + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- + +#warning TO DO: add functions required by EOMtheEMSappl + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- + + +//// we need to define it ... even if it is useless. we shall remove it later on +//const EOVtheEMSapplCfgBody theapplbodyconfig = +//{ +// EO_INIT(.type) 0, +// EO_INIT(.thetrueconfig) NULL +//}; + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + +//static void s_overridden_appl_led_error_init(void); + +static void s_overridden_appl_initialise_services(void); + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- + +//static const char s_eobj_ownname[] = "EOMtheEMSappl"; + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- + +// it is called by the init task before scheduling is enabled and before we activate the CFG-RUN-ERR state machine +//extern void eom_emsappl_hid_userdef_initialise() //(EOMtheEMSappl* p) +//{ +// // pulse led3 forever at 20 hz. +//// eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/20, 0); +// +// // do whatever is needed to start services. +// s_overridden_appl_initialise_services(); +//} + + +//extern void eom_emsappl_hid_userdef_on_entry_CFG() //(EOMtheEMSappl* p) +//{ +//// eo_errman_Trace(eo_errman_GetHandle(), "called: eom_emsappl_hid_userdef_on_entry_CFG()", s_eobj_ownname); +// +// // pulse led3 forever at 0.50 hz. +//// eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, 2*EOK_reltime1sec, 0); + +// // EOtheCANservice: set straight mode and force parsing of all packets in the RX queues. +// eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_straight); +// eo_canserv_ParseAll(eo_canserv_GetHandle()); + +//#warning -> carefully see why the eth monitor should alert ... +// // tell the ethmonitor to alert the task of the configurator +//// eo_ethmonitor_SetAlert(eo_ethmonitor_GetHandle(), eom_emsconfigurator_GetTask(eom_emsconfigurator_GetHandle()), emsconfigurator_evt_userdef02); +// +// // prefer sending a tx request just in case. because cfg state transmit only if requested an we dont want to have missed a previous request. +// eom_task_SetEvent(eom_emsconfigurator_GetTask(eom_emsconfigurator_GetHandle()), emsconfigurator_evt_ropframeTx); +//} + + +//extern void eom_emsappl_hid_userdef_on_exit_CFG() //(EOMtheEMSappl* p) +//{ +//#warning -> carefully see why the eth monitor should alert ... +// // tell the ethmonitor to alert no task. the relevant on_entry_xxx function will set a new action +//// eo_ethmonitor_SetAlert(eo_ethmonitor_GetHandle(), NULL, 0); +//} + + +//extern void eom_emsappl_hid_userdef_on_entry_RUN()//(EOMtheEMSappl* p) +//{ +//// eo_errman_Trace(eo_errman_GetHandle(), "called: eom_emsappl_hid_userdef_on_entry_RUN()", s_eobj_ownname); +// +// // pulse led3 forever at 1 hz. +// #warning -> carefully see to have all the leds blinking correctly ... +// // eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/1, 0); +//// embot::app::theLEDmanager::getInstance().get(embot::hw::LED::three).pulse(1*embot::core::time1second, 0); +// +// // EOtheCANservice: set on demand mode. then tx all canframes remained in the tx queues +// eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_ondemand); +// eo_canserv_TXstartAll(eo_canserv_GetHandle(), NULL, NULL); +// eo_canserv_TXwaitAllUntilDone(eo_canserv_GetHandle(), 5*eok_reltime1ms); + +//#warning -> carefully see why the eth monitor should alert ... +// // tell the ethmonitor to alert no task, because the runner will tick it now at every cycle +//// eo_ethmonitor_SetAlert(eo_ethmonitor_GetHandle(), NULL, 0); + +// // we dont start services +//} + + +//extern void eom_emsappl_hid_userdef_on_exit_RUN() //EOMtheEMSappl* p) +//{ +// // EOtheCANservice: set straigth mode and force parsing of all packets in the RX queues. +// eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_straight); +// eo_canserv_ParseAll(eo_canserv_GetHandle()); +// +// +// // stop services which were started before on_entry_RUN() +// +//// // motion-control +//// eo_motioncontrol_Stop(eo_motioncontrol_GetHandle()); +//// eo_motioncontrol_Deactivate(eo_motioncontrol_GetHandle()); +//// +//// // stop tx activity of services that may have been started by callback function +//// +//// // strain +//// eo_strain_Stop(eo_strain_GetHandle()); +//// eo_strain_Deactivate(eo_strain_GetHandle()); + +// // ft service +// embot::app::eth::theFTservice::getInstance().Stop(); +// embot::app::eth::theFTservice::getInstance().Deactivate(); + +//// // skin +//// eo_skin_Stop(eo_skin_GetHandle()); +//// eo_skin_Deactivate(eo_skin_GetHandle()); +//// +//// // mais +//// eo_mais_Stop(eo_mais_GetHandle()); +//// eo_mais_Deactivate(eo_mais_GetHandle()); + +//// // inertials +//// eo_inertials2_Stop(eo_inertials2_GetHandle()); +//// eo_inertials2_Deactivate(eo_inertials2_GetHandle()); +//// +//// // inertials3 +//// eo_inertials3_Stop(eo_inertials3_GetHandle()); +//// eo_inertials3_Deactivate(eo_inertials3_GetHandle()); +//// +//// // temperatures +//// eo_temperatures_Stop(eo_temperatures_GetHandle()); +//// eo_temperatures_Deactivate(eo_temperatures_GetHandle()); +//// +//// // psc +//// eo_psc_Stop(eo_psc_GetHandle()); +//// eo_psc_Deactivate(eo_psc_GetHandle()); +//// +//// // pos +//// eo_pos_Stop(eo_pos_GetHandle()); +//// eo_pos_Deactivate(eo_pos_GetHandle()); +//} + +//extern void eom_emsappl_hid_userdef_on_entry_ERR() //EOMtheEMSappl* p) +//{ +//// eo_errman_Trace(eo_errman_GetHandle(), "called: eom_emsappl_hid_userdef_on_entry_ERR()", s_eobj_ownname); + +// // pulse led3 forever at 4 hz. +//// eo_ledpulser_Start(eo_ledpulser_GetHandle(), eo_ledpulser_led_three, EOK_reltime1sec/4, 0); +//// embot::app::theLEDmanager::getInstance().get(embot::hw::LED::three).pulse(embot::core::time1second/4, 0); +// + +// // EOtheCANservice: set straight mode and force parsing of all packets in the RX queues. +// eo_canserv_SetMode(eo_canserv_GetHandle(), eocanserv_mode_straight); +// eo_canserv_ParseAll(eo_canserv_GetHandle()); + +// +// +// // stop services which may have been started +// +//// // motion-control +//// eo_motioncontrol_Stop(eo_motioncontrol_GetHandle()); +//// eo_motioncontrol_Deactivate(eo_motioncontrol_GetHandle()); +//// +//// // stop tx activity of services that may have been started by callback function +//// +//// // strain +//// eo_strain_Stop(eo_strain_GetHandle()); +//// eo_strain_Deactivate(eo_strain_GetHandle()); + +// // ft service +// embot::app::eth::theFTservice::getInstance().Stop(); +// embot::app::eth::theFTservice::getInstance().Deactivate(); +// +//// // skin +//// eo_skin_Stop(eo_skin_GetHandle()); +//// eo_skin_Deactivate(eo_skin_GetHandle()); +//// +//// // mais +//// eo_mais_Stop(eo_mais_GetHandle()); +//// eo_mais_Deactivate(eo_mais_GetHandle()); + +//// // inertials +//// eo_inertials2_Stop(eo_inertials2_GetHandle()); +//// eo_inertials2_Deactivate(eo_inertials2_GetHandle()); +//// +//// // inertials3 +//// eo_inertials3_Stop(eo_inertials3_GetHandle()); +//// eo_inertials3_Deactivate(eo_inertials3_GetHandle()); +//// +//// // temperatures +//// eo_temperatures_Stop(eo_temperatures_GetHandle()); +//// eo_temperatures_Deactivate(eo_temperatures_GetHandle()); +//// +//// // psc +//// eo_psc_Stop(eo_psc_GetHandle()); +//// eo_psc_Deactivate(eo_psc_GetHandle()); +//// +//// // pos +//// eo_pos_Stop(eo_pos_GetHandle()); +//// eo_pos_Deactivate(eo_pos_GetHandle()); +//} + + + +// marco.accame on Nov 26 2014 +// this is the function used by EOtheErrorManager from start-up until redefinition done by the EOMtheEMSappl +// in this first phase, the EOtheErrorManager shall just print a string into the trace port and in case of a fatal error +// it stops execution and start blinking leds +// in the second phase, the EOtheEMSappl redefines begaviours so that error messages are inserted into a sig<> ROP which +// is sent to the remote host inside the standard EOMtheEMSsocket + + +// this function is exactly the same as the default one except for the blinking of leds + +//extern void eom_emsapplcfg_hid_userdef_OnError(eOerrmanErrorType_t errtype, const char *info, eOerrmanCaller_t *caller, const eOerrmanDescriptor_t *des) +//{ +// const char empty[] = "EO?"; +// const char *err = eo_errman_ErrorStringGet(eo_errman_GetHandle(), errtype); +// char str[128]; +// EOMtheEMSapplCfg *emsapplcfg = eom_emsapplcfg_GetHandle(); +// +// const char *eobjstr = (NULL != caller) ? (caller->eobjstr) : (empty); +// uint32_t taskid = (NULL != caller) ? (caller->taskid) : (0); + +// if(emsapplcfg->errmng_haltrace_enabled) +// { +// uint64_t tt = eov_sys_LifeTimeGet(eov_sys_GetHandle()); +// uint32_t sec = tt/(1000*1000); +// uint32_t tmp = tt%(1000*1000); +// uint32_t msec = tmp / 1000; +// uint32_t usec = tmp % 1000; +// +// snprintf(str, sizeof(str), "[%s] @ s%d m%d u%d %s in tsk %d: %s", err, sec, msec, usec, eobjstr, taskid, info); +// hal_trace_puts(str); +// } +// if(errtype <= eo_errortype_error) +// { +// return; +// } +// +// eov_sys_Stop(eov_sys_GetHandle()); +// +// // init leds +// s_overridden_appl_led_error_init(); +// +// // compute the mask of led to be toggled. +// uint8_t ledmask = 0xff; +// if((NULL != caller) && (NULL != caller->eobjstr)) +// { +// if(0 == strcmp(caller->eobjstr, "HAL")) +// { +// eo_common_byte_bitclear(&ledmask, 0); +// } +// else if(0 == strcmp(caller->eobjstr, "OSAL")) +// { +// eo_common_byte_bitclear(&ledmask, 1); +// } +// else if(0 == strcmp(caller->eobjstr, "IPAL")) +// { +// eo_common_byte_bitclear(&ledmask, 2); +// } +// } + +// for(;;) +// { +// hal_sys_delay(100); +// +// if(eobool_true == eo_common_byte_bitcheck(ledmask, 0)) +// { +// hal_led_toggle(hal_led0); +// } +// if(eobool_true == eo_common_byte_bitcheck(ledmask, 1)) +// { +// hal_led_toggle(hal_led1); +// } +// if(eobool_true == eo_common_byte_bitcheck(ledmask, 2)) +// { +// hal_led_toggle(hal_led2); +// } +// +// hal_led_toggle(hal_led3); +// hal_led_toggle(hal_led4); +// hal_led_toggle(hal_led5); +// } +//} + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + +//static void s_overridden_appl_led_error_init(void) +//{ +// // marco.accame: we init all the leds and we switch them off +// hal_led_init(hal_led0, NULL); +// hal_led_init(hal_led1, NULL); +// hal_led_init(hal_led2, NULL); +// hal_led_init(hal_led3, NULL); +// hal_led_init(hal_led4, NULL); +// hal_led_init(hal_led5, NULL); +// +// hal_led_off(hal_led0); +// hal_led_off(hal_led1); +// hal_led_off(hal_led2); +// hal_led_off(hal_led3); +// hal_led_off(hal_led4); +// hal_led_off(hal_led5); +//} + + + +//static void s_overridden_appl_initialise_services(void) +//{ +//// eOipv4addr_t ipaddress = eom_ipnet_GetIPaddress(eom_ipnet_GetHandle()); +//// // if we want to use another board ... uncomment the following for board 10.0.1.2 ... or change to other address. +//// // ipaddress = eo_common_ipv4addr(10, 0, 1, 2); + +//// // initialise services ... +//// eo_services_Initialise(ipaddress); +//// +//// // and start them on the basis of the boardnumber +//// // ..... DONT DO IT ANYMORE because now we use runtime configuration. +//// // eo_services_Start(eo_services_GetHandle()); +//} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + + + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_configurator.cpp b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_configurator.cpp new file mode 100644 index 0000000000..69a6c3d56d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_configurator.cpp @@ -0,0 +1,134 @@ + +/* + * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "EoCommon.h" +#include "EoError.h" +#include "EOtheErrorManager.h" +#include "EOtheCANservice.h" +#include "EOtheCANdiscovery2.h" +#include "embot_app_eth_theETHmonitor.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOMtheEMSconfigurator.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOMtheEMSconfigurator_hid.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- + +//// marco.accame on 20 oct 2015: the object EOtheCANservice is configured with the argument passed to eo_canserv_Initialise() +//// to make the CAN-RX handler to send the event emsconfigurator_evt_userdef00 to the task of EOMtheEMSconfigurator whenever +//// a CAN frame is received in CFG state. the task then executes this function, where it is correct to put the parser. + +//extern void eom_emsconfigurator_hid_userdef_ProcessUserdef00Event(EOMtheEMSconfigurator* p) +//{ +// eo_canserv_ParseAll(eo_canserv_GetHandle()); +//} + + +//// marco.accame on 15 sept 15: the event emsconfigurator_evt_userdef01 is send by a timer inside the EOtheCANdiscovery2 +//// if we are in CFG state so that we can _Tick() it. + +//#warning must configure someone inside transitions of the SM to alert teh candiscovery2 +//extern void eom_emsconfigurator_hid_userdef_ProcessUserdef01Event(EOMtheEMSconfigurator* p) +//{ +// eo_candiscovery2_Tick(eo_candiscovery2_GetHandle()); +//} + + +//extern void eom_emsconfigurator_hid_userdef_ProcessUserdef02Event(EOMtheEMSconfigurator* p) +//{ +// embot::app::eth::theETHmonitor::getInstance().tick(); +//} + + + +// marco.accame on 20 oct 2015: this function is triggered if function eom_emssocket_Transmit() inside the task +// of EOMtheEMSconfigurator it there is a failure to transmit a UDP packet. + +//extern void eom_emsconfigurator_hid_userdef_onemstransceivererror(EOMtheEMStransceiver* p) +//{ +// eOerrmanDescriptor_t errdes = {0}; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_configurator_udptxfailure); +// errdes.par16 = 0; +// errdes.par64 = 0; +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, "EOMtheEMSconfigurator", &errdes); +//} + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_controller.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_controller.c new file mode 100644 index 0000000000..f26cb83526 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_controller.c @@ -0,0 +1,192 @@ +/* + * Copyright (C) 2015 ICub Facility - Istituto Italiano di Tecnologia + * Author: Davide Pollarolo + * email: davide.pollarolo@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + + +#include "EOtheCANservice.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "Controller.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- + +#if 0 +extern void eo_emsController_hid_userdef_config_motor(EOemsController* ctrl, uint8_t motor) +{ + int8_t KpKiKdKs[7]; + ((int16_t*)KpKiKdKs)[0] = 8; //Kp + ((int16_t*)KpKiKdKs)[1] = 2; //Ki + ((int16_t*)KpKiKdKs)[2] = 0; //Kd (unused in 2FOC) + KpKiKdKs [6] = 10; // shift + + uint32_t max_current = ctrl->motor_config_maxcurrentofmotor[motor]; + /* max_current = 5000; // 5A*/ + + #define HAS_QE 0x0001 + #define HAS_HALL 0x0002 + #define HAS_TSENS 0x0004 + #define USE_INDEX 0x0008 + + uint8_t motor_config[6]; + motor_config[0] = 0; // HAS_QE|HAS_HALL; + if (ctrl->motor_config_hasRotorEncoder[motor]) motor_config[0] |= HAS_QE; + if (ctrl->motor_config_hasHallSensor[motor]) motor_config[0] |= HAS_HALL; + if (ctrl->motor_config_hasRotorEncoderIndex[motor]) motor_config[0] |= USE_INDEX; + if (ctrl->motor_config_hasTempSensor[motor]) motor_config[0] |= HAS_TSENS; + /*motor_config[0] = HAS_HALL; // |HAS_QE;*/ + + *(int16_t*)(motor_config+1) = ctrl->motor_config_rotorencoder[motor];//-14400;//-8192; + *(int16_t*)(motor_config+3) = 0; // offset (degrees) + + motor_config[5] = ctrl->motor_config_motorPoles[motor]; + /* motor_config[5] = 8;*/ + + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, motor, 0); + + eOcanprot_command_t cmdPid; + cmdPid.clas = eocanprot_msgclass_pollingMotorControl; + cmdPid.type = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PID; + cmdPid.value = KpKiKdKs; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &cmdPid, id32); + + eOcanprot_command_t cmdMaxCurrent; + cmdMaxCurrent.clas = eocanprot_msgclass_pollingMotorControl; + cmdMaxCurrent.type = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT; + cmdMaxCurrent.value = &max_current; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &cmdMaxCurrent, id32); + + eOcanprot_command_t cmdMotorConfig; + cmdMotorConfig.clas = eocanprot_msgclass_pollingMotorControl; + cmdMotorConfig.type = ICUBCANPROTO_POL_MC_CMD__SET_MOTOR_CONFIG; + cmdMotorConfig.value = motor_config; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &cmdMotorConfig, id32); + + eo_motors_new_state_req(ctrl->motors, motor, icubCanProto_controlmode_idle); +} + +extern void eo_emsController_hid_userdef_set_motor_idle(EOemsController* ctrl, uint8_t motor) +{ + icubCanProto_controlmode_t controlmode_2foc = icubCanProto_controlmode_idle; + + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, motor, 0); + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE; + command.value = &controlmode_2foc; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, id32); + + eo_motors_new_state_req(ctrl->motors, motor, icubCanProto_controlmode_idle); +} + +extern void eo_emsController_hid_userdef_force_motor_idle(EOemsController* ctrl, uint8_t motor) +{ + icubCanProto_controlmode_t controlmode_2foc = icubCanProto_controlmode_forceIdle; + + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, motor, 0); + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE; + command.value = &controlmode_2foc; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, id32); + + eo_motors_new_state_req(ctrl->motors, motor, icubCanProto_controlmode_idle); +} + +extern void eo_emsController_hid_userdef_set_motor_running(EOemsController* ctrl, uint8_t motor) +{ + icubCanProto_controlmode_t controlmode_2foc = icubCanProto_controlmode_openloop; + + #ifdef EXPERIMENTAL_SPEED_CONTROL + controlmode_2foc = icubCanProto_controlmode_speed_voltage; + #endif + + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, motor, 0); + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE; + command.value = &controlmode_2foc; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, id32); + + eo_motors_new_state_req(ctrl->motors, motor, controlmode_2foc); +} +#endif + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_error.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_error.c new file mode 100644 index 0000000000..bce551f745 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_error.c @@ -0,0 +1,95 @@ + +/* + * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOMtheEMSerror.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOMtheEMSerror_hid.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_runner.cpp b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_runner.cpp new file mode 100644 index 0000000000..265acd8f0e --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_runner.cpp @@ -0,0 +1,241 @@ +/* + * Copyright (C) 2015 iCub Facility - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "EoCommon.h" + +#include "EOtheErrorManager.h" +#include "EoError.h" + + +#include "EOtheCANservice.h" +//#include "EOtheInertials2.h" +//#include "EOtheInertials3.h" +//#include "EOtheTemperatures.h" +#include "EOtheCANdiscovery2.h" +//#include "EOtheMotionController.h" +//#include "EOtheSKIN.h" +//#include "EOtheMais.h" +//#include "EOtheStrain.h" +//#include "EOthePSC.h" +//#include "EOthePOS.h" +//#include "EOtheETHmonitor.h" + +#include "EOMtheEMStransceiver.h" + +#include "embot_app_eth_theFTservice.h" + +#if defined(TESTRTC_IS_ACTIVE) +#include "testRTC.h" +#elif defined(enableTHESERVICETESTER) +#include "servicetester.h" +#endif + + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOMtheEMSrunner.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- + +//#include "EOMtheEMSrunner_hid.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- + +//extern void eom_emsrunner_hid_userdef_taskRX_activity_beforedatagramreception(EOMtheEMSrunner *p) +//{ +// +//} + + +//extern void eom_emsrunner_hid_userdef_taskRX_activity_afterdatagramreception(EOMtheEMSrunner *p) +//{ +// // i tick the can-discovery. +// // this function does something only if a discovery is active and if the search timer period has expired. +// eo_candiscovery2_Tick(eo_candiscovery2_GetHandle()); +// +// // i manage the can-bus reception. i parse everything. +// // it will be the can parser functions which will call the relevant objects which will do what they must. +// // as an example, the broadcast skin can frames are always parsed and are given to EOtheSKIN which will decide what to do with them +// eo_canserv_ParseAll(eo_canserv_GetHandle()); +// +//#if defined(TESTRTC_IS_ACTIVE) +// testRTC_RUN_tick(); +//#elif defined(enableTHESERVICETESTER) +// servicetester_runtick(); +//#endif +//} + + + + + +//extern void eom_emsrunner_hid_userdef_taskDO_activity(EOMtheEMSrunner *p) +//{ +//// // so far we tick only the motion control. +//// eo_motioncontrol_Tick(eo_motioncontrol_GetHandle()); +//// +//// eo_mais_Tick(eo_mais_GetHandle()); +//// eo_strain_Tick(eo_strain_GetHandle()); + +// // ft service +// embot::app::eth::theFTservice::getInstance().Tick(); +// +//// eo_psc_Tick(eo_psc_GetHandle()); +//// eo_pos_Tick(eo_pos_GetHandle()); +// +// // however, we could also tick others .... +// // TODO: see if i can move all the _Tick() in the do phase. +// +// // eo_ethmonitor_Tick(eo_ethmonitor_GetHandle()); // if we do it in here, then in eb2/eb4 it warns about execution time overflow +//} + + +//extern void eom_emsrunner_hid_userdef_taskTX_activity_beforedatagramtransmission(EOMtheEMSrunner *p) +//{ +// uint8_t txcan1frames = 0; +// uint8_t txcan2frames = 0; + +// eo_canserv_TXstartAll(eo_canserv_GetHandle(), &txcan1frames, &txcan2frames); +// +// eom_emsrunner_Set_TXcanframes(eom_emsrunner_GetHandle(), txcan1frames, txcan2frames); +//} + + + +//extern void eom_emsrunner_hid_userdef_taskTX_activity_afterdatagramtransmission(EOMtheEMSrunner *p) +//{ +// eObool_t prevTXhadRegulars = eom_emsrunner_CycleHasJustTransmittedRegulars(eom_emsrunner_GetHandle()); +// +// // ticks some services ... +// // marco.accame: i put them in here just after tx phase. however, we can move it even in eom_emsrunner_hid_userdef_taskDO_activity() +// // because eom_emsrunner_CycleHasJustTransmittedRegulars() keeps memory of previous tx cycle. +//// eo_skin_Tick(eo_skin_GetHandle(), prevTXhadRegulars); +// +////#if defined(TESTRTC_IS_ACTIVE) +////#warning ---------------> just for test +//// prevTXhadRegulars = eobool_true; +////#endif +// +//// eo_inertials2_Tick(eo_inertials2_GetHandle(), prevTXhadRegulars); +//// +//// eo_inertials3_Tick(eo_inertials3_GetHandle(), prevTXhadRegulars); +//// +//// eo_temperatures_Tick(eo_temperatures_GetHandle(), prevTXhadRegulars); +//// +//#warning add ethmonitor +//// eo_ethmonitor_Tick(eo_ethmonitor_GetHandle()); + + +// +// // ABSOLUTELY KEEP IT LAST: wait until can tx started by eo_canserv_TXstartAll() in eom_emsrunner_hid_userdef_taskTX_activity_beforedatagramtransmission() is all done +// const eOreltime_t timeout = 3*EOK_reltime1ms; +// eo_canserv_TXwaitAllUntilDone(eo_canserv_GetHandle(), timeout); + +// return; +//} + + +//extern void eom_emsrunner_hid_userdef_onfailedtransmission(EOMtheEMSrunner *p) +//{ +// eOerrmanDescriptor_t errdes = {0}; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_runner_udptxfailure); +// errdes.par16 = 0; +// errdes.par64 = 0; +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, "EOMtheEMSrunner", &errdes); +//} + + +//extern void eom_emsrunner_hid_userdef_onemstransceivererror(EOMtheEMStransceiver *p) +//{ +// eOerrmanDescriptor_t errdes = {0}; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_runner_transceivererror); +// errdes.par16 = 0; +// errdes.par64 = 0; +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_warning, NULL, "EOMtheEMSrunner", &errdes); +//} + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_sm.cpp b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_sm.cpp new file mode 100644 index 0000000000..11efe1c5cc --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/overridden_sm.cpp @@ -0,0 +1,233 @@ + + + +// this file contains redefinition of functions in eOcfg_sm_EMSappl_hid.h, +// in legacy ETH application they were redefined in EOMtheEMSappl.c +// but not anymore. FOR NOW + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + + +#include "EoProtocol.h" +#include "EoProtocolMN.h" +#include "EOaction.h" +#include "EOMtheEMSconfigurator.h" +#include "EOMtheEMSrunner.h" +#include "EOMtheEMSerror.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "eOcfg_sm_EMSappl_hid.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- + + + +// -- init + +extern void eo_cfg_sm_EMSappl_hid_init(EOsm *s) +{ + // does nothing +} + +// -- reset + +extern void eo_cfg_sm_EMSappl_hid_reset(EOsm *s) +{ + // does nothing +} + + +// -- on entry + +extern void eo_cfg_sm_EMSappl_hid_on_entry_CFG(EOsm *s) +{ + // set the correct state + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status); + eOmn_appl_status_t *status = reinterpret_cast(eoprot_variable_ramof_get(eoprot_board_localboard, id32)); + status->currstate = applstate_config; + + // tell the ems socket that it must alert the EOMtheEMSconfigurator upon RX of packets + EOaction_strg astg = {0}; + EOaction *onrx = reinterpret_cast(&astg); + eo_action_SetEvent(onrx, emssocket_evt_packet_received, eom_emsconfigurator_GetTask(eom_emsconfigurator_GetHandle())); + eom_emssocket_Open(eom_emssocket_GetHandle(), onrx, NULL, NULL); + + // if any rx packets is already in the socket then alert the cfg task + if(0 != eom_emssocket_NumberOfReceivedPackets(eom_emssocket_GetHandle())) + { + eom_task_SetEvent(eom_emsconfigurator_GetTask(eom_emsconfigurator_GetHandle()), emssocket_evt_packet_received); + } + + // exec any user-defined activity + #warning: this one is inside ... overridden_appl + //eom_emsappl_hid_userdef_on_entry_CFG(); +} + +extern void eo_cfg_sm_EMSappl_hid_on_entry_RUN(EOsm *s) +{ + // set the correct state + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status); + eOmn_appl_status_t *status = reinterpret_cast(eoprot_variable_ramof_get(eoprot_board_localboard, id32)); + status->currstate = applstate_running; + + EOaction_strg astg = {0}; + EOaction *ontxdone = reinterpret_cast(&astg); + // tell the ems socket that it must alert the EOMtheEMSconfigurator upon RX of packets + eo_action_SetCallback(ontxdone, (eOcallback_t)eom_emsrunner_OnUDPpacketTransmitted, eom_emsrunner_GetHandle(), NULL); + // the socket does not alert anybody when it receives a pkt, but will alert the sending task, so that it knows that it can stop wait. + // the alert is done by a callback, eom_emsrunner_OnUDPpacketTransmitted(), which executed by the sender of the packet directly. + // this funtion is executed with eo_action_Execute(s->socket->ontransmission) inside the EOMtheIPnet object + // for this reason we call eo_action_SetCallback(....., exectask = NULL_); IT MUST NOT BE the callback-manager!!!! + eom_emssocket_Open(eom_emssocket_GetHandle(), NULL, ontxdone, NULL); + + // we activate the runner + eom_emsrunner_Start(eom_emsrunner_GetHandle()); + +#warning add something in here + // exec any user-defined activity +// eom_emsappl_hid_userdef_on_entry_RUN(&s_emsappl_singleton); +} + +extern void eo_cfg_sm_EMSappl_hid_on_entry_ERR(EOsm *s) +{ + // set the correct state + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status); + eOmn_appl_status_t *status = reinterpret_cast(eoprot_variable_ramof_get(eoprot_board_localboard, id32)); + status->currstate = applstate_error; + + // redirect the socket + EOaction_strg astg = {0}; + EOaction *onrx = reinterpret_cast(&astg); + eo_action_SetEvent(onrx, emssocket_evt_packet_received, eom_emserror_GetTask(eom_emserror_GetHandle())); + // the socket alerts the error task for any newly received packets + eom_emssocket_Open(eom_emssocket_GetHandle(), onrx, NULL, NULL); + + // if any rx packets already in socket then alert the err task + if(0 != eom_emssocket_NumberOfReceivedPackets(eom_emssocket_GetHandle())) + { + eom_task_SetEvent(eom_emserror_GetTask(eom_emserror_GetHandle()), emssocket_evt_packet_received); + } + +#warning add something in here + // exec any user-defined activity +// eom_emsappl_hid_userdef_on_entry_ERR(&s_emsappl_singleton); +} + + + +// -- on exit + +extern void eo_cfg_sm_EMSappl_hid_on_exit_CFG(EOsm *s) +{ + #warning add something in here + // eom_emsappl_hid_userdef_on_exit_CFG(&s_emsappl_singleton); +} + +extern void eo_cfg_sm_EMSappl_hid_on_exit_RUN(EOsm *s) +{ + //#warning --> it is good thing to attempt to stop the hal timers in here as well. see comment below. + // marco.accame: if we exit from the runner in an un-expected way with a fatal error, then we dont + // stop teh timers smoothly. thus we do it in here as well. + + // in normal case instead, the stop of the emsrunner is not executed in one function, but in steps + // inside its rx-do-tx tasks. + + + eom_emsrunner_Stop(eom_emsrunner_GetHandle()); + #warning add something in here + // eom_emsappl_hid_userdef_on_exit_RUN(&s_emsappl_singleton); +} + +extern void eo_cfg_sm_EMSappl_hid_on_exit_ERR(EOsm *s) +{ + #warning add something in here + // eom_emsappl_hid_userdef_on_exit_ERR(&s_emsappl_singleton); +} + + + +// -- on trans + +extern void eo_cfg_sm_EMSappl_hid_on_trans_CFG_EVgo2run(EOsm *s) +{ + // nothing to do +} + +extern void eo_cfg_sm_EMSappl_hid_on_trans_CFG_EVgo2err(EOsm *s) +{ + // nothing to do +} + +extern void eo_cfg_sm_EMSappl_hid_on_trans_RUN_EVgo2cfg(EOsm *s) +{ + // nothing to do +} + +extern void eo_cfg_sm_EMSappl_hid_on_trans_RUN_EVgo2err(EOsm *s) +{ + // nothing to do +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/removed.overridden_backdoortransceiver.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/removed.overridden_backdoortransceiver.c new file mode 100644 index 0000000000..cf30cf0ba2 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/overridden/removed.overridden_backdoortransceiver.c @@ -0,0 +1,109 @@ + +/* + * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "OPCprotocolManager.h" + + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + +// use this in case you dont want use the backdoortransceiver +extern opcprotman_cfg_t* opcprotman_getconfiguration(void) +{ + return(NULL); +} + + +// this one is used only in the ems board. use it if you dont want to use the backdoortransceiver. +// otehrwise use calls to opcprotman_personalize_var() function +extern opcprotman_res_t opcprotman_personalize_database(OPCprotocolManager *p) +{ + return(opcprotman_NOK_generic); +} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- + + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_fun_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_fun_amc.c new file mode 100644 index 0000000000..47e7cc6d24 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_fun_amc.c @@ -0,0 +1,261 @@ + + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "stdlib.h" +#include "string.h" +#include "stdio.h" + +#include "EoCommon.h" +#include "EOarray.h" + +#include "EoAnalogSensors.h" + +#include "EOtheCANservice.h" +#include "EoProtocol.h" +#include "EoProtocolAS.h" + + +//#include "EOtheEntities.h" + +#include "EoError.h" +#include "EOtheErrorManager.h" + +#if defined(USE_EMBOT_theServices) +#include "embot_app_eth_theServices.h" +#else +#include "EOtheServices.h" +#endif + +#if defined(USE_EMBOT_theHandler) +#include "theApplication_config.h" +#include "theHandler_Config.h" +#include "embot_app_eth_theHandler.h" +#else +#include "EOMtheEMSappl.h" +#include "EOMtheEMSapplCfg.h" +#endif + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + +extern void eoprot_fun_UPDT_as_strain_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_strain_config_t *cfg = (eOas_strain_config_t*)rd->data; + eo_strain_Set(eo_strain_GetHandle(), cfg); +} + +extern void eoprot_fun_UPDT_as_strain_config_mode(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_strainmode_t *mode = (eOas_strainmode_t*)rd->data; + eo_strain_SetMode(eo_strain_GetHandle(), *mode); + +} + +extern void eoprot_fun_UPDT_as_strain_config_datarate(const EOnv* nv, const eOropdescriptor_t* rd) +{ + uint8_t *datarate = (uint8_t*)rd->data; + eo_strain_SetDataRate(eo_strain_GetHandle(), *datarate); +} + + +extern void eoprot_fun_UPDT_as_strain_config_signaloncefullscale(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eObool_t *signaloncefullscale = (eObool_t*)rd->data; + if(eobool_true == *signaloncefullscale) + { + eo_strain_GetFullScale(eo_strain_GetHandle(), NULL); + } +} + +extern void eoprot_fun_UPDT_as_inertial_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_inertial_config_t *cfg = (eOas_inertial_config_t*)rd->data; + eo_inertials2_Config(eo_inertials2_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_inertial_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_inertial_commands_t *cmd = (eOas_inertial_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_inertials2_Stop(eo_inertials2_GetHandle()); + } + else + { + eo_inertials2_Start(eo_inertials2_GetHandle()); + eo_inertials2_Transmission(eo_inertials2_GetHandle(), eobool_true); + } +} + + + +extern void eoprot_fun_UPDT_as_inertial3_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_inertial3_config_t *cfg = (eOas_inertial3_config_t*)rd->data; + eo_inertials3_Config(eo_inertials3_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_inertial3_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_inertial3_commands_t *cmd = (eOas_inertial3_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_inertials3_Stop(eo_inertials3_GetHandle()); + } + else + { + eo_inertials3_Start(eo_inertials3_GetHandle()); + eo_inertials3_Transmission(eo_inertials3_GetHandle(), eobool_true); + } +} + + +extern void eoprot_fun_UPDT_as_temperature_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_temperature_config_t *cfg = (eOas_temperature_config_t*)rd->data; + eo_temperatures_Config(eo_temperatures_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_temperature_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_temperature_commands_t *cmd = (eOas_temperature_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_temperatures_Stop(eo_temperatures_GetHandle()); + } + else + { + eo_temperatures_Start(eo_temperatures_GetHandle()); + eo_temperatures_Transmission(eo_temperatures_GetHandle(), eobool_true); + } +} + + +extern void eoprot_fun_UPDT_as_psc_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_psc_config_t *cfg = (eOas_psc_config_t*)rd->data; + eo_psc_Config(eo_psc_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_psc_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_psc_commands_t *cmd = (eOas_psc_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_psc_Stop(eo_psc_GetHandle()); + } + else + { + eo_psc_Start(eo_psc_GetHandle()); + eo_psc_Transmission(eo_psc_GetHandle(), eobool_true); + } +} + + +extern void eoprot_fun_UPDT_as_pos_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_pos_config_t *cfg = (eOas_pos_config_t*)rd->data; + eo_pos_Config(eo_pos_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_pos_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_pos_commands_t *cmd = (eOas_pos_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_pos_Stop(eo_pos_GetHandle()); + } + else + { + eo_pos_Start(eo_pos_GetHandle()); + eo_pos_Transmission(eo_pos_GetHandle(), eobool_true); + } +} + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- + + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_fun_ems4rd.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_fun_ems4rd.c new file mode 100644 index 0000000000..d550ddfa12 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_fun_ems4rd.c @@ -0,0 +1,307 @@ +/* + * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia + * Author: Valentina Gaggero + * email: valentina.gaggero@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +/* @file EoProtocolAS_fun_ems4rd.c + @brief This file keeps the user-defined functions used in every ems board ebx for endpoint as + @author valentina.gaggero@iit.it + @date 06/11/2012 +**/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "stdlib.h" +#include "string.h" +#include "stdio.h" +#include "hal.h" + +#include "EoCommon.h" +#include "EOarray.h" + +#include "EoAnalogSensors.h" + + +#include "EOMtheEMSappl.h" + +#include "EOtheCANservice.h" + + +#include "EoProtocol.h" + +#include "EoProtocolAS.h" + + +#include "EOtheEntities.h" + +#include "EoError.h" +#include "EOtheErrorManager.h" + +#include "EOtheMAIS.h" +#include "EOtheSTRAIN.h" +#include "EOtheInertials2.h" +#include "EOtheInertials3.h" +#include "EOtheTemperatures.h" +#include "EOthePSC.h" +#include "EOthePOS.h" + +#include "EOMtheEMStransceiver.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + +extern void eoprot_fun_UPDT_as_mais_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_mais_config_t *cfg = (eOas_mais_config_t*)rd->data; + eo_mais_Set(eo_mais_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_mais_config_mode(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_maismode_t *maismode = (eOas_maismode_t*)rd->data; + eo_mais_SetMode(eo_mais_GetHandle(), *maismode); +} + + +extern void eoprot_fun_UPDT_as_mais_config_datarate(const EOnv* nv, const eOropdescriptor_t* rd) +{ + uint8_t *datarate = (uint8_t*)rd->data; + eo_mais_SetDataRate(eo_mais_GetHandle(), *datarate); +} + + +extern void eoprot_fun_UPDT_as_mais_config_resolution(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_maisresolution_t *resolution = (eOas_maisresolution_t*)rd->data; + eo_mais_SetResolution(eo_mais_GetHandle(), *resolution); +} + + +extern void eoprot_fun_UPDT_as_strain_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_strain_config_t *cfg = (eOas_strain_config_t*)rd->data; + eo_strain_Set(eo_strain_GetHandle(), cfg); +} + +extern void eoprot_fun_UPDT_as_strain_config_mode(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_strainmode_t *mode = (eOas_strainmode_t*)rd->data; + eo_strain_SetMode(eo_strain_GetHandle(), *mode); + +} + +extern void eoprot_fun_UPDT_as_strain_config_datarate(const EOnv* nv, const eOropdescriptor_t* rd) +{ + uint8_t *datarate = (uint8_t*)rd->data; + eo_strain_SetDataRate(eo_strain_GetHandle(), *datarate); +} + + +extern void eoprot_fun_UPDT_as_strain_config_signaloncefullscale(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eObool_t *signaloncefullscale = (eObool_t*)rd->data; + if(eobool_true == *signaloncefullscale) + { + eo_strain_GetFullScale(eo_strain_GetHandle(), NULL); + } +} + +extern void eoprot_fun_UPDT_as_inertial_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_inertial_config_t *cfg = (eOas_inertial_config_t*)rd->data; + eo_inertials2_Config(eo_inertials2_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_inertial_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_inertial_commands_t *cmd = (eOas_inertial_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_inertials2_Stop(eo_inertials2_GetHandle()); + } + else + { + eo_inertials2_Start(eo_inertials2_GetHandle()); + eo_inertials2_Transmission(eo_inertials2_GetHandle(), eobool_true); + } +} + + + +extern void eoprot_fun_UPDT_as_inertial3_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_inertial3_config_t *cfg = (eOas_inertial3_config_t*)rd->data; + eo_inertials3_Config(eo_inertials3_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_inertial3_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_inertial3_commands_t *cmd = (eOas_inertial3_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_inertials3_Stop(eo_inertials3_GetHandle()); + } + else + { + eo_inertials3_Start(eo_inertials3_GetHandle()); + eo_inertials3_Transmission(eo_inertials3_GetHandle(), eobool_true); + } +} + + +extern void eoprot_fun_UPDT_as_temperature_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_temperature_config_t *cfg = (eOas_temperature_config_t*)rd->data; + eo_temperatures_Config(eo_temperatures_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_temperature_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_temperature_commands_t *cmd = (eOas_temperature_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_temperatures_Stop(eo_temperatures_GetHandle()); + } + else + { + eo_temperatures_Start(eo_temperatures_GetHandle()); + eo_temperatures_Transmission(eo_temperatures_GetHandle(), eobool_true); + } +} + + +extern void eoprot_fun_UPDT_as_psc_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_psc_config_t *cfg = (eOas_psc_config_t*)rd->data; + eo_psc_Config(eo_psc_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_psc_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_psc_commands_t *cmd = (eOas_psc_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_psc_Stop(eo_psc_GetHandle()); + } + else + { + eo_psc_Start(eo_psc_GetHandle()); + eo_psc_Transmission(eo_psc_GetHandle(), eobool_true); + } +} + + +extern void eoprot_fun_UPDT_as_pos_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_pos_config_t *cfg = (eOas_pos_config_t*)rd->data; + eo_pos_Config(eo_pos_GetHandle(), cfg); +} + + +extern void eoprot_fun_UPDT_as_pos_cmmnds_enable(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOas_pos_commands_t *cmd = (eOas_pos_commands_t*)rd->data; + + if(0 == cmd->enable) + { + eo_pos_Stop(eo_pos_GetHandle()); + } + else + { + eo_pos_Start(eo_pos_GetHandle()); + eo_pos_Transmission(eo_pos_GetHandle(), eobool_true); + } +} + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- + + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_overridden_fun.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_overridden_fun.h new file mode 100644 index 0000000000..928f91633d --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolAS_overridden_fun.h @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef _EOPROTOCOLAS_OVERRIDDEN_FUN_H_ +#define _EOPROTOCOLAS_OVERRIDDEN_FUN_H_ + + + + +/** @file EoProtocolAS_overridden_fun.h + @brief This header file specifies which functions are overridden in the analogsensor of analosensor + @author marco.accame@iit.it + @date 06/06/2013 +**/ + +/** @defgroup eo_asfdgr1231 Functions overridden in analog sensor endpoint + edfefle. + + @{ + **/ + + + +// - external dependencies -------------------------------------------------------------------------------------------- +// empty-section + + + + +// - public #define -------------------------------------------------------------------------------------------------- + +//#define OVERRIDE_eoprot_fun_INIT_as_strain_config + + +// - declaration of public user-defined types ------------------------------------------------------------------------- +// empty-section + + +// - declaration of extern public variables, ... but better using use _get/_set instead ------------------------------- +// empty-section + +// - declaration of extern public functions --------------------------------------------------------------------------- +// empty-section + + + + +/** @} + end of group eo_asfdgr1231 + **/ + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_fun_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_fun_amc.c new file mode 100644 index 0000000000..38aa9beb05 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_fun_amc.c @@ -0,0 +1,583 @@ + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "stdlib.h" +#include "string.h" +#include "stdio.h" + +#include "EoCommon.h" +#include "EOnv.h" + +#include "EoMotionControl.h" + + + +#include "EOtheEntities.h" + +#include "EOtheErrorManager.h" +#include "EoError.h" + +#include "EOtheCANservice.h" + + +#include "EOtheBOARDtransceiver.h" + + +#include "EOtheMotionController.h" + + + + +// foc-case +#include "Controller.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EoProtocolMC.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- + +// remove this if you want to use function eoprot_fun_UPDT_mc_controller_config_jointcoupling() as defined inside this file +#define EOMOTIONCONTROL_DONTREDEFINE_JOINTCOUPLING_CALLBACK + + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- + +typedef enum +{ + pid_typePOS = 0, + pid_typeTOR = 1 +} pid_type_t; + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + +// -- common functions + +static eOmotioncontroller_mode_t s_motorcontrol_getmode(void); +static void send_diagnostic_debugmessage(eOerrmanErrorType_t type, eOerror_value_DEB_t value, uint8_t jointnum, uint16_t par16, uint64_t par64, const char* info); + + +// -- foc-based functions + + +// -- mc4plus-based functions + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + +// -- entity joint + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_joint_config_t *cfg = (eOmc_joint_config_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_joint_status_t *jstatus = eo_entities_GetJointStatus(eo_entities_GetHandle(), jxx); + + // now we see if it is a mc4can or a 2foc or a mc4plus + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_config_joint(jxx, cfg, mcmode); + } + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_pidposition(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_config_minjerk_pid(jxx, pid); + } +} + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_pidvelocity(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + //MController_config_direct_pid(jxx, pid); + } +} + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_pidtorque(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + //float rescaler = 1.0f/(float)(1<scale); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_motor_config_torque_PID(jxx, pid); + } + +} + + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_motor_params(const EOnv* nv, const eOropdescriptor_t* rd) +{ // not for mc4can + eOmc_motor_params_t *mparams = (eOmc_motor_params_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_config_motor_friction(jxx, mparams); + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_impedance(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_impedance_t *impedance = (eOmc_impedance_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_config_joint_impedance(jxx, impedance); + } // mark +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_userlimits(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmeas_position_limits_t *limitsofjoint = (eOmeas_position_limits_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_config_joint_pos_limits(jxx, limitsofjoint->min, limitsofjoint->max); + } + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_velocitysetpointtimeout(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmeas_time_t *time = (eOmeas_time_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_config_joint_vel_ref_timeout(jxx, *time); + } // marker + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_status_core_modes_ismotiondone(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + // do nothing + } // marker + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_setpoint(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_setpoint_t *setpoint = (eOmc_setpoint_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_joint_t *joint = eo_entities_GetJoint(eo_entities_GetHandle(), jxx); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if(NULL == joint) + { + return; // error... maybe robotInterface sends a command to a joint with index higher than supported + } + + + joint->status.core.modes.ismotiondone = eobool_false; + + if((eo_motcon_mode_foc == mcmode)) + { + switch(setpoint->type) + { + case eomc_setpoint_position: + { + MController_set_joint_pos_ref(jxx, setpoint->to.position.value, setpoint->to.position.withvelocity); + } break; + + case eomc_setpoint_positionraw: + { + MController_set_joint_pos_raw(jxx, setpoint->to.positionraw.value); + } break; + + case eomc_setpoint_velocity: + { + MController_set_joint_vel_ref(jxx, setpoint->to.velocity.value, setpoint->to.velocity.withacceleration); + //MController_set_joint_vel_raw(jxx, setpoint->to.velocity.value); + } break; + + case eomc_setpoint_torque: + { + MController_set_joint_trq_ref(jxx, setpoint->to.torque.value); + } break; + + case eomc_setpoint_openloop: + { + MController_set_joint_pwm_ref(jxx, setpoint->to.openloop.value); + } break; + + case eomc_setpoint_current: + { + MController_set_joint_cur_ref(jxx, setpoint->to.current.value); + } break; + + default: + { + + } break; + } + + MController_update_joint_targets(jxx); + } + +} // f-marker-end + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_stoptrajectory(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + MController_stop_joint(jxx); + } + +} // fmarker-end + + +// f-marker-begin +static void send_diagnostic_debugmessage(eOerrmanErrorType_t type, eOerror_value_DEB_t value, uint8_t jointnum, uint16_t par16, uint64_t par64, const char* info) +{ + eOerrmanDescriptor_t errdes = {0}; + + errdes.code = eoerror_code_get(eoerror_category_Debug, value); + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = jointnum; + errdes.par16 = par16; + errdes.par64 = par64; + eo_errman_Error(eo_errman_GetHandle(), type, info, NULL, &errdes); +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_calibration(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_calibrator_t *calibrator = (eOmc_calibrator_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_calibrate(jxx, calibrator); + } + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_controlmode(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_controlmode_command_t *controlmode = (eOmc_controlmode_command_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_set_control_mode(jxx, (eOmc_controlmode_command_t)(*controlmode)); + } + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_interactionmode(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_interactionmode_t* interaction = (eOmc_interactionmode_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_set_interaction_mode(jxx, *interaction); + } // marker + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_inputs_externallymeasuredtorque(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmeas_torque_t *torque = (eOmeas_torque_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_update_joint_torque_fbk(jxx, *torque); + } // marker +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_gearbox_M2J(const EOnv* nv, const eOropdescriptor_t* rd) +{ // not for mc4can + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + float32_t *gbxratio = (float32_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_config_motor_gearbox_M2J(jxx, *gbxratio); + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_rotorencoder(const EOnv* nv, const eOropdescriptor_t* rd) +{ // not for mc4can + + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + int32_t *rotenc = (int32_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + MController_config_motor_encoder(jxx, *rotenc); + } +} +// -- entity motor + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_motor_config_t *mconfig = (eOmc_motor_config_t*)rd->data; + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if(eo_motcon_mode_foc == mcmode) + { + eo_motioncontrol_ConfigMotor(eo_motioncontrol_GetHandle(), mxx, mconfig); + } + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_pidcurrent(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if(eo_motcon_mode_foc == mcmode) + { + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + + MController_motor_config_current_PID(mxx, pid); + + return; + } +} + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_pidspeed(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if(eo_motcon_mode_foc == mcmode) + { + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + + MController_motor_config_speed_PID(mxx, pid); + + return; + } +} + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_currentlimits(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + + eOmc_current_limits_params_t *currentLimits = (eOmc_current_limits_params_t*)rd->data; + eOmeas_current_t curr = currentLimits->overloadCurrent; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + // marco.accame on 22 apr 2021: detected ERROR + // it is wrong the way the CAN frame of type ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT is + // managed, both in here and inside MController_motor_config_max_currents(). + // because: + // 1. ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT requires command.value to be a pointer + // to eOmc_current_limits_params_t and in here we have a pointer to eOmc_current_limits_params_t::overloadCurrent + // 2. also MController_motor_config_max_currents() in its internals sends a CAN frame + // of type ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT but in a wrong way because + // it uses eOmc_current_limits_params_t::peakCurrent instead + // luckily, the MController from inside Motor_config_2FOC() sends correctly the + // current limits using the correct argument eOmc_current_limits_params_t. + // so far I prefer just to log this error. + + + // foc-base and mc4based should send overloadCurrent to can-boards + + + if(eo_motcon_mode_foc == mcmode) + { + // send the can message to relevant board + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT; + command.value = &curr; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + MController_motor_config_max_currents(mxx, currentLimits); + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_pwmlimit(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + eOmeas_pwm_t *pwm_limit = (eOmeas_pwm_t *)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode)) + { + *pwm_limit = MController_config_motor_pwm_limit(mxx, *pwm_limit); + } + +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + +// -- common functions + +static eOmotioncontroller_mode_t s_motorcontrol_getmode(void) +{ + return(eo_motioncontrol_GetMode(eo_motioncontrol_GetHandle())); +} + + +// -- 2foc-based functions + + +// -- mc4plus-based funtions + + + +// -- mc4can-based functions + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_fun_ems4rd.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_fun_ems4rd.c new file mode 100644 index 0000000000..ac6430fcf3 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_fun_ems4rd.c @@ -0,0 +1,1753 @@ +/* + * Copyright (C) 2015 iCub Facility - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +/* @file EoProtocolMC_fun_ems4rd.c + @brief This file contains the callback functions used for handling mc eth messages + @author marco.accame@iit.it + @date 05/28/2015 +**/ + + + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "stdlib.h" +#include "string.h" +#include "stdio.h" + +#include "EoCommon.h" +#include "EOnv.h" + +#include "EoMotionControl.h" + + + +#include "EOtheEntities.h" + +#include "EOtheErrorManager.h" +#include "EoError.h" + +#include "EOtheCANservice.h" + + +#include "EOtheBOARDtransceiver.h" + + +#include "EOtheMotionController.h" + + + +// mc4-case +#include "EOproxy.h" +#include "EOtheMC4boards.h" +#include "EOtheVirtualStrain.h" + +// foc-case +#include "Controller.h" + +// mc4lus-case +#include "EOCurrentsWatchdog.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EoProtocolMC.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- + +// remove this if you want to use function eoprot_fun_UPDT_mc_controller_config_jointcoupling() as defined inside this file +#define EOMOTIONCONTROL_DONTREDEFINE_JOINTCOUPLING_CALLBACK + + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- + +typedef enum +{ + pid_typePOS = 0, + pid_typeTOR = 1 +} pid_type_t; + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + +// -- common functions + +static eOmotioncontroller_mode_t s_motorcontrol_getmode(void); +static void send_diagnostic_debugmessage(eOerrmanErrorType_t type, eOerror_value_DEB_t value, uint8_t jointnum, uint16_t par16, uint64_t par64, const char* info); + + +// -- foc-based functions + + +// -- mc4plus-based functions + + +// -- mc4can-based functions + +static void s_onpid(const EOnv* nv, const eOropdescriptor_t* rd, pid_type_t type); +static eOresult_t s_translate_eOmcControlMode2icubCanProtoControlMode(eOmc_controlmode_command_t eomc_controlmode, eOprotIndex_t jId, + icubCanProto_controlmode_t *icubcanProto_controlmode); + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + +// -- entity joint + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_joint_config_t *cfg = (eOmc_joint_config_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_joint_status_t *jstatus = eo_entities_GetJointStatus(eo_entities_GetHandle(), jxx); + + // now we see if it is a mc4can or a 2foc or a mc4plus + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_config_joint(jxx, cfg); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + // first of all: set conversion factor +// EOtheMeasuresConverter* mc4boards = eo_measconv_GetHandle(); +// eo_measconv_SetJntEncoderConversionFactor(mc4boards, jxx, (eOmeasconv_encConversionFactor_t)eo_common_Q17_14_to_float(cfg->encoderconversionfactor)); +// eo_measconv_SetJntEncoderConversionOffset(mc4boards, jxx, (eOmeasconv_encConversionOffset_t)eo_common_Q17_14_to_float(cfg->encoderconversionoffset)); + EOtheMC4boards *mc4boards = eo_mc4boards_GetHandle(); +// eo_mc4boards_Convert_encoderfactor_Set(mc4boards, jxx, (eOmc4boards_conv_encoder_factor_t)eo_common_Q17_14_to_float(cfg->DEPRECATED_encoderconversionfactor)); +// eo_mc4boards_Convert_encoderoffset_Set(mc4boards, jxx, (eOmc4boards_conv_encoder_offset_t)eo_common_Q17_14_to_float(cfg->DEPRECATED_encoderconversionoffset)); + eo_mc4boards_Convert_encoderfactor_Set(mc4boards, jxx, (float)cfg->jntEncoderResolution/65535.0f); + eo_mc4boards_Convert_encoderoffset_Set(mc4boards, jxx, 0); //->>> moved to the calibrators. + + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + + // 1) send pid position + command.type = ICUBCANPROTO_POL_MC_CMD__SET_POS_PID; + command.value = &cfg->pidtrajectory; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_POS_PIDLIMITS; + command.value = &cfg->pidtrajectory; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_POS_STICTION_PARAMS; + command.value = &cfg->pidtrajectory; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + + + // 2) send torque pid + command.type = ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PID; + command.value = &cfg->pidtorque; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PIDLIMITS; + command.value = &cfg->pidtorque; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_STICTION_PARAMS; + command.value = &cfg->pidtrajectory; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + + // 3) send velocity pid: currently is not send: neither MC4 nor 2foc use pid velocity. + + // marco.accame TODO: (tag FIX_CALIB_MC4) manage max and min limits as in new_fw_ems ... + // 4) set max/min limits... they will be used during calibration phase + + eo_mc4boards_Convert_minJointPos_Set(mc4boards, jxx, cfg->userlimits.min); + eo_mc4boards_Convert_maxJointPos_Set(mc4boards, jxx, cfg->userlimits.max); + + // 5) set vel timeout + command.type = ICUBCANPROTO_POL_MC_CMD__SET_VEL_TIMEOUT; + command.value = &cfg->velocitysetpointtimeout; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + + // 6) set impedance + icubCanProto_impedance_t impedence_icubCanProtValues = {0}; +// impedence_icubCanProtValues.stiffness = eo_measconv_impedenceStiffness_I2S(mc4boards, jxx, cfg->impedance.stiffness); +// impedence_icubCanProtValues.damping = eo_mc4boards_Convert_impedanceDamping_I2S(mc4boards, jxx, cfg->impedance.damping); +// impedence_icubCanProtValues.offset = eo_mc4boards_Convert_torque_I2S(mc4boards, jxx, cfg->impedance.offset); + + impedence_icubCanProtValues.stiffness = eo_mc4boards_Convert_impedanceStiffness_I2S(eo_mc4boards_GetHandle(), jxx, cfg->impedance.stiffness); + impedence_icubCanProtValues.damping = eo_mc4boards_Convert_impedanceDamping_I2S(eo_mc4boards_GetHandle(), jxx, cfg->impedance.damping); + impedence_icubCanProtValues.offset = eo_mc4boards_Convert_torque_I2S(eo_mc4boards_GetHandle(), jxx, cfg->impedance.offset); + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_PARAMS; + command.value = &impedence_icubCanProtValues; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_OFFSET; + command.value = &impedence_icubCanProtValues.offset; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + + // max velocity of joint. it MUST be positive + icubCanProto_velocity_t vel_ticks = eo_mc4boards_Convert_Velocity_toCAN(eo_mc4boards_GetHandle(), jxx, cfg->maxvelocityofjoint, eomc4_velocitycontext_toCAN_unsigned); + command.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_VELOCITY; + command.value = &vel_ticks; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + } + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_pidposition(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_config_minjerk_pid(jxx, pid); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + s_onpid(nv, rd, pid_typePOS); + } +} + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_pidvelocity(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + //MController_config_direct_pid(jxx, pid); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + //s_onpid(nv, rd, pid_typePOS); + } +} + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_pidtorque(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + //float rescaler = 1.0f/(float)(1<scale); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_motor_config_torque_PID(jxx, pid); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + s_onpid(nv, rd, pid_typeTOR); + } +} + + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_motor_params(const EOnv* nv, const eOropdescriptor_t* rd) +{ // not for mc4can + eOmc_motor_params_t *mparams = (eOmc_motor_params_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_config_motor_friction(jxx, mparams); + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_impedance(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_impedance_t *impedance = (eOmc_impedance_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_config_joint_impedance(jxx, impedance); + } // mark + else if(eo_motcon_mode_mc4 == mcmode) + { + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + + if(eo_ropcode_set == rd->ropcode) + { + EOtheMC4boards* mc4boards = eo_mc4boards_GetHandle(); + icubCanProto_impedance_t impedence_icubCanProtValues = {0}; + impedence_icubCanProtValues.stiffness = eo_mc4boards_Convert_impedanceStiffness_I2S(mc4boards, jxx, impedance->stiffness); + impedence_icubCanProtValues.damping = eo_mc4boards_Convert_impedanceDamping_I2S(mc4boards, jxx, impedance->damping); + impedence_icubCanProtValues.offset = eo_mc4boards_Convert_torque_I2S(mc4boards, jxx, impedance->offset); + + //char info[200]; + //snprintf(info, sizeof(info), "s=%.4f ss=%d, d=%.4f ds=%d, o=%.4f, os=%d", impedance->stiffness, impedence_icubCanProtValues.stiffness, impedance->damping, impedence_icubCanProtValues.damping, impedance->offset, impedence_icubCanProtValues.offset); + //send_diagnostic_debugmessage(eo_errortype_debug, eoerror_value_DEB_tag01, (uint8_t)jxx, impedence_icubCanProtValues.offset, impedence_icubCanProtValues.damping, info); + + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_PARAMS; + command.value = &impedence_icubCanProtValues; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_OFFSET; + command.value = &impedence_icubCanProtValues.offset; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + return; + } + else if(eo_ropcode_ask == rd->ropcode) + { + eOerrmanDescriptor_t errdes = {0}; + EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); + eOproxy_params_t *param = eo_proxy_Params_Get(proxy, rd->id32); + if(NULL == param) + { + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_callback_fails); + errdes.par16 = 0; + errdes.par64 = ((uint64_t)rd->signature << 32) | (rd->id32); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); + return; + } + param->p08_1 = 2; // we expect two can frames + param->p08_2 = 0; // and we havent received any yet + + command.type = ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_PARAMS; + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_OFFSET; + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + +#if defined(DEBUG_LOG_PROXY_ACTIVITY) + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_ok); + errdes.par16 = (param->p08_2 << 8) | (param->p08_1); + errdes.par64 = rd->id32; + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, NULL, NULL, &errdes); +#endif + return; + } + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_userlimits(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmeas_position_limits_t *limitsofjoint = (eOmeas_position_limits_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_config_joint_pos_limits(jxx, limitsofjoint->min, limitsofjoint->max); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + + if(eo_ropcode_set == rd->ropcode) + { + icubCanProto_position_t minpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(eo_mc4boards_GetHandle(), jxx, limitsofjoint->min); + icubCanProto_position_t maxpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(eo_mc4boards_GetHandle(), jxx, limitsofjoint->max); + + if(maxpos_icubCanProtValue < minpos_icubCanProtValue) + { + //swap min and max + icubCanProto_position_t pos_icubCanProtValue = minpos_icubCanProtValue; + minpos_icubCanProtValue = maxpos_icubCanProtValue; + maxpos_icubCanProtValue = pos_icubCanProtValue; + } + command.type = ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION; + command.value = &minpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION; + command.value = &maxpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + return; + } + else if(eo_ropcode_ask == rd->ropcode) + { + eOerrmanDescriptor_t errdes = {0}; + EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); + eOproxy_params_t *param = eo_proxy_Params_Get(proxy, rd->id32); + if(NULL == param) + { + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_callback_fails); + errdes.par16 = 0; + errdes.par64 = ((uint64_t)rd->signature << 32) | (rd->id32); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); + return; + } + param->p08_1 = 2; // we expect two can frames + param->p08_2 = 0; // and we havent received any yet + + command.type = ICUBCANPROTO_POL_MC_CMD__GET_MIN_POSITION; + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = ICUBCANPROTO_POL_MC_CMD__GET_MAX_POSITION; + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + +#if defined(DEBUG_LOG_PROXY_ACTIVITY) + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_ok); + errdes.par16 = (param->p08_2 << 8) | (param->p08_1); + errdes.par64 = rd->id32; + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, NULL, NULL, &errdes); +#endif + return; + } + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_config_velocitysetpointtimeout(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmeas_time_t *time = (eOmeas_time_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_config_joint_vel_ref_timeout(jxx, *time); + } // marker + else if(eo_motcon_mode_mc4 == mcmode) + { + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_VEL_TIMEOUT; + command.value = time; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_status_core_modes_ismotiondone(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + // do nothing + } // marker + else if(eo_motcon_mode_mc4 == mcmode) + { + if(eo_ropcode_ask == rd->ropcode) + { + // must send a get motion done request to the mc4 board and then ... activate the proxy for the reply to robotInterface + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__MOTION_DONE; + command.value = NULL; + + eOerrmanDescriptor_t errdes = {0}; + EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); + eOproxy_params_t *param = eo_proxy_Params_Get(proxy, rd->id32); + if(NULL == param) + { + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_callback_fails); + errdes.par16 = 0; + errdes.par64 = ((uint64_t)rd->signature << 32) | (rd->id32); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); + return; + } + param->p08_1 = 1; // we expect one can frame + param->p08_2 = 0; // and we havent received any yet + + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + +#if defined(DEBUG_LOG_PROXY_ACTIVITY) + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_ok); + errdes.par16 = (param->p08_2 << 8) | (param->p08_1); + errdes.par64 = rd->id32; + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, NULL, NULL, &errdes); +#endif + return; + } + + } + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_setpoint(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_setpoint_t *setpoint = (eOmc_setpoint_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_joint_t *joint = eo_entities_GetJoint(eo_entities_GetHandle(), jxx); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if(NULL == joint) + { + return; // error... maybe robotInterface sends a command to a joint with index higher than supported + } + + + joint->status.core.modes.ismotiondone = eobool_false; + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + switch(setpoint->type) + { + case eomc_setpoint_position: + { + MController_set_joint_pos_ref(jxx, setpoint->to.position.value, setpoint->to.position.withvelocity); + } break; + + case eomc_setpoint_positionraw: + { + MController_set_joint_pos_raw(jxx, setpoint->to.positionraw.value); + } break; + + case eomc_setpoint_velocity: + { + MController_set_joint_vel_ref(jxx, setpoint->to.velocity.value, setpoint->to.velocity.withacceleration); + //MController_set_joint_vel_raw(jxx, setpoint->to.velocity.value); + } break; + + case eomc_setpoint_torque: + { + MController_set_joint_trq_ref(jxx, setpoint->to.torque.value); + } break; + + case eomc_setpoint_openloop: + { + MController_set_joint_pwm_ref(jxx, setpoint->to.openloop.value); + } break; + + case eomc_setpoint_current: + { + MController_set_joint_cur_ref(jxx, setpoint->to.current.value); + } break; + + default: + { + + } break; + } + + MController_update_joint_targets(jxx); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + EOtheMC4boards *mc4boards = eo_mc4boards_GetHandle(); + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = 0; + command.value = NULL; + + + // marco.accame: i put them in here and not inside the case {} because of scope of the variables ... + icubCanProto_setpoint_position_t setpoint_pos = {0}; + icubCanProto_setpoint_velocity_t setpoint_vel = {0}; + icubCanProto_setpoint_torque_t setpoint_torque = {0}; + icubCanProto_setpoint_openloop_t setpoint_openloop = {0}; + icubCanProto_position_t pos = 0; + + command.type = 0; + + switch(setpoint->type) + { + case eomc_setpoint_position: + { + setpoint_pos.value = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, setpoint->to.position.value); + setpoint_pos.withvelocity = eo_mc4boards_Convert_Velocity_toCAN(mc4boards, jxx, setpoint->to.position.withvelocity, eomc4_velocitycontext_toCAN_positionsetpoint); + + command.type = ICUBCANPROTO_POL_MC_CMD__POSITION_MOVE; + command.value = &setpoint_pos; + + joint->status.target.trgt_position = setpoint->to.position.value; + } break; + + case eomc_setpoint_velocity: + { + setpoint_vel.value = eo_mc4boards_Convert_Velocity_toCAN(mc4boards, jxx, setpoint->to.velocity.value, eomc4_velocitycontext_toCAN_signed); + setpoint_vel.withacceleration = eo_mc4boards_Convert_Acceleration_toCAN(mc4boards, jxx, setpoint->to.velocity.withacceleration); + // acceleration in velocity setpoint must be positive, as it is the velocity which has the sign. moreover: it must be higher that 1 tick/ms^2 + if (setpoint_vel.withacceleration < 1) + { + setpoint_vel.withacceleration = 1; + } + + command.type = ICUBCANPROTO_POL_MC_CMD__VELOCITY_MOVE; + command.value = &setpoint_vel; + + joint->status.target.trgt_velocity = setpoint->to.velocity.value; +//#undef DEBUG_SETPOINT_VELOCITY +//#if defined(DEBUG_SETPOINT_VELOCITY) +// +// eOerrmanDescriptor_t errdes = {0}; +// static eOmeas_acceleration_t prevAcc = -1; +// static eOmeas_velocity_t prevVel = -1; +// +// eObool_t print = eobool_false; +// +// if((prevAcc != setpoint->to.velocity.withacceleration) || (prevVel != setpoint->to.velocity.value)) +// { +// print = eobool_true; +// } +// prevAcc = setpoint->to.velocity.withacceleration; +// prevVel = setpoint->to.velocity.value; +// +// if(eobool_true == print) +// { +// // original +// errdes.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag00); +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = jxx; +// errdes.par16 = 0xfff1; +// errdes.par64 = (setpoint->to.velocity.value) | ((uint64_t)(setpoint->to.velocity.withacceleration)<<32); +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +// } +// +// if(eobool_true == print) +// { +// // conversion +// errdes.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag00); +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = jxx; +// errdes.par16 = 0xfff2; +// errdes.par64 = (setpoint_vel.value) | ((uint64_t)(setpoint_vel.withacceleration)<<32); +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +// } +// +//#endif //defined(DEBUG_SETPOINT_VELOCITY) + + } break; + + case eomc_setpoint_torque: + { + setpoint_torque.value = eo_mc4boards_Convert_torque_I2S(mc4boards,jxx, setpoint->to.torque.value); + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_TORQUE; + command.value = &setpoint_torque; + + joint->status.target.trgt_torque = setpoint->to.torque.value; + } break; + + case eomc_setpoint_openloop: + { + // marco.accame on 27 feb 2015. + // when embObjMotionControl::setRefOutputRaw() sends a set, + // here is what it is done: we send it to mc4 with a openloop params command ICUBCANPROTO_POL_MC_CMD__SET_OPENLOOP_PARAMS. + // then the reading back is done with embObjMotionControl::getRefOutputRaw() which + // just reads its buffered value of joint.status.ofpid.positionreference which in this case + // must contain the value of the openloop current as received and accepted by the mc4 + // board. the mc4 board could refuse to accept the value if for instance is not in openloop mode. + // moreover: the mc4 board does not normally broadcast this value, thus we must ask it + // in an explicit way with a further CAN message of type ICUBCANPROTO_POL_MC_CMD__GET_OPENLOOP_PARAMS + // at this point, in the tx phase the two CAN commands (set and then get) are sent and the + // reply of teh get is received at 1 ms from now in the next rx phase (hopefully). + // at this point the rx handler of the openloop param fills the value of joint.status.ofpid.positionreference + // which is broadcasted up to robotInterface about 1.8 ms after now. + // ... i can smell danger of a race condition if a high level user calls setRefOutputRaw() and then getRefOutputRaw() + // without any wait time in between. + // ... moreover: the getRefOutputRaw() returns the value buffered in EMS and asked to mc4 the last time a setRefOutputRaw() + // was called. + // anyway: for now it works fine and having asked around to people in the lab the thing is not worrying them + // a solution: getRefOutputRaw() does not read the joint.status but directly + // send a ask WHICH IS A NEW TAG TO BE ADDED. + // then we use the proxy so that this request is sent to the mc4 which gives the value and then the value + // is sent up with a say + // in such a way the value is always correct + + setpoint_openloop.value = setpoint->to.openloop.value; + + command.type = ICUBCANPROTO_POL_MC_CMD__SET_OPENLOOP_PARAMS; + command.value = &setpoint_openloop; + + joint->status.target.trgt_pwm = setpoint->to.openloop.value; + } break; + + case eomc_setpoint_positionraw: + { + pos = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, setpoint->to.positionraw.value); + command.type = ICUBCANPROTO_POL_MC_CMD__SET_COMMAND_POSITION; + command.value = &pos; + + joint->status.target.trgt_positionraw = setpoint->to.positionraw.value; + } break; + + default: + { + command.type = 0; + } break; + } + + if(0 == command.type) + { + return; + } + + // send msg + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + + if(eomc_setpoint_openloop == setpoint->type) + { + // since mc4 does not send periodic messages with openloop reference values + // i need to ask it direct to mc4 + command.type = ICUBCANPROTO_POL_MC_CMD__GET_OPENLOOP_PARAMS; + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + } + + } // marker-end-of-case-mc4-can + +} // f-marker-end + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_stoptrajectory(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + MController_stop_joint(jxx); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__STOP_TRAJECTORY; + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + } + +} // fmarker-end + + +// f-marker-begin +static void send_diagnostic_debugmessage(eOerrmanErrorType_t type, eOerror_value_DEB_t value, uint8_t jointnum, uint16_t par16, uint64_t par64, const char* info) +{ + eOerrmanDescriptor_t errdes = {0}; + + errdes.code = eoerror_code_get(eoerror_category_Debug, value); + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = jointnum; + errdes.par16 = par16; + errdes.par64 = par64; + eo_errman_Error(eo_errman_GetHandle(), type, info, NULL, &errdes); +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_calibration(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + eOmc_calibrator_t *calibrator = (eOmc_calibrator_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_calibrate(jxx, calibrator); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + EOtheMC4boards *mc4boards = eo_mc4boards_GetHandle(); + icubCanProto_calibrator_t iCubCanProtCalibrator = { EO_INIT(.type) icubCanProto_calibration_type0_hard_stops, {0}}; + iCubCanProtCalibrator.type = (icubCanProto_calibration_type_t)calibrator->type; + + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__CALIBRATE_ENCODER; + command.value = &iCubCanProtCalibrator; + + eObool_t found = eobool_false; + + switch(calibrator->type) + { + case eomc_calibration_type0_hard_stops: + { + iCubCanProtCalibrator.params.type0.pwmlimit = calibrator->params.type0.pwmlimit; + // iCubCanProtCalibrator.params.type0.velocity = eo_measconv_jntVelocity_toCAN(mc4boards, jxx, calibrator->params.type0.velocity); + iCubCanProtCalibrator.params.type0.velocity = calibrator->params.type0.velocity; + eo_mc4boards_Convert_encoderoffset_Set(mc4boards, jxx, calibrator->params.type0.calibrationZero); + //the following block is repeated beatween all calibration types... it should be revised. + { + icubCanProto_position_t tmp_minpos = eo_mc4boards_Convert_minJointPos_Get(mc4boards, jxx); + icubCanProto_position_t tmp_maxpos = eo_mc4boards_Convert_maxJointPos_Get(mc4boards, jxx); + icubCanProto_position_t minpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_minpos); + icubCanProto_position_t maxpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_maxpos); + if(maxpos_icubCanProtValue < minpos_icubCanProtValue) + { + //swap min and max + icubCanProto_position_t pos_icubCanProtValue = minpos_icubCanProtValue; + minpos_icubCanProtValue = maxpos_icubCanProtValue; + maxpos_icubCanProtValue = pos_icubCanProtValue; + } + eOcanprot_command_t command_limit = {0}; + command_limit.clas = eocanprot_msgclass_pollingMotorControl; + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION; + command_limit.value = &minpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION; + command_limit.value = &maxpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + } + found = eobool_true; + } break; + + case eomc_calibration_type1_abs_sens_analog: + { + // icubCanProto_position_t pos = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, calibrator->params.type1.position); + // /*sice pos param is a word of 16 bits i must check min and max*/ + // if((pos < INT16_MIN)||(pos>INT16_MAX)) + // { + // return; + // } + // iCubCanProtCalibrator.params.type1.position = (icubCanProto_position4calib_t)pos; + // iCubCanProtCalibrator.params.type1.velocity = eo_measconv_jntVelocity_toCAN(mc4boards, jxx, calibrator->params.type1.velocity); + iCubCanProtCalibrator.params.type1.position = calibrator->params.type1.position; + iCubCanProtCalibrator.params.type1.velocity = calibrator->params.type1.velocity; + eo_mc4boards_Convert_encoderoffset_Set(mc4boards, jxx, calibrator->params.type1.calibrationZero); + //the following block is repeated beatween all calibration types... it should be revised. + { + icubCanProto_position_t tmp_minpos = eo_mc4boards_Convert_minJointPos_Get(mc4boards, jxx); + icubCanProto_position_t tmp_maxpos = eo_mc4boards_Convert_maxJointPos_Get(mc4boards, jxx); + icubCanProto_position_t minpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_minpos); + icubCanProto_position_t maxpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_maxpos); + if(maxpos_icubCanProtValue < minpos_icubCanProtValue) + { + //swap min and max + icubCanProto_position_t pos_icubCanProtValue = minpos_icubCanProtValue; + minpos_icubCanProtValue = maxpos_icubCanProtValue; + maxpos_icubCanProtValue = pos_icubCanProtValue; + } + eOcanprot_command_t command_limit = {0}; + command_limit.clas = eocanprot_msgclass_pollingMotorControl; + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION; + command_limit.value = &minpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION; + command_limit.value = &maxpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + } + found = eobool_true; + } break; + + case eomc_calibration_type2_hard_stops_diff: + { + iCubCanProtCalibrator.params.type2.pwmlimit = calibrator->params.type2.pwmlimit; + // iCubCanProtCalibrator.params.type2.velocity = eo_measconv_jntVelocity_toCAN(mc4boards, jxx, calibrator->params.type2.velocity); + iCubCanProtCalibrator.params.type2.velocity = calibrator->params.type2.velocity; + eo_mc4boards_Convert_encoderoffset_Set(mc4boards, jxx, calibrator->params.type2.calibrationZero); + //the following block is repeated beatween all calibration types... it should be revised. + { + icubCanProto_position_t tmp_minpos = eo_mc4boards_Convert_minJointPos_Get(mc4boards, jxx); + icubCanProto_position_t tmp_maxpos = eo_mc4boards_Convert_maxJointPos_Get(mc4boards, jxx); + icubCanProto_position_t minpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_minpos); + icubCanProto_position_t maxpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_maxpos); + if(maxpos_icubCanProtValue < minpos_icubCanProtValue) + { + //swap min and max + icubCanProto_position_t pos_icubCanProtValue = minpos_icubCanProtValue; + minpos_icubCanProtValue = maxpos_icubCanProtValue; + maxpos_icubCanProtValue = pos_icubCanProtValue; + } + eOcanprot_command_t command_limit = {0}; + command_limit.clas = eocanprot_msgclass_pollingMotorControl; + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION; + command_limit.value = &minpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION; + command_limit.value = &maxpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + } + found = eobool_true; + } break; + + case eomc_calibration_type3_abs_sens_digital: + { + // icubCanProto_position_t pos = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, calibrator->params.type3.position); + // /*sice pos param is a word of 16 bits i must check min and max*/ + // if((pos < INT16_MIN)||(pos>INT16_MAX)) + // { + // return; + // } + // iCubCanProtCalibrator.params.type1.position = (icubCanProto_position4calib_t)pos; + // iCubCanProtCalibrator.params.type3.velocity = eo_measconv_jntVelocity_toCAN(mc4boards, jxx, calibrator->params.type3.velocity); + iCubCanProtCalibrator.params.type1.position = calibrator->params.type3.position; + iCubCanProtCalibrator.params.type3.velocity = calibrator->params.type3.velocity; + iCubCanProtCalibrator.params.type3.offset = calibrator->params.type3.offset; + eo_mc4boards_Convert_encoderoffset_Set(mc4boards, jxx, calibrator->params.type3.calibrationZero); + //the following block is repeated beatween all calibration types... it should be revised. + { + icubCanProto_position_t tmp_minpos = eo_mc4boards_Convert_minJointPos_Get(mc4boards, jxx); + icubCanProto_position_t tmp_maxpos = eo_mc4boards_Convert_maxJointPos_Get(mc4boards, jxx); + icubCanProto_position_t minpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_minpos); + icubCanProto_position_t maxpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_maxpos); + if(maxpos_icubCanProtValue < minpos_icubCanProtValue) + { + //swap min and max + icubCanProto_position_t pos_icubCanProtValue = minpos_icubCanProtValue; + minpos_icubCanProtValue = maxpos_icubCanProtValue; + maxpos_icubCanProtValue = pos_icubCanProtValue; + } + eOcanprot_command_t command_limit = {0}; + command_limit.clas = eocanprot_msgclass_pollingMotorControl; + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION; + command_limit.value = &minpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION; + command_limit.value = &maxpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + } + found = eobool_true; + } break; + + case eomc_calibration_type6_mais: + { + float computed_zero=0; + float computed_encoder=0; + + //compute the encoder conversion + { + char info [50]; + + icubCanProto_position_t tmp_min_joint_pos = eo_mc4boards_Convert_minJointPos_Get(mc4boards, jxx); + icubCanProto_position_t tmp_max_joint_pos = eo_mc4boards_Convert_maxJointPos_Get(mc4boards, jxx); + //icubCanProto_position_t tmp_min_motor_pos = eo_mc4boards_Convert_minMotorPos_Get(mc4boards, jxx); + //icubCanProto_position_t tmp_max_motor_pos = eo_mc4boards_Convert_maxMotorPos_Get(mc4boards, jxx); + + if (calibrator->params.type6.current==1) + { + iCubCanProtCalibrator.params.type6.position= calibrator->params.type6.vmax; + } + else if (calibrator->params.type6.current==-1) + { + iCubCanProtCalibrator.params.type6.position= calibrator->params.type6.vmin; + } + else + { + sprintf(info,"error type6.current=%d",calibrator->params.type6.current); + send_diagnostic_debugmessage(eo_errortype_debug, eoerror_value_DEB_tag01, jxx, 0, 0, info); + iCubCanProtCalibrator.params.type6.position=0; + } + + //sprintf(info,"type6 pos%d curr%d %d %d",iCubCanProtCalibrator.params.type6.position,calibrator->params.type6.current,tmp_max_motor_pos,tmp_min_motor_pos); + //send_diagnostic_debugmessage(eo_errortype_debug, eoerror_value_DEB_tag01, jxx, 0, 0, info); + iCubCanProtCalibrator.params.type6.velocity = calibrator->params.type6.velocity; + + computed_encoder = (float)(calibrator->params.type6.vmax - calibrator->params.type6.vmin) / (float)(tmp_max_joint_pos - tmp_min_joint_pos); + + //sprintf(info,"vmin %d vmax %d enc %f",calibrator->params.type6.vmin, calibrator->params.type6.vmax,computed_encoder); + //send_diagnostic_debugmessage(eo_errortype_debug, eoerror_value_DEB_tag01, jxx, 0, 0, info); + //sprintf(info,"calicom %d",iCubCanProtCalibrator.params.type6.velocity); + //send_diagnostic_debugmessage(eo_errortype_debug, eoerror_value_DEB_tag01, jxx, 0, 0, info); + + computed_zero = - (float)(tmp_min_joint_pos) + ((float)(calibrator->params.type6.vmin) / computed_encoder); + + //sprintf(info,"min %d max %d zero %f",tmp_min_motor_pos, tmp_max_motor_pos, computed_zero); + //send_diagnostic_debugmessage(eo_errortype_debug, eoerror_value_DEB_tag00, jxx, 0, 0, info); + + eo_mc4boards_Convert_encoderoffset_Set(mc4boards, jxx, calibrator->params.type6.calibrationZero+computed_zero); + eo_mc4boards_Convert_encoderfactor_Set(mc4boards, jxx, computed_encoder); + + icubCanProto_position_t min_joint_pos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_min_joint_pos); + icubCanProto_position_t max_joint_pos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_max_joint_pos); + if(max_joint_pos_icubCanProtValue < min_joint_pos_icubCanProtValue) + { + //swap min and max + icubCanProto_position_t pos_icubCanProtValue = min_joint_pos_icubCanProtValue; + min_joint_pos_icubCanProtValue = max_joint_pos_icubCanProtValue; + max_joint_pos_icubCanProtValue = pos_icubCanProtValue; + } + eOcanprot_command_t command_limit = {0}; + command_limit.clas = eocanprot_msgclass_pollingMotorControl; + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION; + command_limit.value = &min_joint_pos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION; + command_limit.value = &max_joint_pos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + } + found = eobool_true; + } break; + + case eomc_calibration_type7_hall_sensor: + { + float computed_zero=0; + float computed_encoder=0; + + iCubCanProtCalibrator.params.type7.position = calibrator->params.type7.position; + iCubCanProtCalibrator.params.type7.velocity = calibrator->params.type7.velocity; + iCubCanProtCalibrator.params.type7.reserved = 0; //fixed to zero + + //compute the encoder conversion + { + //char info [50]; + + icubCanProto_position_t tmp_min_joint_pos = eo_mc4boards_Convert_minJointPos_Get(mc4boards, jxx); + icubCanProto_position_t tmp_max_joint_pos = eo_mc4boards_Convert_maxJointPos_Get(mc4boards, jxx); + icubCanProto_position_t tmp_min_motor_pos = eo_mc4boards_Convert_minMotorPos_Get(mc4boards, jxx); + icubCanProto_position_t tmp_max_motor_pos = eo_mc4boards_Convert_maxMotorPos_Get(mc4boards, jxx); + computed_encoder = (float)(calibrator->params.type7.vmax - calibrator->params.type7.vmin) / (float)(tmp_max_joint_pos - tmp_min_joint_pos); + + //sprintf(info,"vmin %d vmax %d enc %f",calibrator->params.type7.vmin, calibrator->params.type7.vmax,computed_encoder); + //send_diagnostic_debugmessage(eo_errortype_debug, eoerror_value_DEB_tag02, jxx, 0, 0, info); + + computed_zero = - (float)(tmp_min_joint_pos) + ((float)(calibrator->params.type7.vmin) / computed_encoder); + + //sprintf(info,"min %d max %d zero %f",tmp_min_motor_pos, tmp_max_motor_pos, computed_zero); + //send_diagnostic_debugmessage(eo_errortype_debug, eoerror_value_DEB_tag03, jxx, 0, 0, info); + + eo_mc4boards_Convert_encoderoffset_Set(mc4boards, jxx, calibrator->params.type7.calibrationZero+computed_zero); + eo_mc4boards_Convert_encoderfactor_Set(mc4boards, jxx, computed_encoder); + + icubCanProto_position_t minpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_min_joint_pos); + icubCanProto_position_t maxpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_max_joint_pos); + if(maxpos_icubCanProtValue < minpos_icubCanProtValue) + { + //swap min and max + icubCanProto_position_t pos_icubCanProtValue = minpos_icubCanProtValue; + minpos_icubCanProtValue = maxpos_icubCanProtValue; + maxpos_icubCanProtValue = pos_icubCanProtValue; + } + eOcanprot_command_t command_limit = {0}; + command_limit.clas = eocanprot_msgclass_pollingMotorControl; + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION; + command_limit.value = &minpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION; + command_limit.value = &maxpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + } + found = eobool_true; + } break; + + case eomc_calibration_type4_abs_and_incremental: + { + // icubCanProto_position_t pos = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, calibrator->params.type4.position); + // //here position is in int16_t ==> so i must verify if pos is out of int16_t range + // if((pos > INT16_MAX) || (pos < INT16_MIN)) + // { + // return; + // #warning VALE --> how to manage this error??? + // } + // iCubCanProtCalibrator.params.type4.position = (icubCanProto_position4calib_t)pos; + // iCubCanProtCalibrator.params.type4.velocity = eo_measconv_jntVelocity_toCAN(mc4boards, jxx, calibrator->params.type4.velocity); + iCubCanProtCalibrator.params.type4.position = calibrator->params.type4.position; + iCubCanProtCalibrator.params.type4.velocity = calibrator->params.type4.velocity; + iCubCanProtCalibrator.params.type4.maxencoder = calibrator->params.type4.maxencoder; + eo_mc4boards_Convert_encoderoffset_Set(mc4boards, jxx, calibrator->params.type4.calibrationZero); + //the following block is repeated beatween all calibration types... it should be revised. + { + icubCanProto_position_t tmp_minpos = eo_mc4boards_Convert_minJointPos_Get(mc4boards, jxx); + icubCanProto_position_t tmp_maxpos = eo_mc4boards_Convert_maxJointPos_Get(mc4boards, jxx); + icubCanProto_position_t minpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_minpos); + icubCanProto_position_t maxpos_icubCanProtValue = eo_mc4boards_Convert_Position_toCAN(mc4boards, jxx, tmp_maxpos); + if(maxpos_icubCanProtValue < minpos_icubCanProtValue) + { + //swap min and max + icubCanProto_position_t pos_icubCanProtValue = minpos_icubCanProtValue; + minpos_icubCanProtValue = maxpos_icubCanProtValue; + maxpos_icubCanProtValue = pos_icubCanProtValue; + } + eOcanprot_command_t command_limit = {0}; + command_limit.clas = eocanprot_msgclass_pollingMotorControl; + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION; + command_limit.value = &minpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + + command_limit.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION; + command_limit.value = &maxpos_icubCanProtValue; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command_limit, rd->id32); + } + found = eobool_true; + } break; + + default: + { + found = eobool_false; + } break; + } + + if(eobool_false == found) + { + return; + } + + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + } + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_controlmode(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_controlmode_command_t *controlmode = (eOmc_controlmode_command_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_set_control_mode(jxx, (eOmc_controlmode_command_t)(*controlmode)); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + icubCanProto_controlmode_t icubcanProto_controlmode = icubCanProto_controlmode_idle; + if(eores_OK != s_translate_eOmcControlMode2icubCanProtoControlMode(*controlmode, jxx, &icubcanProto_controlmode)) + { + return; + } + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE; + command.value = &icubcanProto_controlmode; + + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_cmmnds_interactionmode(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_interactionmode_t* interaction = (eOmc_interactionmode_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_set_interaction_mode(jxx, *interaction); + } // marker + else if(eo_motcon_mode_mc4 == mcmode) + { + icubCanProto_interactionmode_t icub_interactionmode = icubCanProto_interactionmode_unknownError; + + if(eOmc_interactionmode_stiff == *interaction) + { + icub_interactionmode = icubCanProto_interactionmode_stiff; + } + else if(eOmc_interactionmode_compliant == *interaction) + { + icub_interactionmode = icubCanProto_interactionmode_compliant; + } + else + { + return; + } + + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_INTERACTION_MODE; + command.value = &icub_interactionmode; + + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_joint_inputs_externallymeasuredtorque(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmeas_torque_t *torque = (eOmeas_torque_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_update_joint_torque_fbk(jxx, *torque); + } // marker + else if(eo_motcon_mode_mc4 == mcmode) + { + eOmeas_torque_t trq = eo_mc4boards_Convert_torque_I2S(eo_mc4boards_GetHandle(), jxx, *torque); + + icubCanProto_torque_t icub_torque = trq + 0x8000; //to simulate value of strain + eo_virtualstrain_SetTorque(eo_virtualstrain_GetHandle(), jxx, icub_torque); + + //update value to send back like feedback: it is equal to received input, but limited to 2Nm, because on lower arm joints the max torque possible is 2 Nm + //The feedback is read by ITorqueControl::getTorque(..) function + eOmc_joint_status_t *jstatus = NULL; + if(NULL != (jstatus = eo_entities_GetJointStatus(eo_entities_GetHandle(), jxx))) + { + jstatus->core.measures.meas_torque = eo_mc4boards_Convert_torque_S2I(eo_mc4boards_GetHandle(), jxx, trq); + } + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_gearbox_M2J(const EOnv* nv, const eOropdescriptor_t* rd) +{ // not for mc4can + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + float32_t *gbxratio = (float32_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_config_motor_gearbox_M2J(jxx, *gbxratio); + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_rotorencoder(const EOnv* nv, const eOropdescriptor_t* rd) +{ // not for mc4can + + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + int32_t *rotenc = (int32_t*)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + MController_config_motor_encoder(jxx, *rotenc); + } +} +// -- entity motor + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmc_motor_config_t *mconfig = (eOmc_motor_config_t*)rd->data; + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + + if(eo_motcon_mode_foc == mcmode) + { + eo_motioncontrol_ConfigMotor(eo_motioncontrol_GetHandle(), mxx, mconfig); + return; + } + else if((eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + eo_motioncontrol_ConfigMotor(eo_motioncontrol_GetHandle(), mxx, mconfig); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + //char info [50]; + EOtheMC4boards *mc4boards = eo_mc4boards_GetHandle(); + + eo_mc4boards_Convert_minMotorPos_Set(mc4boards, mxx, mconfig->limitsofrotor.min); + eo_mc4boards_Convert_maxMotorPos_Set(mc4boards, mxx, mconfig->limitsofrotor.max); + + // marco.accame on 8oct2015: removed because it is wrong. message ICUBCANPROTO_POL_MC_CMD__SET_MAX_VELOCITY must be used with max velocity of JOINT !!! + // // set max velocity + // icubCanProto_velocity_t vel_icubCanProtValue = eo_mc4boards_Convert_Velocity_toCAN(eo_mc4boards_GetHandle(), mxx, mconfig->maxvelocityofmotor); + // command.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_VELOCITY; + // command.value = &vel_icubCanProtValue; + // eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + // set current limit + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT; + command.value = &mconfig->currentLimits.overloadCurrent ; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + // set max motor encoder limit + command.type = ICUBCANPROTO_POL_MC_CMD__SET_MAX_MOTOR_POS; + command.value = &mconfig->limitsofrotor.max; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + // set min motor encoder limit + command.type = ICUBCANPROTO_POL_MC_CMD__SET_MIN_MOTOR_POS; + command.value = &mconfig->limitsofrotor.min; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + //set pwmlimit + icubCanProto_pwm_t icubcan_pwm = mconfig->pwmLimit; //eOmeas_pwm_t and icubCanProto_pwm_t are defined like int16 + command.type = ICUBCANPROTO_POL_MC_CMD__SET_PWM_LIMIT; + command.value = &icubcan_pwm; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + //sprintf(info,"motomax %d motomin %d",mconfig->limitsofrotor.max,mconfig->limitsofrotor.min); + //send_diagnostic_debugmessage(eo_errortype_debug, eoerror_value_DEB_tag01, mxx, 0, 0, info); + } + +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_pidcurrent(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if(eo_motcon_mode_foc == mcmode) + { + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + + MController_motor_config_current_PID(mxx, pid); + + return; + } + else if((eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + // TODOALE + return; + } + else if(eo_motcon_mode_mc4 == mcmode) + { + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + + // send current pid + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PID; + command.value = pid; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + // send current pid limits + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PIDLIMITS; + command.value = pid; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + } + +} + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_pidspeed(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if(eo_motcon_mode_foc == mcmode) + { + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + + MController_motor_config_speed_PID(mxx, pid); + + return; + } + else if((eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode)) + { + // TODOALE + return; + } + else if(eo_motcon_mode_mc4 == mcmode) + { + return; + } + +} + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_currentlimits(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + + eOmc_current_limits_params_t *currentLimits = (eOmc_current_limits_params_t*)rd->data; + eOmeas_current_t curr = currentLimits->overloadCurrent; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + // marco.accame on 22 apr 2021: detected ERROR + // it is wrong the way the CAN frame of type ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT is + // managed, both in here and inside MController_motor_config_max_currents(). + // because: + // 1. ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT requires command.value to be a pointer + // to eOmc_current_limits_params_t and in here we have a pointer to eOmc_current_limits_params_t::overloadCurrent + // 2. also MController_motor_config_max_currents() in its internals sends a CAN frame + // of type ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT but in a wrong way because + // it uses eOmc_current_limits_params_t::peakCurrent instead + // luckily, the MController from inside Motor_config_2FOC() sends correctly the + // current limits using the correct argument eOmc_current_limits_params_t. + // so far I prefer just to log this error. + + + // foc-base and mc4based should send overloadCurrent to can-boards + + + if(eo_motcon_mode_foc == mcmode) + { + // send the can message to relevant board + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT; + command.value = &curr; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + MController_motor_config_max_currents(mxx, currentLimits); + } + else if((eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || (eo_motcon_mode_mc4plusfaps == mcmode)) + { + eo_currents_watchdog_UpdateCurrentLimits( eo_currents_watchdog_GetHandle(), mxx); + // now for the case of mc4plus, the watchdog updates the current limits inside ems controller. + // we should re-evaluate the situation after refactoring of mc-controller + } + else if(eo_motcon_mode_mc4pluspmc == mcmode) + { + eo_currents_watchdog_UpdateCurrentLimits( eo_currents_watchdog_GetHandle(), mxx); + // marco.accame on 22 apr 2021: ERROR + // MController_motor_config_max_currents() does not form correctly the CAN frame because it uses + // eOmc_current_limits_params_t::peakCurrent instead + MController_motor_config_max_currents(mxx, currentLimits); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + // just send the can message to relevant board + // send the can message to relevant board + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT; + command.value = &curr; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + } +} + + +// f-marker-begin +extern void eoprot_fun_UPDT_mc_motor_config_pwmlimit(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOprotIndex_t mxx = eoprot_ID2index(rd->id32); + eOmeas_pwm_t *pwm_limit = (eOmeas_pwm_t *)rd->data; + + eOmotioncontroller_mode_t mcmode = s_motorcontrol_getmode(); + + if((eo_motcon_mode_foc == mcmode) || (eo_motcon_mode_mc4plus == mcmode) || (eo_motcon_mode_mc4plusmais == mcmode) || (eo_motcon_mode_mc2pluspsc == mcmode) || + (eo_motcon_mode_mc4plusfaps == mcmode) || (eo_motcon_mode_mc4pluspmc == mcmode)) + { + *pwm_limit = MController_config_motor_pwm_limit(mxx, *pwm_limit); + } + else if(eo_motcon_mode_mc4 == mcmode) + { + if(eo_ropcode_set == rd->ropcode) + { + icubCanProto_pwm_t icubcan_pwm = *pwm_limit; //eOmeas_pwm_t and icubCanProto_pwm_t are defined like int16 + + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + command.type = ICUBCANPROTO_POL_MC_CMD__SET_PWM_LIMIT; + command.value = &icubcan_pwm; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + } + else if(eo_ropcode_ask == rd->ropcode) + { + eOerrmanDescriptor_t errdes = {0}; + EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); + eOproxy_params_t *param = eo_proxy_Params_Get(proxy, rd->id32); + eOcanprot_command_t command = {0}; + if(NULL == param) + { + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_callback_fails); + errdes.par16 = 0; + errdes.par64 = ((uint64_t)rd->signature << 32) | (rd->id32); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); + return; + } + param->p08_1 = 1; // we expect one can frames + param->p08_2 = 0; // and we havent received any yet + + command.type = ICUBCANPROTO_POL_MC_CMD__GET_PWM_LIMIT; + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + +#if defined(DEBUG_LOG_PROXY_ACTIVITY) + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_ok); + errdes.par16 = (param->p08_2 << 8) | (param->p08_1); + errdes.par64 = rd->id32; + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, NULL, NULL, &errdes); +#endif + } + } + +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + +// -- common functions + +static eOmotioncontroller_mode_t s_motorcontrol_getmode(void) +{ + return(eo_motioncontrol_GetMode(eo_motioncontrol_GetHandle())); +} + + +// -- 2foc-based functions + + +// -- mc4plus-based funtions + + + +// -- mc4can-based functions + +static eOresult_t s_translate_eOmcControlMode2icubCanProtoControlMode(eOmc_controlmode_command_t eomc_controlmode, eOprotIndex_t jId, + icubCanProto_controlmode_t *icubcanProto_controlmode) +{ + eOresult_t res = eores_OK; + + switch(eomc_controlmode) + { + case eomc_controlmode_cmd_position: + { + *icubcanProto_controlmode = icubCanProto_controlmode_position; + } break; + case eomc_controlmode_cmd_velocity: + { + *icubcanProto_controlmode = icubCanProto_controlmode_velocity; + } break; + case eomc_controlmode_cmd_torque: + { + *icubcanProto_controlmode = icubCanProto_controlmode_torque; + } break; + case eomc_controlmode_cmd_impedance_pos: + { + *icubcanProto_controlmode = icubCanProto_controlmode_impedance_pos; + } break; + case eomc_controlmode_cmd_impedance_vel: + { + *icubcanProto_controlmode = icubCanProto_controlmode_impedance_vel; + } break; + case eomc_controlmode_cmd_current: + { + *icubcanProto_controlmode = icubCanProto_controlmode_current; + } break; + case eomc_controlmode_cmd_openloop: + { + *icubcanProto_controlmode = icubCanProto_controlmode_openloop; + } break; + case eomc_controlmode_cmd_switch_everything_off: + { + res = eores_NOK_generic; + } break; + case eomc_controlmode_cmd_force_idle: + { + *icubcanProto_controlmode = icubCanProto_controlmode_forceIdle; + } break; + case eomc_controlmode_cmd_mixed: + case eomc_controlmode_cmd_velocity_pos: + { + *icubcanProto_controlmode = icubCanProto_controlmode_mixed; + } break; + case eomc_controlmode_cmd_direct: + { + *icubcanProto_controlmode = icubCanProto_controlmode_direct; + } break; + case eomc_controlmode_cmd_idle: + { + *icubcanProto_controlmode = icubCanProto_controlmode_idle; + } break; + default: + { + res = eores_NOK_generic; + } break; + + } + return(res); +} + + +// type is: 0 pos, 1 torque +static void s_onpid(const EOnv* nv, const eOropdescriptor_t* rd, pid_type_t type) +{ + static const uint8_t cmd_set_pid[2] = {ICUBCANPROTO_POL_MC_CMD__SET_POS_PID, ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PID}; + static const uint8_t cmd_set_pidlimits[2] = {ICUBCANPROTO_POL_MC_CMD__SET_POS_PIDLIMITS, ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PIDLIMITS}; + static const uint8_t cmd_get_pid[2] = {ICUBCANPROTO_POL_MC_CMD__GET_POS_PID, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PID}; + static const uint8_t cmd_get_pidlimits[2] = {ICUBCANPROTO_POL_MC_CMD__GET_POS_PIDLIMITS, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PIDLIMITS}; + static const uint8_t cmd_set_stiction[2] = {ICUBCANPROTO_POL_MC_CMD__SET_POS_STICTION_PARAMS, ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_STICTION_PARAMS}; + static const uint8_t cmd_get_stiction[2] = {ICUBCANPROTO_POL_MC_CMD__GET_POS_STICTION_PARAMS, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS}; + + eOmc_PID_t *pid = (eOmc_PID_t*)rd->data; + eOprotIndex_t jxx = eoprot_ID2index(rd->id32); + + eOcanprot_command_t command = {0}; + command.clas = eocanprot_msgclass_pollingMotorControl; + + // if set, we just send two commands + if(eo_ropcode_set == rd->ropcode) + { + command.type = cmd_set_pid[type]; // ICUBCANPROTO_POL_MC_CMD__SET_POS_PID or .. + command.value = pid; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = cmd_set_pidlimits[type]; // ICUBCANPROTO_POL_MC_CMD__SET_POS_PIDLIMITS or .. + command.value = pid; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = cmd_set_stiction[type]; // ICUBCANPROTO_POL_MC_CMD__SET_POS_STICTION_PARAMS or .. + command.value = pid; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + return; + } + // if ask and we have the proxy, we ask to can about two values + else if(eo_ropcode_ask == rd->ropcode) + { + eOerrmanDescriptor_t errdes = {0}; + EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); + eOproxy_params_t *param = eo_proxy_Params_Get(proxy, rd->id32); + if(NULL == param) + { + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_callback_fails); + errdes.par16 = 0; + errdes.par64 = ((uint64_t)rd->signature << 32) | (rd->id32); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); + return; + } + param->p08_1 = 3; // we expect three can frames + param->p08_2 = 0; // and we havent received any yet + + command.type = cmd_get_pid[type]; // ICUBCANPROTO_POL_MC_CMD__GET_POS_PID or .. + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = cmd_get_pidlimits[type]; // ICUBCANPROTO_POL_MC_CMD__GET_POS_PIDLIMITS or .. + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + + command.type = cmd_get_stiction[type]; // ICUBCANPROTO_POL_MC_CMD__GET_POS_STICTION_PARAMS or .. + command.value = NULL; + eo_canserv_SendCommandToEntity(eo_canserv_GetHandle(), &command, rd->id32); + +#if defined(DEBUG_LOG_PROXY_ACTIVITY) + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_forward_ok); + errdes.par16 = (param->p08_2 << 8) | (param->p08_1); + errdes.par64 = rd->id32; + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, NULL, NULL, &errdes); +#endif + return; + } + +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_overridden_fun.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_overridden_fun.h new file mode 100644 index 0000000000..f53bbe9b81 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMC_overridden_fun.h @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef _EOPROTOCOLMC_OVERRIDDEN_FUN_H_ +#define _EOPROTOCOLMC_OVERRIDDEN_FUN_H_ + + + + +/** @file EoProtocolMC_overridden_fun.h + @brief This header file specifies which functions are overridden in the management of motion control + @author marco.accame@iit.it + @date 06/06/2013 +**/ + +/** @defgroup eo_asfdgr1231 Functions overridden in motion control endpoint + edfefle. + + @{ + **/ + + + +// - external dependencies -------------------------------------------------------------------------------------------- +// empty-section + + + + +// - public #define -------------------------------------------------------------------------------------------------- + +//#define OVERRIDE_eoprot_fun_INIT_mc_joint_config + + +// - declaration of public user-defined types ------------------------------------------------------------------------- +// empty-section + + +// - declaration of extern public variables, ... but better using use _get/_set instead ------------------------------- +// empty-section + +// - declaration of extern public functions --------------------------------------------------------------------------- +// empty-section + + + + +/** @} + end of group eo_asfdgr1231 + **/ + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_fun_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_fun_amc.c new file mode 100644 index 0000000000..c6e67cb7c9 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_fun_amc.c @@ -0,0 +1,1002 @@ + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "stdlib.h" +#include "string.h" +#include "stdio.h" + +#include "EoManagement.h" +#include "EOnv_hid.h" + +#include "EOtheBOARDtransceiver.h" +#include "EOtheErrorManager.h" +#include "EoError.h" +#include "eEsharedServices.h" +#include "EOVtheSystem.h" + +#include "EOMtheEMStransceiver.h" +#include "EOMtheEMSrunner.h" + +#if defined(USE_EMBOT_theServices) +#include "embot_app_eth_theServices.h" +#else +#include "EOtheServices.h" +#endif + +#if defined(USE_EMBOT_theHandler) +#include "theApplication_config.h" +#include "theHandler_Config.h" +#include "embot_app_eth_theHandler.h" +#else +#include "EOMtheEMSappl.h" +#include "EOMtheEMSapplCfg.h" +#endif + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EoProtocolMN.h" + + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- + + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static void s_eoprot_ep_mn_fun_apply_config(uint32_t period, uint16_t rxtime, uint16_t dotime, uint16_t txtime, uint8_t txratedivider); + +static void s_eoprot_ep_mn_fun_apply_config_txratedivider(uint8_t txratedivider); + + +static void s_eoprot_ep_mn_fun_configcommand(eOmn_command_t* command); + +static void s_eoprot_ep_mn_fun_querynumofcommand(eOmn_command_t* command); +static void s_eoprot_ep_mn_fun_queryarraycommand(eOmn_command_t* command); + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + +extern void eoprot_fun_INIT_mn_comm_status(const EOnv* nv) +{ + eOmn_comm_status_t* status = (eOmn_comm_status_t*)nv->ram; + + // 1. init the management protocol version + + eOversion_t* version = &status->managementprotocolversion; + + version->major = eoprot_version_mn_major; + version->minor = eoprot_version_mn_minor; + + + // 2. init the transceiver + + eOmn_transceiver_properties_t* transp = &status->transceiver; + +#if defined(USE_EMBOT_theHandler) + transp->listeningPort = embot::app::eth::theHandler_EOMtheEMSsocket_Config.localport; + transp->destinationPort = embot::app::eth::theHandler_EOMtheEMStransceiver_Config.hostipv4port; + transp->maxsizeRXpacket = embot::app::eth::theHandler_EOMtheEMSsocket_Config.inpdatagramsizeof; + transp->maxsizeTXpacket = embot::app::eth::theHandler_EOMtheEMSsocket_Config.outdatagramsizeof; + transp->maxsizeROPframeRegulars = embot::app::eth::theHandler_EOMtheEMStransceiver_Config.sizes.capacityofropframeregulars; + transp->maxsizeROPframeReplies = embot::app::eth::theHandler_EOMtheEMStransceiver_Config.sizes.capacityofropframereplies; + transp->maxsizeROPframeOccasionals = embot::app::eth::theHandler_EOMtheEMStransceiver_Config.sizes.capacityofropframeoccasionals; + transp->maxsizeROP = embot::app::eth::theHandler_EOMtheEMStransceiver_Config.sizes.capacityofrop; + transp->maxnumberRegularROPs = embot::app::eth::theHandler_EOMtheEMStransceiver_Config.sizes.maxnumberofregularrops; + memset(transp->filler06, 0, sizeof(transp->filler06)); +#else + EOMtheEMSapplCfg* emscfg = eom_emsapplcfg_GetHandle(); + + transp->listeningPort = emscfg->socketcfg.localport; + transp->destinationPort = emscfg->transcfg.hostipv4port; + transp->maxsizeRXpacket = emscfg->socketcfg.inpdatagramsizeof; + transp->maxsizeTXpacket = emscfg->socketcfg.outdatagramsizeof; + transp->maxsizeROPframeRegulars = emscfg->transcfg.sizes.capacityofropframeregulars; + transp->maxsizeROPframeReplies = emscfg->transcfg.sizes.capacityofropframereplies; + transp->maxsizeROPframeOccasionals = emscfg->transcfg.sizes.capacityofropframeoccasionals; + transp->maxsizeROP = emscfg->transcfg.sizes.capacityofrop; + transp->maxnumberRegularROPs = emscfg->transcfg.sizes.maxnumberofregularrops; + memset(transp->filler06, 0, sizeof(transp->filler06)); +#endif +} + + + +extern void eoprot_fun_UPDT_mn_comm_cmmnds_command_querynumof(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmn_command_t* command = (eOmn_command_t*)nv->ram; + eOmn_opc_t opc = (eOmn_opc_t)command->cmd.opc; + + if(eobool_true == eo_nv_hid_isLocal(nv)) + { // function is called from within the local board + + switch(opc) + { + case eomn_opc_query_numof_EPs: + case eomn_opc_query_numof_ENs: + case eomn_opc_query_numof_REGROPs: + { + s_eoprot_ep_mn_fun_querynumofcommand(command); + } break; + + default: + { + } break; + + } + } + else + { // function is called from within the remote host because it has received a say or a sig + } + +} + +extern void eoprot_fun_UPDT_mn_comm_cmmnds_command_queryarray(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmn_command_t* command = (eOmn_command_t*)nv->ram; + eOmn_opc_t opc = (eOmn_opc_t)command->cmd.opc; + + if(eobool_true == eo_nv_hid_isLocal(nv)) + { // function is called from within the local board + switch(opc) + { + case eomn_opc_query_array_EPs: + case eomn_opc_query_array_EPdes: + case eomn_opc_query_array_ENdes: + case eomn_opc_query_array_REGROPs: + { + s_eoprot_ep_mn_fun_queryarraycommand(command); + } break; + + default: + { + } break; + + } + } + else + { // function is called from within the remote host because it has received a say or a sig + } +} + +extern void eoprot_fun_UPDT_mn_comm_cmmnds_command_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmn_command_t* command = (eOmn_command_t*)nv->ram; + eOmn_opc_t opc = (eOmn_opc_t)command->cmd.opc; + + if(eobool_true == eo_nv_hid_isLocal(nv)) + { // function is called from within the local board + switch(opc) + { + case eomn_opc_config_REGROPs_clear: + case eomn_opc_config_REGROPs_assign: + case eomn_opc_config_REGROPs_append: + case eomn_opc_config_REGROPs_remove: + case eomn_opc_config_PROT_boardnumber: + case eomn_opc_config_PROT_endpoint: + { + s_eoprot_ep_mn_fun_configcommand(command); + } break; + + default: + { + } break; + } + } + else + { // function is called from within the remote host because it has received a say or a sig + } + +} + +extern void eoprot_fun_INIT_mn_appl_config(const EOnv* nv) +{ + eOmn_appl_config_t config = {0}; + +#if defined(USE_EMBOT_theHandler) + config.cycletime = embot::app::eth::theHandler_EOMtheEMSrunner_Config.period; + config.txratedivider = embot::app::eth::theHandler_EOMtheEMSrunner_Config.defaultTXdecimationfactor; +#else + EOMtheEMSapplCfg* emscfg = eom_emsapplcfg_GetHandle(); + config.cycletime = emscfg->runobjcfg.period; + config.txratedivider = emscfg->runobjcfg.defaultTXdecimationfactor; +#endif + // set it + eo_nv_Set(nv, &config, eobool_true, eo_nv_upd_dontdo); +} + + + +extern void eoprot_fun_INIT_mn_appl_status(const EOnv* nv) +{ + eOmn_appl_status_t status = {0}; + +#if defined(USE_EMBOT_theHandler) + + // build date + status.buildate.year = embot::app::eth::theApplication_Config.property.date.year; + status.buildate.month = embot::app::eth::theApplication_Config.property.date.month; + status.buildate.day = embot::app::eth::theApplication_Config.property.date.day; + status.buildate.hour = embot::app::eth::theApplication_Config.property.date.hour; + status.buildate.min = embot::app::eth::theApplication_Config.property.date.minute; + + // version + status.version.major = embot::app::eth::theApplication_Config.property.version.major; + status.version.minor = embot::app::eth::theApplication_Config.property.version.minor; + + // control loop timings + status.cloop_timings[0] = embot::app::eth::theHandler_EOMtheEMSrunner_Config.execDOafter; + status.cloop_timings[1] = embot::app::eth::theHandler_EOMtheEMSrunner_Config.execTXafter - embot::app::eth::theHandler_EOMtheEMSrunner_Config.execDOafter; + status.cloop_timings[2] = embot::app::eth::theHandler_EOMtheEMSrunner_Config.period - embot::app::eth::theHandler_EOMtheEMSrunner_Config.execTXafter; + status.txdecimationfactor = embot::app::eth::theHandler_EOMtheEMSrunner_Config.defaultTXdecimationfactor; + + // name + static const char * nn[] = {"amc"}; + memcpy(status.name, nn, std::min(sizeof(status.name), sizeof(nn))); + + // curr state + status.currstate = applstate_config; + status.boardtype = eobrd_ethtype_amc; + +#else +// EOMtheEMSapplCfg* emscfg = eom_emsapplcfg_GetHandle(); +// +// // build date +// status.buildate.year = emscfg->applcfg.emsappinfo->info.entity.builddate.year; +// status.buildate.month = emscfg->applcfg.emsappinfo->info.entity.builddate.month; +// status.buildate.day = emscfg->applcfg.emsappinfo->info.entity.builddate.day; +// status.buildate.hour = emscfg->applcfg.emsappinfo->info.entity.builddate.hour; +// status.buildate.min = emscfg->applcfg.emsappinfo->info.entity.builddate.min; +// +// // version +// status.version.major = emscfg->applcfg.emsappinfo->info.entity.version.major; +// status.version.minor = emscfg->applcfg.emsappinfo->info.entity.version.minor; +// +// // control loop timings +// status.cloop_timings[0] = emscfg->runobjcfg.execDOafter; +// status.cloop_timings[1] = emscfg->runobjcfg.execTXafter - emscfg->runobjcfg.execDOafter; +// status.cloop_timings[2] = emscfg->runobjcfg.period - emscfg->runobjcfg.execTXafter; +// status.txdecimationfactor = emscfg->runobjcfg.defaultTXdecimationfactor; +// +// uint16_t min = EO_MIN(sizeof(status.name), sizeof(emscfg->applcfg.emsappinfo->info.name)); +// memcpy(status.name, emscfg->applcfg.emsappinfo->info.name, min); + + +// // curr state +// status.currstate = applstate_config; +// status.boardtype = eobrd_ethtype_unknown; +// +//#if defined(USE_EMS4RD) +// status.boardtype = eobrd_ethtype_ems4; +//#elif defined(USE_MC4PLUS) +// status.boardtype = eobrd_ethtype_mc4plus; +//#elif defined(USE_MC2PLUS) +// status.boardtype = eobrd_ethtype_mc2plus; +//#elif defined(STM32HAL_BOARD_AMC) +// status.boardtype = eobrd_ethtype_amc; +//#endif + +#endif + + // set it + eo_nv_Set(nv, &status, eobool_true, eo_nv_upd_dontdo); +} + +extern void eoprot_fun_UPDT_mn_appl_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmn_appl_config_t *cfg = (eOmn_appl_config_t*)rd->data; + + if(1000 != cfg->cycletime) + { + cfg->cycletime = 1000; + } + + if((cfg->maxtimeRX + cfg->maxtimeDO + cfg->maxtimeTX) > cfg->cycletime) + { + cfg->maxtimeRX = cfg->cycletime/3; + cfg->maxtimeDO = cfg->cycletime/3; + cfg->maxtimeTX = cfg->cycletime - cfg->maxtimeRX - cfg->maxtimeDO; + } + + if(0 == cfg->txratedivider) + { + cfg->txratedivider = 1; + } + + s_eoprot_ep_mn_fun_apply_config(cfg->cycletime, cfg->maxtimeRX, cfg->maxtimeDO, cfg->maxtimeTX, cfg->txratedivider); +} + + +extern void eoprot_fun_UPDT_mn_appl_config_txratedivider(const EOnv* nv, const eOropdescriptor_t* rd) +{ + uint8_t *txratedivider = (uint8_t*)rd->data; + + if(0 == *txratedivider) + { + *txratedivider = 1; + } + + s_eoprot_ep_mn_fun_apply_config_txratedivider(*txratedivider); +} + + +extern void eoprot_fun_UPDT_mn_appl_cmmnds_go2state(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmn_appl_state_t *go2state = (eOmn_appl_state_t *)nv->ram; + + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status); + eOmn_appl_status_t *status = (eOmn_appl_status_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); + + eOresult_t res = eores_NOK_generic; + + switch(*go2state) + { + case applstate_config: + { +#if defined(USE_EMBOT_theHandler) + embot::app::eth::theHandler::getInstance().process(embot::app::eth::theHandler::Command::go2IDLE); +#else +// res = eom_emsappl_ProcessGo2stateRequest(eom_emsappl_GetHandle(), eo_sm_emsappl_STcfg); +// res = res; +#endif + } break; + + case applstate_running: + { + // we always allow entering the control loop. to be in control loop does nothing unless the service is activated. + +#if defined(USE_EMBOT_theHandler) + embot::app::eth::theHandler::getInstance().process(embot::app::eth::theHandler::Command::go2RUN); +#else + res = eom_emsappl_ProcessGo2stateRequest(eom_emsappl_GetHandle(), eo_sm_emsappl_STrun); +#endif + } break; + + case applstate_error: + { +#if defined(USE_EMBOT_theHandler) + embot::app::eth::theHandler::getInstance().process(embot::app::eth::theHandler::Command::go2FATALERROR); +#else + // i don't expect to receive a go2error command + res = eom_emsappl_ProcessGo2stateRequest(eom_emsappl_GetHandle(), eo_sm_emsappl_STerr); +#endif + } break; + + case applstate_resetmicro: + { + // i just reset the micro ... straight away + ee_sharserv_sys_restart(); + } break; + + case applstate_restartapp: + { + ee_sharserv_ipc_gotoproc_set(ee_procApplication); + ee_sharserv_sys_restart(); + } break; + + + default: + { + } break; + } + +} + + + +extern void eoprot_fun_UPDT_mn_appl_cmmnds_timeset(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOabstime_t *timeset = (eOabstime_t *)nv->ram; + #warning TODO: review eoprot_fun_UPDT_mn_appl_cmmnds_timesetfill() + + // first implementation: if the received time is not much different, then i apply it + + eOabstime_t currtime = eov_sys_LifeTimeGet(eov_sys_GetHandle()); + int64_t delta = *timeset - currtime; + + char str[96]; + uint32_t sec = *timeset/(1000*1000); + uint32_t tmp = *timeset%(1000*1000); + uint32_t msec = tmp / 1000; + uint32_t usec = tmp % 1000; + uint32_t years = sec/3600/24/365; + char str0[64]; + snprintf(str0, sizeof(str0), "s%d m%d u%d", sec, msec, usec); + snprintf(str, sizeof(str), "RQST of time change to %s [or years = %d", str0, years); + eo_errman_Trace(eo_errman_GetHandle(), str, "timeset callback"); + + eOerrmanDescriptor_t descriptor = {0}; + char msg[64] = {0}; + + descriptor.par16 = 0; + descriptor.par64 = currtime; + descriptor.sourcedevice = eo_errman_sourcedevice_localboard; + descriptor.sourceaddress = 0; + descriptor.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag00); + snprintf(msg, sizeof(msg), "synch rqst = %lld", *timeset); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, msg, NULL, &descriptor); + + + // + // first implementation: if the received time is not much different, then i apply it + + eObool_t apply = eobool_false; + if(delta > 0) + { + // we go in the future. do we go much? + if(delta >= eok_reltime1ms) + { + apply = eobool_true; + } + } + else + { + // it is either zero or negative (we go in the past) + delta = -delta; + if(delta >= eok_reltime1ms) + { + apply = eobool_true; + } + } + + if(eobool_true == apply) + { + eov_sys_LifeTimeSet(eov_sys_GetHandle(), *timeset); + } + + + currtime = eov_sys_LifeTimeGet(eov_sys_GetHandle()); + + descriptor.par16 = apply; + descriptor.par64 = currtime; + descriptor.sourcedevice = eo_errman_sourcedevice_localboard; + descriptor.sourceaddress = 0; + descriptor.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag01); + snprintf(msg, sizeof(msg), "time = %lld", currtime); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, msg, NULL, &descriptor); + +} + + +extern void eoprot_fun_UPDT_mn_service_cmmnds_command(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmn_service_cmmnds_command_t *command = (eOmn_service_cmmnds_command_t *)nv->ram; + // ok, we have received a command for a given service. we ask the object theservices to manage the thing +#if defined(USE_EMBOT_theServices) + embot::app::eth::theServices::getInstance().process(command); +#else + eo_services_ProcessCommand(eo_services_GetHandle(), command); +#endif +} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static void s_eoprot_ep_mn_fun_querynumofcommand(eOmn_command_t* command) +{ + EOtransceiver* theems00transceiver; + eOmn_cmd_querynumof_t* cmdquerynumof = (eOmn_cmd_querynumof_t*)&command->cmd; + eOmn_cmd_replynumof_t* cmdreplynumof = (eOmn_cmd_replynumof_t*)&command->cmd; + + eOropdescriptor_t ropdesc; + + eOmn_opc_t opc = (eOmn_opc_t) cmdquerynumof->opcpar.opc; + + + if(NULL == (theems00transceiver = eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle()))) + { + return; + } + + // retrieve all useful parametes + uint8_t endpoint = cmdquerynumof->opcpar.endpoint; + + // then clean data to be sent back: + memset(command, 0, sizeof(eOmn_command_t)); + + + switch(opc) + { + + case eomn_opc_query_numof_EPs: + { // must give back the number of endpoints + cmdreplynumof->opcpar.opc = eomn_opc_reply_numof_EPs; + cmdreplynumof->opcpar.endpoint = eoprot_endpoint_all; + cmdreplynumof->opcpar.numberof = eoprot_endpoints_numberof_get(eoprot_board_localboard); + + + // ok, now i send the occasional rop + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replynumof); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. +#if defined(USE_EMBOT_theHandler) +// #warning USE_EMBOT_theHandler is defined. SOLVED how to transmit an occasional ROP + eom_emstransceiver_Transmit_OccasionalROP(eom_emstransceiver_GetHandle(), &ropdesc); +#else + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); // it also alert someone to send the rop +#endif + } break; + + + case eomn_opc_query_numof_ENs: + { // must give back the number of entities + cmdreplynumof->opcpar.opc = eomn_opc_reply_numof_ENs; + cmdreplynumof->opcpar.endpoint = endpoint; + if(eoprot_endpoint_all == endpoint) + { + cmdreplynumof->opcpar.numberof = eoprot_entities_numberof_get(eoprot_board_localboard); + } + else + { + cmdreplynumof->opcpar.numberof = eoprot_entities_in_endpoint_numberof_get(eoprot_board_localboard, endpoint); + } + + // ok, now i send the occasional rop + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replynumof); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. +#if defined(USE_EMBOT_theHandler) + eom_emstransceiver_Transmit_OccasionalROP(eom_emstransceiver_GetHandle(), &ropdesc); +#else + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); +#endif + } break; + + + case eomn_opc_query_numof_REGROPs: + { + cmdreplynumof->opcpar.opc = eomn_opc_reply_numof_REGROPs; + cmdreplynumof->opcpar.endpoint = endpoint; + + if(eoprot_endpoint_all == endpoint) + { + cmdreplynumof->opcpar.numberof = eo_transceiver_RegularROP_ArrayID32Size(theems00transceiver); + } + else + { + cmdreplynumof->opcpar.numberof = eo_transceiver_RegularROP_ArrayID32SizeWithEP(theems00transceiver, endpoint); + } + + // ok, now i send the occasional rop + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replynumof); + ropdesc.data = NULL; // so that data from the EOnv is retrieved. +#if defined(USE_EMBOT_theHandler) + eom_emstransceiver_Transmit_OccasionalROP(eom_emstransceiver_GetHandle(), &ropdesc); +#else + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); +#endif + + } break; + + + default: + { + + } break; + } + + +} + + +static void s_eoprot_ep_mn_fun_queryarraycommand(eOmn_command_t* command) +{ + EOtransceiver* theems00transceiver; + eOmn_cmd_queryarray_t* cmdqueryarray = (eOmn_cmd_queryarray_t*)&command->cmd; + eOmn_cmd_replyarray_t* cmdreplyarray = (eOmn_cmd_replyarray_t*)&command->cmd; + + eOropdescriptor_t ropdesc; + + eOmn_opc_t opc = (eOmn_opc_t) cmdqueryarray->opcpar.opc; + + + if(NULL == (theems00transceiver = eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle()))) + { + return; + } + + // retrieve all useful parametes + uint8_t endpoint = cmdqueryarray->opcpar.endpoint; + uint8_t setnumber = cmdqueryarray->opcpar.setnumber; + uint8_t setsize = cmdqueryarray->opcpar.setsize; + // then clean data to be sent back: + memset(command, 0, sizeof(eOmn_command_t)); + + + switch(opc) + { + + case eomn_opc_query_array_EPs: + { + uint8_t capacity = (sizeof(cmdreplyarray->array) - sizeof(eOarray_head_t)) / sizeof(eOnvEP8_t); + if(0 == setsize) + { + setsize = capacity; + } + + // ok... now i form a rop to send back. at first i write into the nv. + EOarray* ep08array = eo_array_New(setsize, sizeof(eOnvEP8_t), cmdreplyarray->array); + //uint8_t total = eoprot_endpoints_numberof_get(eoprot_board_localboard); + + // ok, prepare the occasional rops + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + + eo_array_Reset(ep08array); + cmdreplyarray->opcpar.opc = eomn_opc_reply_array_EPs; + cmdreplyarray->opcpar.endpoint = eoprot_endpoint_all; + cmdreplyarray->opcpar.setnumber = setnumber; + cmdreplyarray->opcpar.setsize = setsize; + eoprot_endpoints_array_get(eoprot_board_localboard, ep08array, setnumber*setsize); +#if defined(USE_EMBOT_theHandler) + eom_emstransceiver_Transmit_OccasionalROP(eom_emstransceiver_GetHandle(), &ropdesc); +#else + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); +#endif + } break; + + + case eomn_opc_query_array_EPdes: + { + uint8_t capacity = (sizeof(cmdreplyarray->array) - sizeof(eOarray_head_t)) / sizeof(eoprot_endpoint_descriptor_t); + if(0 == setsize) + { + setsize = capacity; + } + + EOarray* epdesarray = eo_array_New(setsize, sizeof(eoprot_endpoint_descriptor_t), cmdreplyarray->array); + //uint8_t total = eoprot_endpoints_numberof_get(eoprot_board_localboard); + + // ok, prepare the occasional rops + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + + eo_array_Reset(epdesarray); + cmdreplyarray->opcpar.opc = eomn_opc_reply_array_EPdes; + cmdreplyarray->opcpar.endpoint = eoprot_endpoint_all; + cmdreplyarray->opcpar.setnumber = setnumber; + cmdreplyarray->opcpar.setsize = setsize; + eoprot_endpoints_arrayofdescriptors_get(eoprot_board_localboard, epdesarray, setnumber*setsize); +#if defined(USE_EMBOT_theHandler) + eom_emstransceiver_Transmit_OccasionalROP(eom_emstransceiver_GetHandle(), &ropdesc); +#else + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); +#endif + } break; + + + case eomn_opc_query_array_ENdes: + { + uint8_t capacity = (sizeof(cmdreplyarray->array) - sizeof(eOarray_head_t)) / sizeof(eoprot_entity_descriptor_t); + if(0 == setsize) + { + setsize = capacity; + } + + EOarray* endesarray = eo_array_New(setsize, sizeof(eoprot_entity_descriptor_t), cmdreplyarray->array); + //uint8_t total = eoprot_entities_numberof_get(eoprot_board_localboard); + + // ok, prepare the occasional rops + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + + eo_array_Reset(endesarray); + cmdreplyarray->opcpar.opc = eomn_opc_reply_array_ENdes; + cmdreplyarray->opcpar.endpoint = endpoint; + cmdreplyarray->opcpar.setnumber = setnumber; + cmdreplyarray->opcpar.setsize = setsize; + + if(eoprot_endpoint_all == endpoint) + { + eoprot_entities_arrayofdescriptors_get(eoprot_board_localboard, endesarray, setnumber*setsize); + } + else + { + eoprot_entities_in_endpoint_arrayofdescriptors_get(eoprot_board_localboard, endpoint, endesarray, setnumber*setsize); + } +#if defined(USE_EMBOT_theHandler) + eom_emstransceiver_Transmit_OccasionalROP(eom_emstransceiver_GetHandle(), &ropdesc); +#else + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); +#endif + + } break; + + + case eomn_opc_query_array_REGROPs: + { + uint8_t capacity = (sizeof(cmdreplyarray->array) - sizeof(eOarray_head_t)) / sizeof(eOnvID32_t); + if(0 == setsize) + { + setsize = capacity; + } + + // ok... now i form a rop to send back. at first i write into the nv. + EOarray* id32array = eo_array_New(setsize, sizeof(eOnvID32_t), cmdreplyarray->array); + // we have total rops ... + uint8_t total = eo_transceiver_RegularROP_ArrayID32Size(theems00transceiver); + + + // ok, prepare the occasional rops + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + + eo_array_Reset(id32array); + cmdreplyarray->opcpar.opc = eomn_opc_reply_array_REGROPs; + cmdreplyarray->opcpar.endpoint = endpoint; + cmdreplyarray->opcpar.setnumber = setnumber; + cmdreplyarray->opcpar.setsize = setsize; + + if(eoprot_endpoint_all == endpoint) + { + eo_transceiver_RegularROP_ArrayID32Get(theems00transceiver, setnumber*setsize, id32array); + } + else + { + eo_transceiver_RegularROP_ArrayID32GetWithEP(theems00transceiver, endpoint, setnumber*setsize, id32array); + } +#if defined(USE_EMBOT_theHandler) + eom_emstransceiver_Transmit_OccasionalROP(eom_emstransceiver_GetHandle(), &ropdesc); +#else + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); +#endif + } break; + + default: + { + + } break; + } + + +} + +static void s_eoprot_ep_mn_fun_configcommand(eOmn_command_t* command) +{ + uint8_t size, i; + eOropSIGcfg_t *sigcfg; + eOropdescriptor_t ropdesc; + EOtransceiver* theems00transceiver; + + //eOmn_cmd_config_t* cmdconfig = (eOmn_cmd_config_t*)&command->cmd; /////// not correct. + eOmn_cmd_config_t* cmdconfig = &command->cmd.config; + EOarray *array = (EOarray*)cmdconfig->array; + + uint16_t targetcapacity = (sizeof(cmdconfig->array) - sizeof(eOarray_head_t)) / sizeof(eOropSIGcfg_t); + + + eOmn_opc_t opc = (eOmn_opc_t) cmdconfig->opcpar.opc; + + eOresult_t res; + + if(NULL == (theems00transceiver = eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle()))) + { + return; + } + + + switch(opc) + { + + case eomn_opc_config_REGROPs_clear: + { // just clear + eo_transceiver_RegularROPs_Clear(theems00transceiver); + } break; + + case eomn_opc_config_REGROPs_assign: + { // clear and load all the sigcfg in the array + + if((eo_array_ItemSize(array) != sizeof(eOropSIGcfg_t)) || (targetcapacity != eo_array_Capacity(array)) || ((size = eo_array_Size(array)) > targetcapacity)) + { + return; + } + + eo_transceiver_RegularROPs_Clear(theems00transceiver); + + for(i=0; iopcpar.plustime) ? (1) : (0); + ropdesc.control.plussign = (eobool_true == cmdconfig->opcpar.plussign) ? (1) : (0); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = sigcfg->id32; + ropdesc.signature = cmdconfig->opcpar.signature; + res = eo_transceiver_RegularROP_Load(theems00transceiver, &ropdesc); + res = res; + if(eores_OK != res) + { + eOerrmanDescriptor_t errdes = {0}; + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.par16 = 0; + errdes.par64 = sigcfg->id32; + errdes.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_comm_cannotloadaregularrop); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); + } + } + } break; + + case eomn_opc_config_REGROPs_append: + { // dont clear and load all the sigcfg in the array + if((eo_array_ItemSize(array) != sizeof(eOropSIGcfg_t)) || (targetcapacity != eo_array_Capacity(array)) || ((size = eo_array_Size(array)) > targetcapacity)) + { + return; + } + + for(i=0; iopcpar.plustime) ? (1) : (0); + ropdesc.control.plussign = (eobool_true == cmdconfig->opcpar.plussign) ? (1) : (0); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = sigcfg->id32; + ropdesc.signature = cmdconfig->opcpar.signature; + res = eo_transceiver_RegularROP_Load(theems00transceiver, &ropdesc); + res = res; + if(eores_OK != res) + { + eOerrmanDescriptor_t errdes = {0}; + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.par16 = 0; + errdes.par64 = sigcfg->id32; + errdes.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_comm_cannotloadaregularrop); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); + } + } + } break; + + case eomn_opc_config_REGROPs_remove: + { // remove all the sigcfg in the array + if((eo_array_ItemSize(array) != sizeof(eOropSIGcfg_t)) || (targetcapacity != eo_array_Capacity(array)) || ((size = eo_array_Size(array)) > targetcapacity)) + { + return; + } + + for(i=0; iopcpar.plustime) ? (1) : (0); + ropdesc.control.plussign = (eobool_true == cmdconfig->opcpar.plussign) ? (1) : (0); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = sigcfg->id32; + ropdesc.signature = cmdconfig->opcpar.signature; + res = eo_transceiver_RegularROP_Unload(theems00transceiver, &ropdesc); + res = res; + } + } break; + + case eomn_opc_config_PROT_boardnumber: + { // simply set the byte in array[0] as the new localboard number + //eOprotBRD_t brdnum = command->cmd.config.array[0]; + eoprot_config_board_local(command->cmd.config.array[0]); + } break; + + case eomn_opc_config_PROT_endpoint: + { // simply use the bytes in array[0->7] as a eOprot_EPcfg_t. but only if the EP is valid and not loaded yet. + eOprot_EPcfg_t *epcfg = (eOprot_EPcfg_t*) &command->cmd.config.array[0]; + if(eobool_true == eoprot_EPcfg_isvalid(epcfg)) + { + if(eobool_false == eoprot_endpoint_configured_is(eoprot_board_localboard, epcfg->endpoint)) + { + EOnvSet* nvset = eom_emstransceiver_GetNVset(eom_emstransceiver_GetHandle()); + eo_nvset_LoadEP(nvset, epcfg, eobool_true); + } + } + } break; + + + default: + { + + } break; + } + + +} + + +static void s_eoprot_ep_mn_fun_apply_config_txratedivider(uint8_t txratedivider) +{ + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status); + eOmn_appl_status_t *status = (eOmn_appl_status_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); + status->txdecimationfactor = txratedivider; + + eom_emsrunner_Set_TXdecimationFactor(eom_emsrunner_GetHandle(), txratedivider); +} + + +static void s_eoprot_ep_mn_fun_apply_config(uint32_t period, uint16_t rxtime, uint16_t dotime, uint16_t txtime, uint8_t txratedivider) +{ + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status); + eOmn_appl_status_t *status = (eOmn_appl_status_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); + + status->cloop_timings[0] = rxtime; + status->cloop_timings[1] = dotime; + status->cloop_timings[2] = txtime; + status->txdecimationfactor = txratedivider; + + eOemsrunner_timing_t timing = {0}; + timing.period = period; + timing.safetygap = 0; + timing.rxstartafter = 0; + timing.dostartafter = rxtime; + timing.txstartafter = rxtime + dotime; + + eom_emsrunner_SetTiming(eom_emsrunner_GetHandle(), &timing); + + eom_emsrunner_Set_TXdecimationFactor(eom_emsrunner_GetHandle(), txratedivider); +} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_fun_ems4rd.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_fun_ems4rd.c new file mode 100644 index 0000000000..36776344b8 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_fun_ems4rd.c @@ -0,0 +1,1297 @@ +/* + * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +/* @file EoProtocolMN_fun_ems4rd.c + @brief This file keeps c... + @author marco.accame@iit.it + @date 06/06/2013 +**/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +#include "stdlib.h" +#include "string.h" +#include "stdio.h" + +#include "EoManagement.h" +#include "EOnv_hid.h" + +#include "EOtheBOARDtransceiver.h" +#include "EOMtheEMSappl.h" +#include "EOMtheEMSapplCfg.h" + +#include "EOMtheEMSappl.h" + +#include "EOtheErrorManager.h" +#include "EoError.h" + +#include "eEsharedServices.h" + +#include "EOtheServices.h" + +#include "EOVtheSystem.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EoProtocolMN.h" + + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- + + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static void s_eoprot_ep_mn_fun_apply_config(uint32_t period, uint16_t rxtime, uint16_t dotime, uint16_t txtime, uint8_t txratedivider); + +static void s_eoprot_ep_mn_fun_apply_config_txratedivider(uint8_t txratedivider); + + +static void s_eoprot_ep_mn_fun_configcommand(eOmn_command_t* command); + +static void s_eoprot_ep_mn_fun_querynumofcommand(eOmn_command_t* command); +static void s_eoprot_ep_mn_fun_queryarraycommand(eOmn_command_t* command); + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- + +#if defined(TESTethboardsfix) +static const eOmn_serv_configuration_t s_serv_config_strain_generic = +{ + .type = eomn_serv_AS_strain, + .filler = {0}, + .data.as.strain = + { + .boardtype = + { + .type = eobrd_strain2, + .firmware = {2, 0, 10}, + .protocol = {2, 0} + }, + .canloc = + { + .port = eOcanport1, + .addr = 13, + .insideindex = eobrd_caninsideindex_none, + .dummy = 0 + } + } +}; + +static const eOmn_serv_configuration_t s_serv_config_mc_mc4plus_eb24 = +{ + .type = eomn_serv_MC_mc4plus, + .filler = {0}, + .data.mc.mc4plus_based = + { + .arrayofjomodescriptors = + { + .head = + { + .capacity = 4, + .itemsize = sizeof(eOmc_jomo_descriptor_t), + .size = 4, + .internalmem = 0 + }, + .data = + { + { // joint 0 + .actuator.pwm = + { + .port = eobrd_port_mc4plusP5, + .dummy = 0 + }, + .encoder1 = + { + .type = eomc_enc_qenc, + .port = eobrd_port_mc4plusP5, + .pos = eomc_pos_atjoint + }, + .encoder2 = + { + .type = eomc_enc_qenc, + .port = eobrd_port_mc4plusP5, + .pos = eomc_pos_atmotor + } + }, + { // joint 1 + .actuator.pwm = + { + .port = eobrd_port_mc4plusP2, + .dummy = 0 + }, + .encoder1 = + { + .type = eomc_enc_aea, + .port = eobrd_port_mc4plusP10, + .pos = eomc_pos_atjoint + }, + .encoder2 = + { + .type = eomc_enc_qenc, + .port = eobrd_port_mc4plusP2, + .pos = eomc_pos_atmotor + } + }, + { // joint 2 + .actuator.pwm = + { + .port = eobrd_port_mc4plusP4, + .dummy = 0 + }, + .encoder1 = + { + .type = eomc_enc_aea, + .port = eobrd_port_mc4plusP11, + .pos = eomc_pos_atjoint + }, + .encoder2 = + { + .type = eomc_enc_qenc, + .port = eobrd_port_mc4plusP4, + .pos = eomc_pos_atmotor + } + }, + { // joint 3 + .actuator.pwm = + { + .port = eobrd_port_mc4plusP3, + .dummy = 0 + }, + .encoder1 = + { + .type = eomc_enc_absanalog, + .port = eobrd_port_mc4plusP3, + .pos = eomc_pos_atjoint + }, + .encoder2 = + { + .type = eomc_enc_none, + .port = eobrd_port_none, + .pos = eomc_pos_none + } + } + } + }, + .jomocoupling = + { + .joint2set = + { // 3 sets: set0 = {0}, set1 = {1, 2}, set2 = {3} + 0, 1, 1, 2 + }, + .joint2motor = + { // tbd + { EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(-1.0f), EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(1.0f) } + }, + .encoder2joint = + { // nearly identical matrix + { EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) } + } + } + } +}; + +static const eOmn_serv_configuration_t s_serv_config_mc_eb1_eb3_zeroprotocol = +{ // eb1 / eb3 + .type = eomn_serv_MC_foc, + .filler = {0}, + .data.mc.foc_based = + { + .version = + { + .firmware = { .major = 0, .minor = 0, .build = 0 }, + .protocol = { .major = 0, .minor = 0 } + }, + .filler = {0}, + .arrayofjomodescriptors = + { + .head = + { + .capacity = 4, + .itemsize = sizeof(eOmc_jomo_descriptor_t), + .size = 4, + .internalmem = 0 + }, + .data = + { + { // joint 0 + .actuator.foc.canloc = + { + .port = eOcanport1, + .addr = 1, + .insideindex = eobrd_caninsideindex_first + }, + .encoder1 = + { + .type = eomc_enc_aea, + .port = eobrd_port_emsP6, // hal_encoder1 + .pos = eomc_pos_atjoint + }, + .encoder2 = + { + .type = eomc_enc_none, + .port = eobrd_port_none, + .pos = eomc_pos_none + } + }, + { // joint 1 + .actuator.foc.canloc = + { + .port = eOcanport1, + .addr = 2, + .insideindex = eobrd_caninsideindex_first + }, + .encoder1 = + { + .type = eomc_enc_aea, + .port = eobrd_port_emsP7, // hal_encoder4 + .pos = eomc_pos_atjoint + }, + .encoder2 = + { + .type = eomc_enc_none, + .port = eobrd_port_none, + .pos = eomc_pos_none + } + }, + { // joint 2 + .actuator.foc.canloc = + { + .port = eOcanport1, + .addr = 3, + .insideindex = eobrd_caninsideindex_first + }, + .encoder1 = + { + .type = eomc_enc_aea, + .port = eobrd_port_emsP8, // hal_encoder2 + .pos = eomc_pos_atjoint + }, + .encoder2 = + { + .type = eomc_enc_none, + .port = eobrd_port_none, + .pos = eomc_pos_none + } + }, + { // joint 3 + .actuator.foc.canloc = + { + .port = eOcanport1, + .addr = 4, + .insideindex = eobrd_caninsideindex_first + }, + .encoder1 = + { + .type = eomc_enc_aea, + .port = eobrd_port_emsP9, // hal_encoder5 + .pos = eomc_pos_atjoint + }, + .encoder2 = + { + .type = eomc_enc_none, + .port = eobrd_port_none, + .pos = eomc_pos_none + } + } + } + }, + .jomocoupling = + { + .joint2set = + { // each joint is on a different set + 0, 1, 2, 3 + }, + .joint2motor = + { // zero matrix: use matrix embedded in controller and seecetd by boardtype4mccontroller + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) } + }, + .encoder2joint = + { // identical matrix + { EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(1.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f) }, + { EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(0.0f), EO_COMMON_FLOAT_TO_Q17_14(1.0f) } + } + } + } +}; + +#endif // TESTethboardsfix + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + +extern void eoprot_fun_INIT_mn_comm_status(const EOnv* nv) +{ + eOmn_comm_status_t* status = (eOmn_comm_status_t*)nv->ram; + + // 1. init the management protocol version + + eOversion_t* version = &status->managementprotocolversion; + + version->major = eoprot_version_mn_major; + version->minor = eoprot_version_mn_minor; + + + // 2. init the transceiver + + eOmn_transceiver_properties_t* transp = &status->transceiver; + + EOMtheEMSapplCfg* emscfg = eom_emsapplcfg_GetHandle(); + + transp->listeningPort = emscfg->socketcfg.localport; + transp->destinationPort = emscfg->transcfg.hostipv4port; + transp->maxsizeRXpacket = emscfg->socketcfg.inpdatagramsizeof; + transp->maxsizeTXpacket = emscfg->socketcfg.outdatagramsizeof; + transp->maxsizeROPframeRegulars = emscfg->transcfg.sizes.capacityofropframeregulars; + transp->maxsizeROPframeReplies = emscfg->transcfg.sizes.capacityofropframereplies; + transp->maxsizeROPframeOccasionals = emscfg->transcfg.sizes.capacityofropframeoccasionals; + transp->maxsizeROP = emscfg->transcfg.sizes.capacityofrop; + transp->maxnumberRegularROPs = emscfg->transcfg.sizes.maxnumberofregularrops; + memset(transp->filler06, 0, sizeof(transp->filler06)); +} + + + +extern void eoprot_fun_UPDT_mn_comm_cmmnds_command_querynumof(const EOnv* nv, const eOropdescriptor_t* rd) +{ + //eOprotIndex_t index = eoprot_ID2index(nv->ep, nv->id); + + eOmn_command_t* command = (eOmn_command_t*)nv->ram; + + eOmn_opc_t opc = (eOmn_opc_t)command->cmd.opc; + + + if(eobool_true == eo_nv_hid_isLocal(nv)) + { // function is called from within the local board + + switch(opc) + { + + case eomn_opc_query_numof_EPs: + case eomn_opc_query_numof_ENs: + case eomn_opc_query_numof_REGROPs: + { + s_eoprot_ep_mn_fun_querynumofcommand(command); + } break; + + default: + { + } break; + + } + } + else + { // function is called from within the remote host because it has received a say or a sig + + } + +} + +extern void eoprot_fun_UPDT_mn_comm_cmmnds_command_queryarray(const EOnv* nv, const eOropdescriptor_t* rd) +{ + //eOprotIndex_t index = eoprot_ID2index(nv->ep, nv->id); + + eOmn_command_t* command = (eOmn_command_t*)nv->ram; + + eOmn_opc_t opc = (eOmn_opc_t)command->cmd.opc; + + + if(eobool_true == eo_nv_hid_isLocal(nv)) + { // function is called from within the local board + + switch(opc) + { + + case eomn_opc_query_array_EPs: + case eomn_opc_query_array_EPdes: + case eomn_opc_query_array_ENdes: + case eomn_opc_query_array_REGROPs: + { + s_eoprot_ep_mn_fun_queryarraycommand(command); + } break; + + default: + { + } break; + + } + } + else + { // function is called from within the remote host because it has received a say or a sig + + } + +} + +extern void eoprot_fun_UPDT_mn_comm_cmmnds_command_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + //eOprotIndex_t index = eoprot_ID2index(nv->ep, nv->id); + + eOmn_command_t* command = (eOmn_command_t*)nv->ram; + + eOmn_opc_t opc = (eOmn_opc_t)command->cmd.opc; + + + if(eobool_true == eo_nv_hid_isLocal(nv)) + { // function is called from within the local board + + switch(opc) + { + + case eomn_opc_config_REGROPs_clear: + case eomn_opc_config_REGROPs_assign: + case eomn_opc_config_REGROPs_append: + case eomn_opc_config_REGROPs_remove: + case eomn_opc_config_PROT_boardnumber: + case eomn_opc_config_PROT_endpoint: + { + s_eoprot_ep_mn_fun_configcommand(command); + } break; + + default: + { + } break; + + } + } + else + { // function is called from within the remote host because it has received a say or a sig + + } + +} + +extern void eoprot_fun_INIT_mn_appl_config(const EOnv* nv) +{ + eOmn_appl_config_t config = {0}; + + EOMtheEMSapplCfg* emscfg = eom_emsapplcfg_GetHandle(); + + config.cycletime = emscfg->runobjcfg.period; + config.txratedivider = emscfg->runobjcfg.defaultTXdecimationfactor; + + // set it + eo_nv_Set(nv, &config, eobool_true, eo_nv_upd_dontdo); +} + + + +extern void eoprot_fun_INIT_mn_appl_status(const EOnv* nv) +{ + // i init the application status to ... + eOmn_appl_status_t status = {0}; + + EOMtheEMSapplCfg* emscfg = eom_emsapplcfg_GetHandle(); + + // build date + status.buildate.year = emscfg->applcfg.emsappinfo->info.entity.builddate.year; + status.buildate.month = emscfg->applcfg.emsappinfo->info.entity.builddate.month; + status.buildate.day = emscfg->applcfg.emsappinfo->info.entity.builddate.day; + status.buildate.hour = emscfg->applcfg.emsappinfo->info.entity.builddate.hour; + status.buildate.min = emscfg->applcfg.emsappinfo->info.entity.builddate.min; + + // version + status.version.major = emscfg->applcfg.emsappinfo->info.entity.version.major; + status.version.minor = emscfg->applcfg.emsappinfo->info.entity.version.minor; + + // control loop timings + status.cloop_timings[0] = emscfg->runobjcfg.execDOafter; + status.cloop_timings[1] = emscfg->runobjcfg.execTXafter - emscfg->runobjcfg.execDOafter; + status.cloop_timings[2] = emscfg->runobjcfg.period - emscfg->runobjcfg.execTXafter; + status.txdecimationfactor = emscfg->runobjcfg.defaultTXdecimationfactor; + + uint16_t min = EO_MIN(sizeof(status.name), sizeof(emscfg->applcfg.emsappinfo->info.name)); + memcpy(status.name, emscfg->applcfg.emsappinfo->info.name, min); + + // curr state + status.currstate = applstate_config; + + status.boardtype = eobrd_ethtype_unknown; + +#if defined(USE_EMS4RD) + status.boardtype = eobrd_ethtype_ems4; +#elif defined(USE_MC4PLUS) + status.boardtype = eobrd_ethtype_mc4plus; +#elif defined(USE_MC2PLUS) + status.boardtype = eobrd_ethtype_mc2plus; +#endif + + // set it + eo_nv_Set(nv, &status, eobool_true, eo_nv_upd_dontdo); +} + +extern void eoprot_fun_UPDT_mn_appl_config(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmn_appl_config_t *cfg = (eOmn_appl_config_t*)rd->data; + + if(1000 != cfg->cycletime) + { + cfg->cycletime = 1000; + } + + if((cfg->maxtimeRX + cfg->maxtimeDO + cfg->maxtimeTX) > cfg->cycletime) + { + cfg->maxtimeRX = cfg->cycletime/3; + cfg->maxtimeDO = cfg->cycletime/3; + cfg->maxtimeTX = cfg->cycletime - cfg->maxtimeRX - cfg->maxtimeDO; + } + + if(0 == cfg->txratedivider) + { + cfg->txratedivider = 1; + } + + s_eoprot_ep_mn_fun_apply_config(cfg->cycletime, cfg->maxtimeRX, cfg->maxtimeDO, cfg->maxtimeTX, cfg->txratedivider); +} + + +extern void eoprot_fun_UPDT_mn_appl_config_txratedivider(const EOnv* nv, const eOropdescriptor_t* rd) +{ + uint8_t *txratedivider = (uint8_t*)rd->data; + + if(0 == *txratedivider) + { + *txratedivider = 1; + } + + s_eoprot_ep_mn_fun_apply_config_txratedivider(*txratedivider); +} + + +extern void eoprot_fun_UPDT_mn_appl_cmmnds_go2state(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmn_appl_state_t *go2state = (eOmn_appl_state_t *)nv->ram; + + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status); + eOmn_appl_status_t *status = (eOmn_appl_status_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); + + eOresult_t res = eores_NOK_generic; + + switch(*go2state) + { + case applstate_config: + { + res = eom_emsappl_ProcessGo2stateRequest(eom_emsappl_GetHandle(), eo_sm_emsappl_STcfg); + res = res; + // the new currstate is set inside the on-entry of the state machine + //if(eores_OK == res) + //{ + // status->currstate = applstate_config; + //} + } break; + + case applstate_running: + { + // we always allow entering the control loop. to be in control loop does nothing unless the service is activated. + +// if(eobool_false == eo_services_startupactivation_AllActivated(eo_services_GetHandle())) +// { +// eo_services_startupactivation_SendFailureReport(eo_services_GetHandle()); +// } + res = eom_emsappl_ProcessGo2stateRequest(eom_emsappl_GetHandle(), eo_sm_emsappl_STrun); + + } break; + + case applstate_error: + { + // i don't expect to receive a go2error command + res = eom_emsappl_ProcessGo2stateRequest(eom_emsappl_GetHandle(), eo_sm_emsappl_STerr); + // the new currstate is set inside the on-entry of the state machine + //if(eores_OK == res) + //{ + // status->currstate = applstate_error; + //} + } break; + + case applstate_resetmicro: + { + // i just reset the micro ... straight away + osal_system_scheduling_suspend(); + ee_sharserv_sys_restart(); + } break; + + case applstate_restartapp: + { + osal_system_scheduling_suspend(); + ee_sharserv_ipc_gotoproc_set(ee_procApplication); + ee_sharserv_sys_restart(); + } break; + + + default: + { + } break; + } + +} + + +#if defined(TESTethboardsfix) + + +static eOmn_service_cmmnds_command_t s_command = {0}; + +volatile uint32_t count = 0; +volatile uint8_t thisstep = 0; +extern void eoprot_fun_UPDT_mn_appl_cmmnds_timeset(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOabstime_t *timeset = (eOabstime_t *)nv->ram; + + + static const uint8_t period = 4; + static const eOmn_serv_operation_t ops[4] = + { + eomn_serv_operation_verifyactivate, + eomn_serv_operation_start, + eomn_serv_operation_stop, + eomn_serv_operation_deactivate + }; + + + s_command.operation = ops[count % period]; + + if(count >=8) + { + thisstep = 1; + } + + // strain service: it is ok in both ems and mc4plus +// s_command.category = eomn_serv_category_strain; +// memcpy(&s_command.parameter.configuration, &s_serv_config_strain_generic, sizeof(eOmn_serv_configuration_t)); + + + s_command.category = eomn_serv_category_mc; + #if defined(USE_EMS4RD) + memcpy(&s_command.parameter.configuration, &s_serv_config_mc_eb1_eb3_zeroprotocol, sizeof(eOmn_serv_configuration_t)); + #else + memcpy(&s_command.parameter.configuration, &s_serv_config_mc_mc4plus_eb24, sizeof(eOmn_serv_configuration_t)); + #endif + + + eOmn_service_cmmnds_command_t *command = &s_command; + + count ++; + + // ok, we have received a command for a given service. we ask the object theservices to manage the thing + + eo_services_ProcessCommand(eo_services_GetHandle(), command); + +} + +#else // not TESTethboardsfix + +extern void eoprot_fun_UPDT_mn_appl_cmmnds_timeset(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOabstime_t *timeset = (eOabstime_t *)nv->ram; + #warning TODO: review eoprot_fun_UPDT_mn_appl_cmmnds_timesetfill() + + // first implementation: if the received time is not much different, then i apply it + + eOabstime_t currtime = eov_sys_LifeTimeGet(eov_sys_GetHandle()); + int64_t delta = *timeset - currtime; + + char str[96]; + uint32_t sec = *timeset/(1000*1000); + uint32_t tmp = *timeset%(1000*1000); + uint32_t msec = tmp / 1000; + uint32_t usec = tmp % 1000; + uint32_t years = sec/3600/24/365; + char str0[64]; + snprintf(str0, sizeof(str0), "s%d m%d u%d", sec, msec, usec); + snprintf(str, sizeof(str), "RQST of time change to %s [or years = %d", str0, years); + eo_errman_Trace(eo_errman_GetHandle(), str, "timeset callback"); + + eOerrmanDescriptor_t descriptor = {0}; + char msg[64] = {0}; + + descriptor.par16 = 0; + descriptor.par64 = currtime; + descriptor.sourcedevice = eo_errman_sourcedevice_localboard; + descriptor.sourceaddress = 0; + descriptor.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag00); + snprintf(msg, sizeof(msg), "synch rqst = %lld", *timeset); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, msg, NULL, &descriptor); + + + // + // first implementation: if the received time is not much different, then i apply it + + eObool_t apply = eobool_false; + if(delta > 0) + { + // we go in the future. do we go much? + if(delta >= eok_reltime1ms) + { + apply = eobool_true; + } + } + else + { + // it is either zero or negative (we go in the past) + delta = -delta; + if(delta >= eok_reltime1ms) + { + apply = eobool_true; + } + } + + if(eobool_true == apply) + { + eov_sys_LifeTimeSet(eov_sys_GetHandle(), *timeset); + } + + + currtime = eov_sys_LifeTimeGet(eov_sys_GetHandle()); + + descriptor.par16 = apply; + descriptor.par64 = currtime; + descriptor.sourcedevice = eo_errman_sourcedevice_localboard; + descriptor.sourceaddress = 0; + descriptor.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag01); + snprintf(msg, sizeof(msg), "time = %lld", currtime); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, msg, NULL, &descriptor); + +} + +#endif // TESTethboardsfix + + +extern void eoprot_fun_UPDT_mn_service_cmmnds_command(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOmn_service_cmmnds_command_t *command = (eOmn_service_cmmnds_command_t *)nv->ram; + + // ok, we have received a command for a given service. we ask the object theservices to manage the thing + + eo_services_ProcessCommand(eo_services_GetHandle(), command); +} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static void s_eoprot_ep_mn_fun_querynumofcommand(eOmn_command_t* command) +{ + EOtransceiver* theems00transceiver; + eOmn_cmd_querynumof_t* cmdquerynumof = (eOmn_cmd_querynumof_t*)&command->cmd; + eOmn_cmd_replynumof_t* cmdreplynumof = (eOmn_cmd_replynumof_t*)&command->cmd; + + eOropdescriptor_t ropdesc; + + eOmn_opc_t opc = (eOmn_opc_t) cmdquerynumof->opcpar.opc; + + + if(NULL == (theems00transceiver = eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle()))) + { + return; + } + + // retrieve all useful parametes + uint8_t endpoint = cmdquerynumof->opcpar.endpoint; + + // then clean data to be sent back: + memset(command, 0, sizeof(eOmn_command_t)); + + + switch(opc) + { + + case eomn_opc_query_numof_EPs: + { // must give back the number of endpoints + cmdreplynumof->opcpar.opc = eomn_opc_reply_numof_EPs; + cmdreplynumof->opcpar.endpoint = eoprot_endpoint_all; + cmdreplynumof->opcpar.numberof = eoprot_endpoints_numberof_get(eoprot_board_localboard); + + + // ok, now i send the occasional rop + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replynumof); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + //eo_transceiver_OccasionalROP_Load(theems00transceiver, &ropdesc); + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); // it also alert someone to send the rop + } break; + + + case eomn_opc_query_numof_ENs: + { // must give back the number of entities + cmdreplynumof->opcpar.opc = eomn_opc_reply_numof_ENs; + cmdreplynumof->opcpar.endpoint = endpoint; + if(eoprot_endpoint_all == endpoint) + { + cmdreplynumof->opcpar.numberof = eoprot_entities_numberof_get(eoprot_board_localboard); + } + else + { + cmdreplynumof->opcpar.numberof = eoprot_entities_in_endpoint_numberof_get(eoprot_board_localboard, endpoint); + } + + // ok, now i send the occasional rop + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replynumof); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + //eo_transceiver_OccasionalROP_Load(theems00transceiver, &ropdesc); + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); + } break; + + + case eomn_opc_query_numof_REGROPs: + { + cmdreplynumof->opcpar.opc = eomn_opc_reply_numof_REGROPs; + cmdreplynumof->opcpar.endpoint = endpoint; + + if(eoprot_endpoint_all == endpoint) + { + cmdreplynumof->opcpar.numberof = eo_transceiver_RegularROP_ArrayID32Size(theems00transceiver); + } + else + { + cmdreplynumof->opcpar.numberof = eo_transceiver_RegularROP_ArrayID32SizeWithEP(theems00transceiver, endpoint); + } + + // ok, now i send the occasional rop + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replynumof); + ropdesc.data = NULL; // so that data from the EOnv is retrieved. + //eo_transceiver_OccasionalROP_Load(theems00transceiver, &ropdesc); + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); + + } break; + + + default: + { + + } break; + } + + +} + + +static void s_eoprot_ep_mn_fun_queryarraycommand(eOmn_command_t* command) +{ + EOtransceiver* theems00transceiver; + eOmn_cmd_queryarray_t* cmdqueryarray = (eOmn_cmd_queryarray_t*)&command->cmd; + eOmn_cmd_replyarray_t* cmdreplyarray = (eOmn_cmd_replyarray_t*)&command->cmd; + + eOropdescriptor_t ropdesc; + + eOmn_opc_t opc = (eOmn_opc_t) cmdqueryarray->opcpar.opc; + + + if(NULL == (theems00transceiver = eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle()))) + { + return; + } + + // retrieve all useful parametes + uint8_t endpoint = cmdqueryarray->opcpar.endpoint; + uint8_t setnumber = cmdqueryarray->opcpar.setnumber; + uint8_t setsize = cmdqueryarray->opcpar.setsize; + // then clean data to be sent back: + memset(command, 0, sizeof(eOmn_command_t)); + + + switch(opc) + { + + case eomn_opc_query_array_EPs: + { + uint8_t capacity = (sizeof(cmdreplyarray->array) - sizeof(eOarray_head_t)) / sizeof(eOnvEP8_t); + if(0 == setsize) + { + setsize = capacity; + } + + // ok... now i form a rop to send back. at first i write into the nv. + EOarray* ep08array = eo_array_New(setsize, sizeof(eOnvEP8_t), cmdreplyarray->array); + //uint8_t total = eoprot_endpoints_numberof_get(eoprot_board_localboard); + + // ok, prepare the occasional rops + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + + eo_array_Reset(ep08array); + cmdreplyarray->opcpar.opc = eomn_opc_reply_array_EPs; + cmdreplyarray->opcpar.endpoint = eoprot_endpoint_all; + cmdreplyarray->opcpar.setnumber = setnumber; + cmdreplyarray->opcpar.setsize = setsize; + eoprot_endpoints_array_get(eoprot_board_localboard, ep08array, setnumber*setsize); + //eo_transceiver_OccasionalROP_Load(theems00transceiver, &ropdesc); + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); + + } break; + + + case eomn_opc_query_array_EPdes: + { + uint8_t capacity = (sizeof(cmdreplyarray->array) - sizeof(eOarray_head_t)) / sizeof(eoprot_endpoint_descriptor_t); + if(0 == setsize) + { + setsize = capacity; + } + + EOarray* epdesarray = eo_array_New(setsize, sizeof(eoprot_endpoint_descriptor_t), cmdreplyarray->array); + //uint8_t total = eoprot_endpoints_numberof_get(eoprot_board_localboard); + + // ok, prepare the occasional rops + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + + eo_array_Reset(epdesarray); + cmdreplyarray->opcpar.opc = eomn_opc_reply_array_EPdes; + cmdreplyarray->opcpar.endpoint = eoprot_endpoint_all; + cmdreplyarray->opcpar.setnumber = setnumber; + cmdreplyarray->opcpar.setsize = setsize; + eoprot_endpoints_arrayofdescriptors_get(eoprot_board_localboard, epdesarray, setnumber*setsize); + //eo_transceiver_OccasionalROP_Load(theems00transceiver, &ropdesc); + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); + + } break; + + + case eomn_opc_query_array_ENdes: + { + uint8_t capacity = (sizeof(cmdreplyarray->array) - sizeof(eOarray_head_t)) / sizeof(eoprot_entity_descriptor_t); + if(0 == setsize) + { + setsize = capacity; + } + + EOarray* endesarray = eo_array_New(setsize, sizeof(eoprot_entity_descriptor_t), cmdreplyarray->array); + //uint8_t total = eoprot_entities_numberof_get(eoprot_board_localboard); + + // ok, prepare the occasional rops + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + + eo_array_Reset(endesarray); + cmdreplyarray->opcpar.opc = eomn_opc_reply_array_ENdes; + cmdreplyarray->opcpar.endpoint = endpoint; + cmdreplyarray->opcpar.setnumber = setnumber; + cmdreplyarray->opcpar.setsize = setsize; + + if(eoprot_endpoint_all == endpoint) + { + eoprot_entities_arrayofdescriptors_get(eoprot_board_localboard, endesarray, setnumber*setsize); + } + else + { + eoprot_entities_in_endpoint_arrayofdescriptors_get(eoprot_board_localboard, endpoint, endesarray, setnumber*setsize); + } + + //eo_transceiver_OccasionalROP_Load(theems00transceiver, &ropdesc); + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); + + + } break; + + + case eomn_opc_query_array_REGROPs: + { + uint8_t capacity = (sizeof(cmdreplyarray->array) - sizeof(eOarray_head_t)) / sizeof(eOnvID32_t); + if(0 == setsize) + { + setsize = capacity; + } + + // ok... now i form a rop to send back. at first i write into the nv. + EOarray* id32array = eo_array_New(setsize, sizeof(eOnvID32_t), cmdreplyarray->array); + // we have total rops ... + uint8_t total = eo_transceiver_RegularROP_ArrayID32Size(theems00transceiver); + + + // ok, prepare the occasional rops + memcpy(&ropdesc, &eok_ropdesc_basic, sizeof(eok_ropdesc_basic)); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_comm, 0, eoprot_tag_mn_comm_cmmnds_command_replyarray); + ropdesc.data = NULL; // so that dat from teh EOnv is retrieved. + + eo_array_Reset(id32array); + cmdreplyarray->opcpar.opc = eomn_opc_reply_array_REGROPs; + cmdreplyarray->opcpar.endpoint = endpoint; + cmdreplyarray->opcpar.setnumber = setnumber; + cmdreplyarray->opcpar.setsize = setsize; + + if(eoprot_endpoint_all == endpoint) + { + eo_transceiver_RegularROP_ArrayID32Get(theems00transceiver, setnumber*setsize, id32array); + } + else + { + eo_transceiver_RegularROP_ArrayID32GetWithEP(theems00transceiver, endpoint, setnumber*setsize, id32array); + } + + //eo_transceiver_OccasionalROP_Load(theems00transceiver, &ropdesc); + eom_emsappl_Transmit_OccasionalROP(eom_emsappl_GetHandle(), &ropdesc); + + } break; + + default: + { + + } break; + } + + +} + +static void s_eoprot_ep_mn_fun_configcommand(eOmn_command_t* command) +{ + uint8_t size, i; + eOropSIGcfg_t *sigcfg; + eOropdescriptor_t ropdesc; + EOtransceiver* theems00transceiver; + + //eOmn_cmd_config_t* cmdconfig = (eOmn_cmd_config_t*)&command->cmd; /////// not correct. + eOmn_cmd_config_t* cmdconfig = &command->cmd.config; + EOarray *array = (EOarray*)cmdconfig->array; + + uint16_t targetcapacity = (sizeof(cmdconfig->array) - sizeof(eOarray_head_t)) / sizeof(eOropSIGcfg_t); + + + eOmn_opc_t opc = (eOmn_opc_t) cmdconfig->opcpar.opc; + + eOresult_t res; + + if(NULL == (theems00transceiver = eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle()))) + { + return; + } + + + switch(opc) + { + + case eomn_opc_config_REGROPs_clear: + { // just clear + eo_transceiver_RegularROPs_Clear(theems00transceiver); + } break; + + case eomn_opc_config_REGROPs_assign: + { // clear and load all the sigcfg in the array + + if((eo_array_ItemSize(array) != sizeof(eOropSIGcfg_t)) || (targetcapacity != eo_array_Capacity(array)) || ((size = eo_array_Size(array)) > targetcapacity)) + { + return; + } + + eo_transceiver_RegularROPs_Clear(theems00transceiver); + + for(i=0; iopcpar.plustime) ? (1) : (0); + ropdesc.control.plussign = (eobool_true == cmdconfig->opcpar.plussign) ? (1) : (0); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = sigcfg->id32; + ropdesc.signature = cmdconfig->opcpar.signature; + res = eo_transceiver_RegularROP_Load(theems00transceiver, &ropdesc); + res = res; + if(eores_OK != res) + { + eOerrmanDescriptor_t errdes = {0}; + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.par16 = 0; + errdes.par64 = sigcfg->id32; + errdes.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_comm_cannotloadaregularrop); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); + } + } + } break; + + case eomn_opc_config_REGROPs_append: + { // dont clear and load all the sigcfg in the array + if((eo_array_ItemSize(array) != sizeof(eOropSIGcfg_t)) || (targetcapacity != eo_array_Capacity(array)) || ((size = eo_array_Size(array)) > targetcapacity)) + { + return; + } + + for(i=0; iopcpar.plustime) ? (1) : (0); + ropdesc.control.plussign = (eobool_true == cmdconfig->opcpar.plussign) ? (1) : (0); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = sigcfg->id32; + ropdesc.signature = cmdconfig->opcpar.signature; + res = eo_transceiver_RegularROP_Load(theems00transceiver, &ropdesc); + res = res; + if(eores_OK != res) + { + eOerrmanDescriptor_t errdes = {0}; + errdes.sourcedevice = eo_errman_sourcedevice_localboard; + errdes.sourceaddress = 0; + errdes.par16 = 0; + errdes.par64 = sigcfg->id32; + errdes.code = eoerror_code_get(eoerror_category_Config, eoerror_value_CFG_comm_cannotloadaregularrop); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); + } + } + } break; + + case eomn_opc_config_REGROPs_remove: + { // remove all the sigcfg in the array + if((eo_array_ItemSize(array) != sizeof(eOropSIGcfg_t)) || (targetcapacity != eo_array_Capacity(array)) || ((size = eo_array_Size(array)) > targetcapacity)) + { + return; + } + + for(i=0; iopcpar.plustime) ? (1) : (0); + ropdesc.control.plussign = (eobool_true == cmdconfig->opcpar.plussign) ? (1) : (0); + ropdesc.ropcode = eo_ropcode_sig; + ropdesc.id32 = sigcfg->id32; + ropdesc.signature = cmdconfig->opcpar.signature; + res = eo_transceiver_RegularROP_Unload(theems00transceiver, &ropdesc); + res = res; + } + } break; + + case eomn_opc_config_PROT_boardnumber: + { // simply set the byte in array[0] as the new localboard number + //eOprotBRD_t brdnum = command->cmd.config.array[0]; + eoprot_config_board_local(command->cmd.config.array[0]); + } break; + + case eomn_opc_config_PROT_endpoint: + { // simply use the bytes in array[0->7] as a eOprot_EPcfg_t. but only if the EP is valid and not loaded yet. + eOprot_EPcfg_t *epcfg = (eOprot_EPcfg_t*) &command->cmd.config.array[0]; + if(eobool_true == eoprot_EPcfg_isvalid(epcfg)) + { + if(eobool_false == eoprot_endpoint_configured_is(eoprot_board_localboard, epcfg->endpoint)) + { + EOnvSet* nvset = eom_emstransceiver_GetNVset(eom_emstransceiver_GetHandle()); + eo_nvset_LoadEP(nvset, epcfg, eobool_true); + } + } + } break; + + + default: + { + + } break; + } + + +} + + +static void s_eoprot_ep_mn_fun_apply_config_txratedivider(uint8_t txratedivider) +{ + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status); + eOmn_appl_status_t *status = (eOmn_appl_status_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); + status->txdecimationfactor = txratedivider; + + eom_emsrunner_Set_TXdecimationFactor(eom_emsrunner_GetHandle(), txratedivider); +} + +static void s_eoprot_ep_mn_fun_apply_config(uint32_t period, uint16_t rxtime, uint16_t dotime, uint16_t txtime, uint8_t txratedivider) +{ + eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_management, eoprot_entity_mn_appl, 0, eoprot_tag_mn_appl_status); + eOmn_appl_status_t *status = (eOmn_appl_status_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); + + status->cloop_timings[0] = rxtime; + status->cloop_timings[1] = dotime; + status->cloop_timings[2] = txtime; + status->txdecimationfactor = txratedivider; + + eOemsrunner_timing_t timing = {0}; + timing.period = period; + timing.safetygap = 0; + timing.rxstartafter = 0; + timing.dostartafter = rxtime; + timing.txstartafter = rxtime + dotime; + + eom_emsrunner_SetTiming(eom_emsrunner_GetHandle(), &timing); + + eom_emsrunner_Set_TXdecimationFactor(eom_emsrunner_GetHandle(), txratedivider); +} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_overridden_fun.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_overridden_fun.h new file mode 100644 index 0000000000..30e4aa58e2 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolMN_overridden_fun.h @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef _EOPROTOCOMN_OVERRIDDEN_FUN_H_ +#define _EOPROTOCOMN_OVERRIDDEN_FUN_H_ + + + + +/** @file EoProtocolMN_overridden_fun.h + @brief This header file specifies which functions are overridden in the management of management + @author marco.accame@iit.it + @date 06/06/2013 +**/ + +/** @defgroup eo_asfdgr1231 Functions overridden in management endpoint + edfefle. + + @{ + **/ + + + +// - external dependencies -------------------------------------------------------------------------------------------- +// empty-section + + + + +// - public #define -------------------------------------------------------------------------------------------------- + +//#define OVERRIDE_eoprot_fun_INIT_mn_comm_cmmnds_ropsigcfg + + +// - declaration of public user-defined types ------------------------------------------------------------------------- +// empty-section + + +// - declaration of extern public variables, ... but better using use _get/_set instead ------------------------------- +// empty-section + +// - declaration of extern public functions --------------------------------------------------------------------------- +// empty-section + + + + +/** @} + end of group eo_asfdgr1231 + **/ + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolSK_fun_ems4rd.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolSK_fun_ems4rd.c new file mode 100644 index 0000000000..0b75ba3efb --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolSK_fun_ems4rd.c @@ -0,0 +1,129 @@ +/* + * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia + * Author: Valentina Gaggero + * email: valentina.gaggero@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +/* @file EoProtocolSK_fun_ems4rd.c + @brief This file keeps the user-defined functions used in every ems board ebx for endpoint mc + @author valentina.gaggero@iit.it + @date 05/04/2012 +**/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + + +#include "EOtheSKIN.h" + + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EoProtocolSK.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + +extern void eoprot_fun_UPDT_sk_skin_config_sigmode(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOsk_sigmode_t *sigmode = (eOsk_sigmode_t*)rd->data; + eOprotIndex_t index = eoprot_ID2index(rd->id32); + eo_skin_SetMode(eo_skin_GetHandle(), index, *sigmode); +} + + +extern void eoprot_fun_UPDT_sk_skin_cmmnds_boardscfg(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOsk_cmd_boardsCfg_t *brdCfg = (eOsk_cmd_boardsCfg_t*)rd->data; + eOprotIndex_t index = eoprot_ID2index(rd->id32); + eo_skin_SetBoardsConfig(eo_skin_GetHandle(), index, brdCfg); +} + + +extern void eoprot_fun_UPDT_sk_skin_cmmnds_trianglescfg(const EOnv* nv, const eOropdescriptor_t* rd) +{ + eOsk_cmd_trianglesCfg_t *trgsCfg = (eOsk_cmd_trianglesCfg_t*)rd->data; + eOprotIndex_t index = eoprot_ID2index(rd->id32); + eo_skin_SetTrianglesConfig(eo_skin_GetHandle(), index, trgsCfg); +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolSK_overridden_fun.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolSK_overridden_fun.h new file mode 100644 index 0000000000..ff0976a030 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/EoProtocolSK_overridden_fun.h @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef _EOPROTOCOLSK_OVERRIDDEN_FUN_H_ +#define _EOPROTOCOLSK_OVERRIDDEN_FUN_H_ + + + + +/** @file EoProtocolSK_overridden_fun.h + @brief This header file specifies which functions are overridden in the skin of skin + @author marco.accame@iit.it + @date 06/06/2013 +**/ + +/** @defgroup eo_asfdgr1231 Functions overridden in skin endpoint + edfefle. + + @{ + **/ + + + +// - external dependencies -------------------------------------------------------------------------------------------- +// empty-section + + + + +// - public #define -------------------------------------------------------------------------------------------------- + +//#define OVERRIDE_eoprot_fun_INIT_sk_skin_status_arrayof10canframes + + +// - declaration of public user-defined types ------------------------------------------------------------------------- +// empty-section + + +// - declaration of extern public variables, ... but better using use _get/_set instead ------------------------------- +// empty-section + +// - declaration of extern public functions --------------------------------------------------------------------------- +// empty-section + + + + +/** @} + end of group eo_asfdgr1231 + **/ + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotASperiodic_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotASperiodic_amc.c new file mode 100644 index 0000000000..07c57f25d4 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotASperiodic_amc.c @@ -0,0 +1,239 @@ + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + + +#include "stdlib.h" +#include "EoCommon.h" + +// in here is whatever is required to offer parsing of can frames and copying into protocol data structures. + +#include "EoProtocol.h" +#include "EoProtocolAS.h" + +// but also to retrieve information of other things ... +#include "EOtheErrorManager.h" +#include "EoError.h" + +#include "EOtheCANmapping.h" + +#include "embot_app_eth_theFTservice.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOtheCANprotocol_functions.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static void s_former_PER_AS_prepare_frame(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t len, uint8_t type); + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + +extern eOresult_t eocanprotASperiodic_parser_PER_AS_MSG__USERDEF(eOcanframe_t *frame, eOcanport_t port) +{ + // marco.accame on 08apr2019: here is support for reception of a userdef message + return(eores_NOK_generic); +} + + +extern eOresult_t eocanprotASperiodic_parser_PER_AS_MSG__POS(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_NOK_generic); +} + + +extern eOresult_t eocanprotASperiodic_parser_PER_AS_MSG__UNCALIBFORCE_VECTOR_DEBUGMODE(eOcanframe_t *frame, eOcanport_t port) +{ + //// this can frame is from strain only ... i dont do the check that the board must be a strain + //eo_strain_AcceptCANframe(eo_strain_GetHandle(), frame, port, processDebugForce); + + embot::app::eth::theFTservice::canFrameDescriptor cfd + { + port, + frame, + embot::app::eth::theFTservice::canFrameDescriptor::Type::unspecified + }; + embot::app::eth::theFTservice::getInstance().AcceptCANframe(cfd); + + return(eores_OK); +} + + +extern eOresult_t eocanprotASperiodic_parser_PER_AS_MSG__UNCALIBTORQUE_VECTOR_DEBUGMODE(eOcanframe_t *frame, eOcanport_t port) +{ + // this can frame is from strain only ... i dont do the check that the board must be a strain + //eo_strain_AcceptCANframe(eo_strain_GetHandle(), frame, port, processDebugTorque); + + embot::app::eth::theFTservice::canFrameDescriptor cfd + { + port, + frame, + embot::app::eth::theFTservice::canFrameDescriptor::Type::unspecified + }; + embot::app::eth::theFTservice::getInstance().AcceptCANframe(cfd); + + return(eores_OK); +} + +extern eOresult_t eocanprotASperiodic_former_PER_AS_MSG__FORCE_VECTOR(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_PER_AS_prepare_frame(descriptor, frame, 6, ICUBCANPROTO_PER_AS_MSG__FORCE_VECTOR); + memcpy(&frame->data[0], descriptor->cmd.value, 6); + return(eores_OK); +} + + +extern eOresult_t eocanprotASperiodic_parser_PER_AS_MSG__FORCE_VECTOR(eOcanframe_t *frame, eOcanport_t port) +{ + // this can frame is from strain only ... i dont do the check that the board must be a strain + // process force + //eo_strain_AcceptCANframe(eo_strain_GetHandle(), frame, port, processForce); + + embot::app::eth::theFTservice::canFrameDescriptor cfd + { + port, + frame, + embot::app::eth::theFTservice::canFrameDescriptor::Type::force + }; + embot::app::eth::theFTservice::getInstance().AcceptCANframe(cfd); + + return(eores_OK); +} + + +extern eOresult_t eocanprotASperiodic_former_PER_AS_MSG__TORQUE_VECTOR(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_PER_AS_prepare_frame(descriptor, frame, 6, ICUBCANPROTO_PER_AS_MSG__TORQUE_VECTOR); + memcpy(&frame->data[0], descriptor->cmd.value, 6); + return(eores_OK); +} + + +extern eOresult_t eocanprotASperiodic_parser_PER_AS_MSG__TORQUE_VECTOR(eOcanframe_t *frame, eOcanport_t port) +{ + // this can frame is from strain only ... i dont do the check that the board must be a strain + // process torque + //eo_strain_AcceptCANframe(eo_strain_GetHandle(), frame, port, processTorque); + + embot::app::eth::theFTservice::canFrameDescriptor cfd + { + port, + frame, + embot::app::eth::theFTservice::canFrameDescriptor::Type::torque + }; + embot::app::eth::theFTservice::getInstance().AcceptCANframe(cfd); + + return(eores_OK); +} + + +extern eOresult_t eocanprotASperiodic_parser_PER_AS_MSG__HES0TO6(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_NOK_generic); +} + +extern eOresult_t eocanprotASperiodic_parser_PER_AS_MSG__HES7TO14(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_NOK_generic); +} + + +extern eOresult_t eocanprotASperiodic_parser_PER_AS_MSG__THERMOMETER_MEASURE(eOcanframe_t *frame, eOcanport_t port) +{ + embot::app::eth::theFTservice::canFrameDescriptor cfd + { + port, + frame, + embot::app::eth::theFTservice::canFrameDescriptor::Type::temperature + }; + embot::app::eth::theFTservice::getInstance().AcceptCANframe(cfd); + + +// if(eobool_true == eocanprotASperiodic_redefinable_SkipParsingOf_ANY_PERIODIC_THERMOMETER_MSG(frame, port)) +// { +// return(eores_OK); +// } + +// eo_temperatures_AcceptCANframe(eo_temperatures_GetHandle(), eoas_temperature_t1, frame, port); + + return(eores_OK); +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + + +static void s_former_PER_AS_prepare_frame(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t len, uint8_t type) +{ // for periodic the descriptor->address contains ... the origin + uint8_t origin = descriptor->loc.addr; + frame->id = EOCANPROT_CREATE_CANID_PERIODIC(eocanprot_msgclass_periodicAnalogSensor, origin, type); + frame->id_type = eocanframeID_std11bits; + frame->frame_type = eocanframetype_data; + frame->size = len; +} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotASpolling_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotASpolling_amc.c new file mode 100644 index 0000000000..f795337174 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotASpolling_amc.c @@ -0,0 +1,434 @@ + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + + +#include "stdlib.h" +#include "EoCommon.h" + +// in here is whatever is required to offer parsing of can frames and copying into protocol data structures. + +#include "EoProtocol.h" +#include "EoProtocolAS.h" + +#include "EOtheCANmapping.h" + + + +#include "EOtheCANservice.h" +#include "EOtheCANdiscovery2.h" + + +#include "EOtheErrorManager.h" + +#include "EoError.h" + +#include "embot_app_eth_theFTservice.h" + +// but also to retrieve information of other things ... + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOtheCANprotocol_functions.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static void s_former_POL_AS_prepare_frame(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t len, uint8_t type); + +static void* s_eocanprotASpolling_get_entity(eOprotEndpoint_t endpoint, eOprot_entity_t entity, eOcanframe_t *frame, eOcanport_t port, uint8_t *index); + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__SET_TXMODE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 2, ICUBCANPROTO_POL_AS_CMD__SET_TXMODE); + eOenum08_t *txmode = (eOenum08_t *)descriptor->cmd.value; + frame->data[1] = *txmode; + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__SET_CANDATARATE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 2, ICUBCANPROTO_POL_AS_CMD__SET_CANDATARATE); + uint8_t *datarate = (uint8_t*)descriptor->cmd.value; + frame->data[1] = *datarate; + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__SET_RESOLUTION(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 2, ICUBCANPROTO_POL_AS_CMD__SET_RESOLUTION); + eOas_maisresolution_t *maisresolution = (eOas_maisresolution_t*)descriptor->cmd.value; + eOresult_t res = eores_OK; + + switch(*maisresolution) + { + case eoas_maisresolution_08: + { + frame->data[1] = 2; + } break; + + case eoas_maisresolution_16: + { + frame->data[1] = 0; + } break; + + case eoas_maisresolution_debug: + { + frame->data[1] = 1; + } break; + + default: + { + res = eores_NOK_generic; + } break; + } + + return(res); +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__GET_FULL_SCALES(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 2, ICUBCANPROTO_POL_AS_CMD__GET_FULL_SCALES); + uint8_t *channel = (uint8_t*)descriptor->cmd.value; + frame->data[1] = *channel; + return(eores_OK); +} + + + +// this function receives the full scale of a channel and asks object EOtheSTRAIN to take care of it +extern eOresult_t eocanprotASpolling_parser_POL_AS_CMD__GET_FULL_SCALES(eOcanframe_t *frame, eOcanport_t port) +{ +// eo_strain_AcceptCANframe(eo_strain_GetHandle(), frame, port, processFullScale); + + embot::app::eth::theFTservice::canFrameDescriptor cfd + { + port, + frame, + embot::app::eth::theFTservice::canFrameDescriptor::Type::fullscale + }; + embot::app::eth::theFTservice::getInstance().AcceptCANframe(cfd); + + return(eores_OK); +} + + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__GET_FIRMWARE_VERSION(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + eObrd_protocolversion_t *reqprot = (eObrd_protocolversion_t*)descriptor->cmd.value; + + s_former_POL_AS_prepare_frame(descriptor, frame, 3, ICUBCANPROTO_POL_AS_CMD__GET_FW_VERSION); + frame->data[1] = reqprot->major; + frame->data[2] = reqprot->minor; + + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_parser_POL_AS_CMD__GET_FIRMWARE_VERSION(eOcanframe_t *frame, eOcanport_t port) +{ + eObrd_canlocation_t loc = {0}; + loc.port = port; + loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); + loc.insideindex = EOCANPROT_FRAME_POLLING_MC_GET_INTERNALINDEX(frame); + + eObool_t match = (1 == frame->data[7]) ? eobool_true : eobool_false; + + eObrd_info_t detected = {0}; + detected.type = frame->data[1]; + detected.firmware.major = frame->data[2]; + detected.firmware.minor = frame->data[3]; + detected.firmware.build = frame->data[4]; + detected.protocol.major = frame->data[5]; + detected.protocol.minor = frame->data[6]; + + eo_candiscovery2_OneBoardIsFound(eo_candiscovery2_GetHandle(), loc, match, &detected); + + return eores_OK; +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__IMU_CONFIG_GET(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_parser_POL_AS_CMD__IMU_CONFIG_GET(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__IMU_CONFIG_SET(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_POL_AS_CMD__IMU_CONFIG_SET); + + icubCanProto_imu_config_t *cfg = (icubCanProto_imu_config_t*)descriptor->cmd.value; + frame->data[1] = cfg->enabledsensors & 0xff; + frame->data[2] = (cfg->enabledsensors >> 8) & 0xff; + frame->data[3] = cfg->fusionmode; + frame->data[4] = cfg->ffu[0]; + frame->data[5] = cfg->ffu[1]; + frame->data[6] = cfg->ffu[2]; + frame->data[7] = cfg->ffu[3]; + + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__IMU_TRANSMIT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 2, ICUBCANPROTO_POL_AS_CMD__IMU_TRANSMIT); + + icubCanProto_imu_transmit_t *trs = (icubCanProto_imu_transmit_t*)descriptor->cmd.value; + frame->data[1] = trs->period; + + return(eores_OK); +} + + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__THERMOMETER_CONFIG_SET(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 2, ICUBCANPROTO_POL_AS_CMD__THERMOMETER_CONFIG_SET); + + icubCanProto_thermo_config_t *tc = (icubCanProto_thermo_config_t*)descriptor->cmd.value; + frame->data[1] = tc->sensormask; + + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__THERMOMETER_TRANSMIT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 2, ICUBCANPROTO_POL_AS_CMD__THERMOMETER_TRANSMIT); + + icubCanProto_thermo_transmit_t *tt = (icubCanProto_thermo_transmit_t*)descriptor->cmd.value; + frame->data[1] = tt->periodsec; + + return(eores_OK); +} + + +extern eOresult_t eocanprotASpolling_former_POL_SK_CMD__TACT_SETUP(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_POL_SK_CMD__TACT_SETUP); + + /* 2) set can command (see SkinPrototype::calibrateSensor)*/ + frame->data[1] = 0x01; //==> risoluzione 8 bit e 12 isure indipendenti + frame->data[2] = 0x01; //==> invia ogni 40 milli + frame->data[3] = 0x03; + frame->data[4] = 0; + frame->data[5] = 0x20; + frame->data[6] = 0; + frame->data[7] = 0x35; + + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_SK_CMD__SET_BRD_CFG(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 4, ICUBCANPROTO_POL_SK_CMD__SET_BRD_CFG); + + icubCanProto_skinboard_config_t* bcfg = (icubCanProto_skinboard_config_t *)descriptor->cmd.value; + /* 2) set can command (see SkinPrototype::calibrateSensor)*/ + frame->data[0] = ICUBCANPROTO_POL_SK_CMD__SET_BRD_CFG; + frame->data[1] = (bcfg->skintype &0x07); + frame->data[2] = bcfg->period; + frame->data[3] = bcfg->noload; + + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_SK_CMD__TACT_SETUP2(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 7, ICUBCANPROTO_POL_SK_CMD__TACT_SETUP2); + + /* 2) set can command (see SkinPrototype::calibrateSensor)*/ + frame->data[1] = 0x00; + frame->data[2] = 0x22; + frame->data[3] = 0xf0; + frame->data[4] = 0x00; + frame->data[5] = 0xFF; + frame->data[6] = 0xff; + + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_SK_CMD__ACC_GYRO_SETUP(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 3, ICUBCANPROTO_POL_SK_CMD__ACC_GYRO_SETUP); + + icubCanProto_inertial_config_t *config = (icubCanProto_inertial_config_t*)descriptor->cmd.value; + frame->data[1] = config->enabledsensors; + frame->data[2] = config->period; + + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_SK_CMD__SET_TRIANG_CFG(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 7, ICUBCANPROTO_POL_SK_CMD__SET_TRIANG_CFG); + + icubCanProto_skintriangles_config_t * tcfg = (icubCanProto_skintriangles_config_t *)descriptor->cmd.value; + + /* 2) set can command (see SkinPrototype::calibrateSensor)*/ + + frame->data[1] = tcfg->idstart; + frame->data[2] = tcfg->idend; + frame->data[3] = tcfg->shift; + frame->data[4] = tcfg->flags; + *((uint16_t*)(&frame->data[5])) = tcfg->CDCoffset; + + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__POS_CONFIG_GET(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + //#warning see if we have anything to set ... maybe we dont ask and it does not tell + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_parser_POL_AS_CMD__POS_CONFIG_GET(eOcanframe_t *frame, eOcanport_t port) +{ + //#warning see if we have anything to retrieve ... maybe it does not tell because we dont ask + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__POS_CONFIG_SET(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_POL_AS_CMD__POS_CONFIG_SET); + + icubCanProto_POS_CONFIG_t *cfg = (icubCanProto_POS_CONFIG_t*)descriptor->cmd.value; + frame->data[1] = cfg->type & 0xff; + if(icubCanProto_pos_decideg == cfg->type) + { + frame->data[1] = icubCanProto_pos_decideg; + // we have settings for two sensors ... + frame->data[2] = (cfg->setting.decideg[0].enabled << 7) | (cfg->setting.decideg[0].invertdirection << 6) | (cfg->setting.decideg[0].rotation << 4) | (cfg->setting.decideg[0].label & 0x0f); + frame->data[3] = (cfg->setting.decideg[0].offset & 0x00ff); // lsb of zero + frame->data[4] = (cfg->setting.decideg[0].offset & 0xff00) >> 8; // msb of zero + + frame->data[5] = (cfg->setting.decideg[1].enabled << 7) | (cfg->setting.decideg[1].invertdirection << 6) | (cfg->setting.decideg[1].rotation << 4) | (cfg->setting.decideg[1].label & 0x0f); + frame->data[6] = (cfg->setting.decideg[1].offset & 0x00ff); // lsb of zero + frame->data[7] = (cfg->setting.decideg[1].offset & 0xff00) >> 8; // msb of zero + } + else + { + frame->data[1] = icubCanProto_pos_unkwown; + memmove(&frame->data[2], cfg->setting.unknown, sizeof(cfg->setting.unknown)); + } + + return(eores_OK); +} + +extern eOresult_t eocanprotASpolling_former_POL_AS_CMD__POS_TRANSMIT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_AS_prepare_frame(descriptor, frame, 2, ICUBCANPROTO_POL_AS_CMD__POS_TRANSMIT); + + icubCanProto_POS_TRANSMIT_t *trs = (icubCanProto_POS_TRANSMIT_t*)descriptor->cmd.value; + frame->data[1] = trs->rate; + + return(eores_OK); +} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static void s_former_POL_AS_prepare_frame(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t len, uint8_t type) +{ // every message coming from the ems has actually id 0 + frame->id = EOCANPROT_CREATE_CANID(eocanprot_msgclass_pollingAnalogSensor, 0, descriptor->loc.addr); + frame->id_type = eocanframeID_std11bits; + frame->frame_type = eocanframetype_data; + frame->size = len; + frame->data[0] = EOCANPROT_CREATE_POLLING_AS_DATA0(type); +} + + +static void* s_eocanprotASpolling_get_entity(eOprotEndpoint_t endpoint, eOprot_entity_t entity, eOcanframe_t *frame, eOcanport_t port, uint8_t *index) +{ + void * ret = NULL; + uint8_t ii = 0; + eObrd_canlocation_t loc = {0}; + + loc.port = port; + loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); + loc.insideindex = eobrd_caninsideindex_none; + + ii = eo_canmap_GetEntityIndex(eo_canmap_GetHandle(), loc, endpoint, entity); + + if(EOK_uint08dummy == ii) + { + //#warning -> TODO: add diagnostics about not found board as in s_eo_icubCanProto_mb_send_runtime_error_diagnostics() + return(NULL); + } + + ret = eoprot_entity_ramof_get(eoprot_board_localboard, endpoint, entity, ii); + + if(NULL != index) + { + *index = ii; + } + + return(ret); +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotBSperiodic_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotBSperiodic_amc.c new file mode 100644 index 0000000000..86e1ba55f1 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotBSperiodic_amc.c @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2022 iCub Facility - Istituto Italiano di Tecnologia + * Author: Luca Tricerri + * email: luca.tricerri@iit.it + * website: www.robotcub.org + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details +*/ + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + + +#include "EoCommon.h" +//#include "embot_app_eth_theBATservice.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOtheCANprotocol_functions.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + +//#include "embot_core.h" +extern eOresult_t eocanprotINperiodic_parser_PER_BS_MSG__INFO(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_OK); +} + +extern eOresult_t eocanprotINperiodic_parser_PER_BS_MSG__STATUS_BMS(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_OK); +} + +extern eOresult_t eocanprotINperiodic_parser_PER_BS_MSG__STATUS_BAT(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_OK); +} + +extern eOresult_t eocanprotINperiodic_parser_PER_BS_MSG__ALLTHEOTHERS(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_OK); +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotISperiodic_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotISperiodic_amc.c new file mode 100644 index 0000000000..6364d80707 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotISperiodic_amc.c @@ -0,0 +1,126 @@ + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + +//#include "EOVtheSystem.h" + +#include "stdlib.h" +#include "EoCommon.h" + +// in here is whatever is required to offer parsing of can frames and copying into protocol data structures. + +#include "EoProtocol.h" +#include "EoProtocolAS.h" + +// but also to retrieve information of other things ... + +#include "EOtheCANmapping.h" + +//#include "EOtheInertials2.h" +//#include "EOtheInertials3.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOtheCANprotocol_functions.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- + + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + +extern eOresult_t eocanprotINperiodic_parser_PER_IS_MSG__DIGITAL_GYROSCOPE(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_NOK_generic); +} + +extern eOresult_t eocanprotINperiodic_parser_PER_IS_MSG__DIGITAL_ACCELEROMETER(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_NOK_generic); +} + + +extern eOresult_t eocanprotINperiodic_parser_PER_IS_MSG__IMU_TRIPLE(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_NOK_generic); +} + + +extern eOresult_t eocanprotINperiodic_parser_PER_IS_MSG__IMU_QUATERNION(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_NOK_generic); +} + +extern eOresult_t eocanprotINperiodic_parser_PER_IS_MSG__IMU_STATUS(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_NOK_generic); +} + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotMCperiodic_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotMCperiodic_amc.c new file mode 100644 index 0000000000..4ef0d2790a --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotMCperiodic_amc.c @@ -0,0 +1,710 @@ + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + + +#include "stdlib.h" +#include "EoCommon.h" + +// in here is whatever is required to offer parsing of can frames and copying into protocol data structures. + +#include "EoProtocol.h" +#include "EoProtocolMC.h" + +#include "EOtheCANmapping.h" + +// but also to retrieve information of other things ... + +#include "Controller.h" + + +//#include "EOtheMC4boards.h" + +#include "EOtheErrorManager.h" +#include "EoError.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOtheCANprotocol_functions.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static eObool_t s_eocanprotMCperiodic_is_from_unused2foc_in_eb5(eOcanframe_t *frame, eOcanport_t port); + +static void* s_eocanprotMCperiodic_get_entity(eOprot_entity_t entity, eOcanframe_t *frame, eOcanport_t port, eObrd_caninsideindex_t insideindex, uint8_t *index); + +static eObrd_cantype_t s_eocanprotMCperiodic_get_boardtype(eOcanframe_t *frame, eOcanport_t port); + +static eOresult_t s_eocanprotMCperiodic_convert_icubCanProtoControlMode2eOmcControlMode(icubCanProto_controlmode_t icubcanProto_controlmode, + eOmc_controlmode_t *eomc_controlmode); +static eOresult_t s_eocanprotMCperiodic_convert_icubCanProtoInteractionMode2eOmcInteractionMode(icubCanProto_interactionmode_t icubcanProto_intermode, + eOmc_interactionmode_t *eomc_intermode); +static void s_former_PER_MC_prepare_frame(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t len, uint8_t type); + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + + + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__DEBUG(eOcanframe_t *frame, eOcanport_t port) +{ + // i decode this frame only if it comes from 2foc and i discard it if it comes from a mc4. + // in date 19 sept 2017 we decided that the 2foc boards would send this diagnostics frame when they find an error of type tbd + // as a first case we want to use it for debugging the case of motor encoder dirty which happens on icub-v3. + + eOerrmanDescriptor_t des = {0}; + + eObrd_cantype_t boardtype = s_eocanprotMCperiodic_get_boardtype(frame, port); + + if(eobrd_cantype_foc == boardtype) + { + // i just forward the full canframe into a debug message. later on i will add a proper message type. + des.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_tag02); + des.par16 = (frame->id & 0x0fff) | ((frame->size & 0x000f) << 12); + des.par64 = eo_common_canframe_data2u64(frame); + des.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2); + des.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &des); + + } + else if(eobrd_cantype_mc4 == boardtype) + { + // we have a mc4, ... we just drop it + } + else if(eobrd_cantype_amcbldc == boardtype) + { + // amcbldc does not emit this message + } + + return(eores_OK); +} + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__2FOC(eOcanframe_t *frame, eOcanport_t port) +{ + // this can frame is from 2foc only ... as the name of the message says. i dont do the check that the board must be a 2foc + // i retrieve the motor related to the frame + eOmc_motor_t *motor = NULL; + eOprotIndex_t motorindex = EOK_uint08dummy; + + if(NULL == (motor = (eOmc_motor_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_motor, frame, port, eobrd_caninsideindex_first, &motorindex))) + { + if(eobool_true == s_eocanprotMCperiodic_is_from_unused2foc_in_eb5(frame, port)) + { // the board eb5 has an additional 1foc board which sends a can frame of this kind but it does not serve any motor + // or joint. hence, its can address is not recognised by the EOtheCANmapping object. + // for this board we must not issue the error and we must do nothing + return(eores_OK); + } + return(eores_OK); + } + + // note of marco.accame: the following code is ok as long as the 2foc has been configured to send up in its periodic message + // current, velocity, and position. if so, frame->data contains: [current:2bytes, velocity:2bytes, position:4bytes]. + // the following code extract these values. + motor->status.basic.mot_current = ((int16_t*)frame->data)[0]; + motor->status.basic.mot_velocity = ((int16_t*)frame->data)[1]; + motor->status.basic.mot_position = ((int32_t*)frame->data)[1]; + + //eo_emsController_AcquireMotorEncoder(motorindex, motor->status.basic.mot_current, motor->status.basic.mot_velocity, motor->status.basic.mot_position); + + MController_update_motor_odometry_fbk_can(motorindex, frame->data); + + return(eores_OK); +} + + + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__POSITION(eOcanframe_t *frame, eOcanport_t port) +//{ +// // this frame is from mc4 only, thus ... it manages two joints. +// // i retrieve the two joints related to the can frame. +// eOprotIndex_t jointindex = 0; +// eOmc_joint_t *joint = NULL; +// icubCanProto_position_t pos = 0; +// +// uint8_t j=0; +// // the two joints have ... +// const eObrd_caninsideindex_t insideindex[2] = {eobrd_caninsideindex_first, eobrd_caninsideindex_second}; +// const uint8_t offset[2] = {0, 4}; +// for(j=0; j<2; j++) +// { +// if(NULL == (joint = (eOmc_joint_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_joint, frame, port, insideindex[j], &jointindex))) +// { +// return(eores_OK); +// } +// +// pos = *((icubCanProto_position_t*)&(frame->data[offset[j]])); +// joint->status.core.measures.meas_position = eo_mc4boards_Convert_Position_fromCAN(eo_mc4boards_GetHandle(), jointindex, pos); +// } + +// return(eores_OK); +//} +{ + return(eores_NOK_generic); +} + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__MOTOR_POSITION(eOcanframe_t *frame, eOcanport_t port) +{ + // this frame is from mc4 only, thus ... it manages two joints. + // i retrieve the two joints related to the can frame. + eOprotIndex_t motorindex = 0; + eOmc_motor_t *motor = NULL; + icubCanProto_position_t pos = 0; + + uint8_t j=0; + // the two motors have ... + const eObrd_caninsideindex_t insideindex[2] = {eobrd_caninsideindex_first, eobrd_caninsideindex_second}; + const uint8_t offset[2] = {0, 4}; + for(j=0; j<2; j++) + { + if(NULL == (motor = (eOmc_motor_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_motor, frame, port, insideindex[j], &motorindex))) + { + return(eores_OK); + } + + pos = *((icubCanProto_position_t*)&(frame->data[offset[j]])); + motor->status.basic.mot_position = pos; + } + + return(eores_OK); +} + + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__PID_VAL(eOcanframe_t *frame, eOcanport_t port) +{ + // this frame is from mc4 only, thus ... + // i retrieve the two joints related to the can frame. such a frame manages two joints per can address + eOprotIndex_t jointindex = 0; + eOmc_joint_t *joint = NULL; + + + uint8_t j=0; + // the two joints have ... + const eObrd_caninsideindex_t insideindex[2] = {eobrd_caninsideindex_first, eobrd_caninsideindex_second}; + const uint8_t offset[2] = {0, 2}; + for(j=0; j<2; j++) + { + if(NULL == (joint = (eOmc_joint_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_joint, frame, port, insideindex[j], &jointindex))) + { + return(eores_OK); + } + #if 1 + //#warning CAVEAT: jstatus->ofpid.output is now an int16_t and the can frame should have a int16_t ... however it works like that + // marco.accame on 02apr15: i ahve seen together with marco.randazzo that the mc4 send uint16, thus in here there is a double error which make things work + // see s_eo_icubCanProto_parser_per_mb_cmd__pidVal() in ems4rd-v01.uvproj + joint->status.core.ofpid.legacy.output = *((uint16_t*)&(frame->data[offset[j]])); + #else + { + // marco.accame on 02apr15: this is the ways it should be after we have changed the type of jstatus->ofpid.output + // ... and if we consider the content of the can frame as a signed int int16_t .... however the can frame contains a uint16 (as checked in mc4 code) + int16_t value = 0; + value = *((int16_t*)&(frame->data[offset[j]])); + joint->status.ofpid.legacy.output = value; + } + #endif + } + + return(eores_OK); +} + + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__STATUS(eOcanframe_t *frame, eOcanport_t port) +{ + // both 2foc and mc4 can send this frame. the parsing is however different because: + // - the 2foc contains info related to one joint only and it is used the EMScontroller to parse the frame, + // - the mc4 contains info related to two joints and we need to parse the frame directly + + eOprotIndex_t jointindex = 0; + eOmc_joint_t *joint = NULL; + eOmc_controlmode_t eomc_controlmode = eomc_controlmode_idle; + eOerrmanDescriptor_t des = {0}; + + eObrd_cantype_t boardtype = s_eocanprotMCperiodic_get_boardtype(frame, port); + + + if(eobrd_cantype_foc == boardtype || eobrd_cantype_amcbldc == boardtype) + { + // in case we have a 2foc ... i treat the first joint only + // first joint: use eobrd_caninsideindex_first and gets the first 2 bytes of the frame + if(NULL == (joint = (eOmc_joint_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_joint, frame, port, eobrd_caninsideindex_first, &jointindex))) + { + if(eobool_true == s_eocanprotMCperiodic_is_from_unused2foc_in_eb5(frame, port)) + { + return(eores_OK); + } + return(eores_OK); + } + + + // marco.accame: the variable eomc_controlmode is not used because eo_emsController_ReadMotorstatus() gets directly what it needs, + // thus no need to perform the conversion + //res = s_eocanprotMCperiodic_convert_icubCanProtoControlMode2eOmcControlMode((icubCanProto_controlmode_t) frame->data[1], &eomc_controlmode); + //if(eores_OK != res) + //{ + // #warning -> TODO: add diagnostics about not found board as in s_eo_icubCanProto_mb_send_runtime_error_diagnostics() + // return(eores_OK); + //} + //eo_emsController_ReadMotorstatus(jointindex, frame->data); + MController_update_motor_state_fbk(jointindex, frame->data); + // l'aggiornamento delle nv del giunto sara' fatto nel DO. + // se l'appl e' in config sicuramente i giunti sono in idle e quindi non c'e' ninete da aggiornare + // marco.accame: the previous code contained the following function which does nothing but putting into motor.status.filler04[] + // some flags ... probably for debug. well, we dont do it in here, but if needed we can add it. + // s_eo_appTheDB_UpdateMototStatusPtr(mId, frame, runmode); + + } +// else if(eobrd_cantype_mc4 == boardtype) +// { // we have a mc4, thus we must manage two joints +// uint8_t j=0; +// // the two joints have ... +// const eObrd_caninsideindex_t insideindex[2] = {eobrd_caninsideindex_first, eobrd_caninsideindex_second}; +// const uint8_t offset[2] = {1, 3}; +// +// for(j=0; j<2; j++) // 2 joints .... +// { +// // joint i-th +// if(NULL == (joint = (eOmc_joint_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_joint, frame, port, insideindex[j], &jointindex))) +// { +// return(eores_OK); +// } +// // manage controlmode +// if(eores_OK != s_eocanprotMCperiodic_convert_icubCanProtoControlMode2eOmcControlMode((icubCanProto_controlmode_t) frame->data[offset[j]], &eomc_controlmode)) +// { +// //#warning -> TODO: add diagnostics about not found control mode as in s_eo_icubCanProto_mb_send_runtime_error_diagnostics() +// return(eores_OK); +// } +// +// joint->status.core.modes.controlmodestatus = eomc_controlmode; +// +// if(eomc_controlmode_hwFault == eomc_controlmode) +// { +// des.code = eoerror_code_get(eoerror_category_Debug, eoerror_value_DEB_hwfault2); +// des.par16 = (frame->data[1] << 8) | eomc_controlmode; +// des.sourceaddress = jointindex; +// des.sourcedevice = eo_errman_sourcedevice_localboard; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &des); +// } +// } +// } + + return(eores_OK); +} + + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__CURRENT(eOcanframe_t *frame, eOcanport_t port) +{ + // this frame is from mc4 only, thus ... + // i retrieve the two motors related to the can frame. such a frame manages two motors per can address + eOprotIndex_t motorindex = 0; + eOmc_motor_t *motor = NULL; + + uint8_t m=0; + // the two motors have ... + const eObrd_caninsideindex_t insideindex[2] = {eobrd_caninsideindex_first, eobrd_caninsideindex_second}; + const uint8_t offset[2] = {0, 2}; + for(m=0; m<2; m++) + { + if(NULL == (motor = (eOmc_motor_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_motor, frame, port, insideindex[m], &motorindex))) + { + return(eores_OK); + } + motor->status.basic.mot_current = *((uint16_t*)&(frame->data[offset[m]])); + } + + return(eores_OK); +} + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__PRINT(eOcanframe_t *frame, eOcanport_t port) +{ + // this frame contains a can print. i forward it to robotinterface using the error manager + eOerrmanDescriptor_t errdes = {0}; + errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_canservices_canprint); + errdes.par16 = frame->size; + errdes.par64 = eo_common_canframe_data2u64((eOcanframe_t*)frame); + errdes.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2); + errdes.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame); + eo_errman_Error(eo_errman_GetHandle(), eo_errortype_info, NULL, NULL, &errdes); + + return(eores_OK); +} + + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__VELOCITY(eOcanframe_t *frame, eOcanport_t port) +//{ +// // this frame is from mc4 only, thus ... +// // i retrieve the two joints related to the can frame. such a frame manages two joints per can address +// eOprotIndex_t jointindex = 0; +// eOmc_joint_t *joint = NULL; +// icubCanProto_velocity_t vel_icubCanProtValue = 0; +// icubCanProto_acceleration_t acc_icubCanProtValue = 0; +// +// +// uint8_t j=0; +// // the two joints have ... +// const eObrd_caninsideindex_t insideindex[2] = {eobrd_caninsideindex_first, eobrd_caninsideindex_second}; +// const uint8_t offsetvelocity[2] = {0, 4}; +// const uint8_t offsetacceleration[2] = {2, 6}; +// for(j=0; j<2; j++) +// { +// if(NULL == (joint = (eOmc_joint_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_joint, frame, port, insideindex[j], &jointindex))) +// { +// return(eores_OK); +// } +// vel_icubCanProtValue = *((icubCanProto_velocity_t*)&(frame->data[offsetvelocity[j]])); +// acc_icubCanProtValue = *((icubCanProto_acceleration_t*)&(frame->data[offsetacceleration[j]])); +// +// joint->status.core.measures.meas_velocity = eo_mc4boards_Convert_Velocity_fromCAN(eo_mc4boards_GetHandle(), jointindex, vel_icubCanProtValue); +// +// joint->status.core.measures.meas_acceleration = eo_mc4boards_Convert_Acceleration_fromCAN(eo_mc4boards_GetHandle(), jointindex, acc_icubCanProtValue); +// } +// +// return(eores_OK); +//} +{ + return(eores_NOK_generic); +} + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__PID_ERROR(eOcanframe_t *frame, eOcanport_t port) +//{ +// // this frame is from mc4 only, thus ... +// // i retrieve the two joints related to the can frame. such a frame manages two joints per can address +// eOprotIndex_t jointindex = 0; +// eOmc_joint_t *joint = NULL; +// uint16_t pidpos_error = 0; +// uint16_t pidtrq_error = 0; +// +// +// uint8_t j=0; +// // the two joints have ... +// const eObrd_caninsideindex_t insideindex[2] = {eobrd_caninsideindex_first, eobrd_caninsideindex_second}; +// const uint8_t offsetpos[2] = {0, 2}; +// const uint8_t offsettrq[2] = {4, 6}; +// for(j=0; j<2; j++) +// { +// if(NULL == (joint = (eOmc_joint_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_joint, frame, port, insideindex[j], &jointindex))) +// { +// return(eores_OK); +// } +// +// pidpos_error = *((uint16_t*)&(frame->data[offsetpos[j]])); +// pidtrq_error = *((uint16_t*)&(frame->data[offsettrq[j]])); +// +// if(eomc_controlmode_torque == joint->status.core.modes.controlmodestatus) +// { +// joint->status.core.ofpid.legacy.error = pidtrq_error; +// } +// else +// { +// joint->status.core.ofpid.legacy.error = pidpos_error; +// } +// } +// +// return(eores_OK); +//} +{ + return(eores_NOK_generic); +} + + +extern eOresult_t eocanprotMCperiodic_parser_PER_MC_MSG__ADDITIONAL_STATUS(eOcanframe_t *frame, eOcanport_t port) +//{ +// // this frame is from mc4 only, thus ... +// // i retrieve the two joints related to the can frame. such a frame manages two joints per can address +// eOprotIndex_t jointindex = 0; +// eOmc_joint_t *joint = NULL; +// icubCanProto_interactionmode_t caninteractionmodes[2] = {icubCanProto_interactionmode_stiff, icubCanProto_interactionmode_stiff}; +// +// uint8_t j=0; +// // the two joints have ... +// const eObrd_caninsideindex_t insideindex[2] = {eobrd_caninsideindex_first, eobrd_caninsideindex_second}; +// caninteractionmodes[0] = (icubCanProto_interactionmode_t)(frame->data[0] & 0x0f); // for first joint +// caninteractionmodes[1] = (icubCanProto_interactionmode_t)((frame->data[0] & 0xf0) >> 4); // for second joint +// eOmc_interactionmode_t tmp = eOmc_interactionmode_stiff; +// for(j=0; j<2; j++) +// { +// if(NULL == (joint = (eOmc_joint_t*) s_eocanprotMCperiodic_get_entity(eoprot_entity_mc_joint, frame, port, insideindex[j], &jointindex))) +// { +// return(eores_OK); +// } +// +// if(eores_OK != s_eocanprotMCperiodic_convert_icubCanProtoInteractionMode2eOmcInteractionMode(caninteractionmodes[j], &tmp)) +// { +// return(eores_OK); +// } +// joint->status.core.modes.interactionmodestatus = tmp; +// } +// +// return(eores_OK); +//} +{ + return(eores_NOK_generic); +} + + +extern eOresult_t eocanprotMCperiodic_former_PER_MC_MSG__EMSTO2FOC_DESIRED_CURRENT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_PER_MC_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_PER_MC_MSG__EMSTO2FOC_DESIRED_CURRENT); + // all the data is in payload + int16_t *pwmList = (int16_t*)descriptor->cmd.value; + *((uint16_t*)(&frame->data[0])) = pwmList[0]; + *((uint16_t*)(&frame->data[2])) = pwmList[1]; + *((uint16_t*)(&frame->data[4])) = pwmList[2]; + *((uint16_t*)(&frame->data[6])) = pwmList[3]; + + return(eores_OK); +} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static eObool_t s_eocanprotMCperiodic_is_from_unused2foc_in_eb5(eOcanframe_t *frame, eOcanport_t port) +{ + const eOprotBRD_t eb5board = 4; + eOprotBRD_t localboard = eoprot_board_local_get(); + + if(localboard != eb5board) + { + return(eobool_false); + } + + if( (eOcanport1 == port) && (2 == EOCANPROT_FRAME_GET_SOURCE(frame)) ) + { + return(eobool_true); + } + else + { + return(eobool_false); + } +} + + +static void* s_eocanprotMCperiodic_get_entity(eOprot_entity_t entity, eOcanframe_t *frame, eOcanport_t port, eObrd_caninsideindex_t insideindex, uint8_t *index) +{ + void * ret = NULL; + uint8_t ii = 0; + eObrd_canlocation_t loc = {0}; + + loc.port = port; + loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); + loc.insideindex = insideindex; + + ii = eo_canmap_GetEntityIndex(eo_canmap_GetHandle(), loc, eoprot_endpoint_motioncontrol, entity); + + if(EOK_uint08dummy == ii) + { + //#warning -> TODO: add diagnostics about not found board as in s_eo_icubCanProto_mb_send_runtime_error_diagnostics() + return(NULL); + } + + ret = eoprot_entity_ramof_get(eoprot_board_localboard, eoprot_endpoint_motioncontrol, entity, ii); + + if(NULL != index) + { + *index = ii; + } + + return(ret); +} + + +static eObrd_cantype_t s_eocanprotMCperiodic_get_boardtype(eOcanframe_t *frame, eOcanport_t port) +{ + eObrd_canlocation_t loc = {0}; + loc.port = port; + loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); + loc.insideindex = eobrd_caninsideindex_none; // dont care of it if we look for a board + return(eo_canmap_GetBoardType(eo_canmap_GetHandle(), loc)); +} + + +static eOresult_t s_eocanprotMCperiodic_convert_icubCanProtoControlMode2eOmcControlMode(icubCanProto_controlmode_t icubcanProto_controlmode, + eOmc_controlmode_t *eomc_controlmode) +{ + eOresult_t res = eores_OK; + + switch(icubcanProto_controlmode) + { + case icubCanProto_controlmode_idle: + { + *eomc_controlmode = eomc_controlmode_idle; + } break; + + case icubCanProto_controlmode_position: + { + *eomc_controlmode = eomc_controlmode_position; + } break; + + case icubCanProto_controlmode_velocity: + { + *eomc_controlmode = eomc_controlmode_velocity; // + } break; + + case icubCanProto_controlmode_torque: + { + *eomc_controlmode = eomc_controlmode_torque; + } break; + + case icubCanProto_controlmode_impedance_pos: + { + *eomc_controlmode = eomc_controlmode_impedance_pos; + } break; + + case icubCanProto_controlmode_impedance_vel: + { + *eomc_controlmode = eomc_controlmode_impedance_vel; + } break; + + case icubCanProto_controlmode_current: + { + *eomc_controlmode = eomc_controlmode_current; + } break; + case icubCanProto_controlmode_mixed: + { + *eomc_controlmode = eomc_controlmode_mixed; + } break; + + case icubCanProto_controlmode_direct: + { + *eomc_controlmode = eomc_controlmode_direct; + } break; + + case icubCanProto_controlmode_openloop: + { + *eomc_controlmode = eomc_controlmode_openloop; + } break; + + case icubCanProto_controlmode_calibration: + { + *eomc_controlmode = eomc_controlmode_calib; + } break; + + case icubCanProto_controlmode_hwFault: + { + *eomc_controlmode = eomc_controlmode_hwFault; + } break; + + case icubCanProto_controlmode_notConfigured: + { + *eomc_controlmode = eomc_controlmode_notConfigured; + } break; + + case icubCanProto_controlmode_configured: + { + *eomc_controlmode = eomc_controlmode_configured; + } break; + + case icubCanProto_controlmode_unknownError: + { + *eomc_controlmode = eomc_controlmode_unknownError; + } break; + + default: + { + res = eores_NOK_generic; + } break; + } + + return(res); +} + + +static eOresult_t s_eocanprotMCperiodic_convert_icubCanProtoInteractionMode2eOmcInteractionMode(icubCanProto_interactionmode_t icubcanProto_intermode, + eOmc_interactionmode_t *eomc_intermode) +{ + eOresult_t res = eores_OK; + + switch(icubcanProto_intermode) + { + case icubCanProto_interactionmode_stiff: + { + *eomc_intermode = eOmc_interactionmode_stiff; + } break; + + case icubCanProto_interactionmode_compliant: + { + *eomc_intermode = eOmc_interactionmode_compliant; + } break; + + default: + { + res = eores_NOK_generic; + } break; + } + + return(res); +} + +// former helper funtions + +static void s_former_PER_MC_prepare_frame(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t len, uint8_t type) +{ // the source address in actually inside descriptor->address + frame->id = EOCANPROT_CREATE_CANID_PERIODIC(eocanprot_msgclass_periodicMotorControl, descriptor->loc.addr, type); + frame->id_type = eocanframeID_std11bits; + frame->frame_type = eocanframetype_data; + frame->size = len; +} + + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotMCpolling_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotMCpolling_amc.c new file mode 100644 index 0000000000..efd4506589 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotMCpolling_amc.c @@ -0,0 +1,1289 @@ + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + + +#include "stdlib.h" +#include "EoCommon.h" + +// in here is whatever is required to offer parsing of can frames and copying into protocol data structures. + +#include "EOtheCANprotocol.h" + +#include "EoProtocol.h" +#include "EoProtocolMC.h" + +// but also to retrieve information of other things ... + +#include "iCubCanProtocol.h" + +#include "EOproxy.h" +#include "EOtheBOARDtransceiver.h" +#include "EOtheErrorManager.h" +#include "EoError.h" + +#include "EOtheCANmapping.h" + +#include "EOtheCANdiscovery2.h" + +//#include "EOtheMC4boards.h" + +//#include "EOMtheEMSappl.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOtheCANprotocol_functions.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static void* s_eocanprotMCpolling_get_entity(eOprot_entity_t entity, eOcanframe_t *frame, eOcanport_t port, uint8_t *index); + + +// parser helper funtions + +//static eOresult_t s_parser_POL_MC_CMD__MOTION_DONE(eOcanframe_t *frame, eOcanport_t port); + +// former helper funtions + +static void s_former_POL_MC_prepare_frame(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t len, uint8_t type); + +static void s_former_POL_MC_CMD_setpid(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t type); +static void s_former_POL_MC_CMD_setpid_limits(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t type); + +//static void s_former_POL_MC_CMD_setpid_7(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t type); +static void s_former_POL_MC_CMD_setpid_limits_7(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t type); + +static eOresult_t s_parser_POL_MC_CMD_getposition(eOcanframe_t *frame, eOcanport_t port, uint8_t type); +static eOresult_t s_parser_POL_MC_CMD_getpid_etc(eOcanframe_t *frame, eOcanport_t port, uint8_t type); +static eOresult_t s_parser_POL_MC_CMD_getimpedance(eOcanframe_t *frame, eOcanport_t port, uint8_t type); + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__CONTROLLER_RUN(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__CONTROLLER_RUN); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__CONTROLLER_IDLE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__CONTROLLER_IDLE); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__CALIBRATE_ENCODER(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_calibrator_t* calib = (icubCanProto_calibrator_t*)descriptor->cmd.value; + + s_former_POL_MC_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_POL_MC_CMD__CALIBRATE_ENCODER); + + // now i prepare data[1] to data[7] + + frame->data[1] = calib->type; + + switch(calib->type) + { + case icubCanProto_calibration_type0_hard_stops: + { + *((uint16_t*)(&frame->data[2])) = calib->params.type0.pwmlimit; + *((uint16_t*)(&frame->data[4])) = calib->params.type0.velocity; + memset(&frame->data[6], 0, 2); // pad with 0 + } break; + + case icubCanProto_calibration_type1_abs_sens_analog: + { + *((uint16_t*)(&frame->data[2])) = calib->params.type1.position; + *((uint16_t*)(&frame->data[4])) = calib->params.type1.velocity; + memset(&frame->data[6], 0, 2); // pad with 0 + } break; + + case icubCanProto_calibration_type2_hard_stops_diff: + { + *((uint16_t*)(&frame->data[2])) = calib->params.type2.pwmlimit; + *((uint16_t*)(&frame->data[4])) = calib->params.type2.velocity; + memset(&frame->data[6], 0, 2); // pad with 0 + } break; + + + case icubCanProto_calibration_type3_abs_sens_digital: + { + *((uint16_t*)(&frame->data[2])) = calib->params.type3.position; + *((uint16_t*)(&frame->data[4])) = calib->params.type3.velocity; + *((uint16_t*)(&frame->data[6])) = calib->params.type3.offset; + } break; + + case icubCanProto_calibration_type4_abs_and_incremental: + { + *((uint16_t*)(&frame->data[2])) = calib->params.type4.position; + *((uint16_t*)(&frame->data[4])) = calib->params.type4.velocity; + *((uint16_t*)(&frame->data[6])) = calib->params.type4.maxencoder; + } break; + + case icubCanProto_calibration_type6_mais: + { + *((uint16_t*)(&frame->data[2])) = calib->params.type6.position; + *((uint16_t*)(&frame->data[4])) = calib->params.type6.velocity; + memset(&frame->data[6], 0, 2); // pad with 0 + } break; + + case icubCanProto_calibration_type7_hall_sensor: + { + *((uint16_t*)(&frame->data[2])) = calib->params.type7.position; + *((uint16_t*)(&frame->data[4])) = calib->params.type7.velocity; + memset(&frame->data[6], 0, 2); // pad with 0 + } break; + + default: + { + //#warning --> TODO error about unknown param ... + } break; + } + + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__ENABLE_PWM_PAD(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__ENABLE_PWM_PAD); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__DISABLE_PWM_PAD(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__DISABLE_PWM_PAD); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_CONTROL_MODE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_CONTROL_MODE); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_CONTROL_MODE(eOcanframe_t *frame, eOcanport_t port) +{ + //#warning -> i am not sure it is still in use, thus i just put an error return + return(eores_NOK_generic); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__MOTION_DONE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__MOTION_DONE); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__MOTION_DONE(eOcanframe_t *frame, eOcanport_t port) +//{ +// eOmc_joint_t *joint = NULL; +// eOprotIndex_t index = EOK_uint08dummy; +// +// // retrieve the joint related to the frame +// if(NULL == (joint = (eOmc_joint_t*) s_eocanprotMCpolling_get_entity(eoprot_entity_mc_joint, frame, port, &index))) +// { +// return(eores_OK); +// } +// +// // in byte data[1] there is: 0/1 +// joint->status.core.modes.ismotiondone = (eObool_t)frame->data[1]; +// +// // and now let's manage the proxy +// +// eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, index, eoprot_tag_mc_joint_status_core_modes_ismotiondone); +// +// eOerrmanDescriptor_t errdes = {0}; +// EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); +// eOproxy_params_t *param = eo_proxy_Params_Get(proxy, id32); +// if(NULL == param) +// { +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_ropdes_notfound); +// errdes.par16 = 0; +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); +// +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_canservices_parsingfailure); +// errdes.par16 = (frame->id & 0x0fff) | ((frame->size & 0x000f) << 12); +// errdes.par64 = eo_common_canframe_data2u64((eOcanframe_t*)frame); +// errdes.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame); +// errdes.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2); +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +// +// return(eores_OK); +// } +// +// param->p08_2 ++; +// +// if(param->p08_1 == param->p08_2) +// { +// eOresult_t res = eo_proxy_ReplyROP_Load(proxy, id32, NULL); +// eom_emsappl_SendTXRequest(eom_emsappl_GetHandle()); +// } + +//#if defined(DEBUG_LOG_PROXY_ACTIVITY) +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_reply_ok); +// errdes.par16 = (param->p08_2 << 8) | (param->p08_1); +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +//#endif +// +// return(eores_OK); +//} +{ + static volatile uint32_t xxx = 0; + for(;;) + { // i dont think trhat the foc control mode uses it, but we never know + xxx = 0; + } + return(eores_NOK_generic); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_CONTROL_MODE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_controlmode_t *controlmode = (icubCanProto_controlmode_t*)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 2, ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE); + frame->data[1] = *controlmode; + + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_SPEED_ESTIM_SHIFT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_estimShift_t *shift = (icubCanProto_estimShift_t*)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_SPEED_ESTIM_SHIFT); + // now i prepare data[1] -> data[4] + frame->data[1] = shift->estimShiftJointVel; + frame->data[2] = shift->estimShiftJointAcc; + frame->data[3] = shift->estimShiftMotorVel; + frame->data[4] = shift->estimShiftMotorAcc; + + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__POSITION_MOVE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_setpoint_position_t *setpointp = (icubCanProto_setpoint_position_t *)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 7, ICUBCANPROTO_POL_MC_CMD__POSITION_MOVE); + // now i prepare data[1] -> data[6] + *((icubCanProto_position_t*)(&frame->data[1])) = setpointp->value; + *((icubCanProto_velocity_t*)(&frame->data[5])) = setpointp->withvelocity; + + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__VELOCITY_MOVE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_setpoint_velocity_t *setpointv = (icubCanProto_setpoint_velocity_t *)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__VELOCITY_MOVE); + // now i prepare data[1] -> data[4] + *((icubCanProto_velocity_t*)(&frame->data[1])) = setpointv->value; + *((icubCanProto_acceleration_t*)(&frame->data[3])) = setpointv->withacceleration; + + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_DESIRED_TORQUE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_setpoint_torque_t *setpointt = (icubCanProto_setpoint_torque_t *)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_TORQUE); + // now i prepare data[1] -> data[4] + // marco.accame: value is a int16_t but i have found it copied byte by byte, so i keep it in this way + frame->data[1] = ((uint8_t*)&(setpointt->value))[0]; + frame->data[2] = ((uint8_t*)&(setpointt->value))[1]; + frame->data[4] = frame->data[3] = (frame->data[2] & 0x80) ? 0xFF : 0; + + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_DESIRED_TORQUE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_TORQUE); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__STOP_TRAJECTORY(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__STOP_TRAJECTORY); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_COMMAND_POSITION(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_COMMAND_POSITION); + // now i prepare data[1] -> data[4] + *((icubCanProto_position_t*)(&frame->data[1])) = *((icubCanProto_position_t*)descriptor->cmd.value); + + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_MIN_POSITION(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION); + // now i prepare data[1] -> data[4] + *((icubCanProto_position_t*)(&frame->data[1])) = *((icubCanProto_position_t*)descriptor->cmd.value); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_MIN_POSITION(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_MIN_POSITION); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_MIN_POSITION(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getposition(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_MIN_POSITION)); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_MAX_POSITION(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION); + // now i prepare data[1] -> data[4] + *((icubCanProto_position_t*)(&frame->data[1])) = *((icubCanProto_position_t*)descriptor->cmd.value); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_MAX_POSITION(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_MAX_POSITION); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_PWM_LIMIT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 3, ICUBCANPROTO_POL_MC_CMD__SET_PWM_LIMIT); + *((icubCanProto_pwm_t*)(&frame->data[1])) = *((icubCanProto_pwm_t*)descriptor->cmd.value); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_PWM_LIMIT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_PWM_LIMIT); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_PWM_LIMIT(eOcanframe_t *frame, eOcanport_t port) +//{ +// eOprotIndex_t index = EOK_uint08dummy; + +// eObrd_canlocation_t loc = {0}; +// loc.port = port; +// loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); +// loc.insideindex = EOCANPROT_FRAME_POLLING_MC_GET_INTERNALINDEX(frame); +// +// index = eo_canmap_GetEntityIndex(eo_canmap_GetHandle(), loc, eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor); +// if(EOK_uint08dummy == index) +// { +// //s_eo_icubCanProto_mb_send_runtime_error_diagnostics(6); +// return(eores_OK); +// } +// +// eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, index, eoprot_tag_mc_motor_config_pwmlimit); +// +// eOerrmanDescriptor_t errdes = {0}; +// EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); +// eOproxy_params_t *param = eo_proxy_Params_Get(proxy, id32); +// if(NULL == param) +// { +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_ropdes_notfound); +// errdes.par16 = 0; +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); +// +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_canservices_parsingfailure); +// errdes.par16 = (frame->id & 0x0fff) | ((frame->size & 0x000f) << 12); +// errdes.par64 = eo_common_canframe_data2u64((eOcanframe_t*)frame); +// errdes.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame); +// errdes.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2); +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +// +// return(eores_OK); +// } + +// eOmeas_pwm_t *pwmLimit = (eOmeas_pwm_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); +// +// *pwmLimit = *((eOmeas_pwm_t*)(&frame->data[1])); + +// param->p08_2 ++; +// +// if(param->p08_1 == param->p08_2) +// { +// eOresult_t res = eo_proxy_ReplyROP_Load(proxy, id32, NULL); +// eom_emsappl_SendTXRequest(eom_emsappl_GetHandle()); +// } + +//#if defined(DEBUG_LOG_PROXY_ACTIVITY) +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_reply_ok); +// errdes.par16 = (param->p08_2 << 8) | (param->p08_1); +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +//#endif +// +// return(eores_OK); +//} +{ + static volatile uint32_t xxx = 0; + for(;;) + { // i dont think trhat the foc control mode uses it, but we never know + xxx = 0; + } + return(eores_NOK_generic); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_MAX_MOTOR_POS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_MAX_MOTOR_POS); + // now i prepare data[1] -> data[4] + *((icubCanProto_position_t*)(&frame->data[1])) = *((icubCanProto_position_t*)descriptor->cmd.value); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_MIN_MOTOR_POS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_MIN_MOTOR_POS); + // now i prepare data[1] -> data[4] + *((icubCanProto_position_t*)(&frame->data[1])) = *((icubCanProto_position_t*)descriptor->cmd.value); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_MAX_POSITION(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getposition(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_MAX_POSITION)); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_MAX_VELOCITY(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 3, ICUBCANPROTO_POL_MC_CMD__SET_MAX_VELOCITY); + // now i prepare data[1] -> data[2] + *((icubCanProto_velocity_t*)(&frame->data[1])) = *((icubCanProto_velocity_t*)descriptor->cmd.value); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_CURRENT_LIMIT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + eOmc_current_limits_params_t *current_limits = (eOmc_current_limits_params_t*)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT); + *((int16_t*)(&frame->data[2])) = current_limits->nominalCurrent; + *((int16_t*)(&frame->data[4])) = current_limits->peakCurrent; + *((int16_t*)(&frame->data[6])) = current_limits->overloadCurrent; + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_BCAST_POLICY(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_BCAST_POLICY); + // now i prepare data[1] -> data[4] + memcpy(&frame->data[1], descriptor->cmd.value, 4); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_VEL_SHIFT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_velocityShift_t shift = *((icubCanProto_velocityShift_t*)descriptor->cmd.value); + s_former_POL_MC_prepare_frame(descriptor, frame, 3, ICUBCANPROTO_POL_MC_CMD__SET_VEL_SHIFT); + // now i prepare data[1] -> data[2] + *((icubCanProto_velocityShift_t*)(&frame->data[1])) = shift; + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_TORQUE_PID(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_CMD_setpid(descriptor, frame, ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PID); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_TORQUE_PID(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PID); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_TORQUE_PID(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getpid_etc(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PID)); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_TORQUE_PIDLIMITS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_CMD_setpid_limits(descriptor, frame, ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PIDLIMITS); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_TORQUE_PIDLIMITS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PIDLIMITS); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_TORQUE_PIDLIMITS(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getpid_etc(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PIDLIMITS)); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_POS_PID(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_CMD_setpid(descriptor, frame, ICUBCANPROTO_POL_MC_CMD__SET_POS_PID); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_POS_PID(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_POS_PID); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_POS_PID(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getpid_etc(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_POS_PID)); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_POS_PIDLIMITS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_CMD_setpid_limits(descriptor, frame, ICUBCANPROTO_POL_MC_CMD__SET_POS_PIDLIMITS); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_POS_PIDLIMITS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_POS_PIDLIMITS); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_POS_PIDLIMITS(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getpid_etc(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_POS_PIDLIMITS)); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_VEL_TIMEOUT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 3, ICUBCANPROTO_POL_MC_CMD__SET_VEL_TIMEOUT); + // now i prepare data[1] -> data[2] + *((uint16_t*)(&frame->data[1])) = *((uint16_t*)descriptor->cmd.value); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_IMPEDANCE_PARAMS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_impedance_t *imp = (icubCanProto_impedance_t *)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_PARAMS); + // now i prepare data[1] -> data[7] + // stiffnes and damping occupy two bytes + *((icubCanProto_stiffness_t*)(&frame->data[1])) = (icubCanProto_stiffness_t)imp->stiffness; + *((icubCanProto_damping_t*)(&frame->data[3])) = (icubCanProto_damping_t)imp->damping; + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_IMPEDANCE_PARAMS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_PARAMS); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_IMPEDANCE_PARAMS(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getimpedance(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_PARAMS)); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_IMPEDANCE_OFFSET(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_torque_t *offset = (icubCanProto_torque_t *)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 3, ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_OFFSET); + // now i prepare data[1] -> data[2] + *((icubCanProto_torque_t*)(&frame->data[1])) = *((icubCanProto_torque_t*)(offset)); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_IMPEDANCE_OFFSET(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_OFFSET); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_IMPEDANCE_OFFSET(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getimpedance(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_OFFSET)); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_FIRMWARE_VERSION(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + eObrd_protocolversion_t *reqprot = (eObrd_protocolversion_t*)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 3, ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION); + + frame->data[1] = reqprot->major; + frame->data[2] = reqprot->minor; + + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_FIRMWARE_VERSION(eOcanframe_t *frame, eOcanport_t port) +{ + // must: retrieve the board given the location, assign the rx values, verify if the fw version is ok + // then: (but it could be put somewhere else) ... if all boards are ready: stop, send config, start mais + // i would like to do it inside a method of ... + + eObrd_canlocation_t loc = {0}; + loc.port = port; + loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); + loc.insideindex = EOCANPROT_FRAME_POLLING_MC_GET_INTERNALINDEX(frame); + + eObool_t match = (1 == frame->data[7]) ? eobool_true : eobool_false; + + eObrd_info_t detected = {0}; + detected.type = frame->data[1]; + detected.firmware.major = frame->data[2]; + detected.firmware.minor = frame->data[3]; + detected.firmware.build = frame->data[4]; + detected.protocol.major = frame->data[5]; + detected.protocol.minor = frame->data[6]; + + + eo_candiscovery2_OneBoardIsFound(eo_candiscovery2_GetHandle(), loc, match, &detected); + + + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_CURRENT_PID(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PID); + memcpy(frame->data+1,descriptor->cmd.value,7); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_CURRENT_PIDLIMITS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + //#warning marco.accame CHECK: see if ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PIDLIMITS has len 7 or 8 as some other SET_xxx_PIDLIMITS + s_former_POL_MC_CMD_setpid_limits_7(descriptor, frame, ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PIDLIMITS); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_VELOCITY_PID(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_POL_MC_CMD__SET_VELOCITY_PID); + memcpy(frame->data+1,descriptor->cmd.value,7); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_VELOCITY_PIDLIMITS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + // ... apparently s_former_POL_MC_CMD_setpid_limits uses len 8 and not len 7 + //#warning marco.accame CHECK: see if ICUBCANPROTO_POL_MC_CMD__SET_VELOCITY_PIDLIMITS has len 7 or 8 as some other SET_xxx_PIDLIMITS + s_former_POL_MC_CMD_setpid_limits_7(descriptor, frame, ICUBCANPROTO_POL_MC_CMD__SET_VELOCITY_PIDLIMITS); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_DESIRED_CURRENT(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + eOmeas_current_t *current = (eOmeas_current_t*)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_CURRENT); + + *((eOmeas_current_t*)(&frame->data[1])) = *current; + frame->data[3] = 0; // LSB Id + frame->data[4] = 0; // MSB Id + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_I2T_PARAMS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + eOmc_i2tParams_t *i2tparams = (eOmc_i2tParams_t*)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_I2T_PARAMS); + *((uint16_t*)(&frame->data[1])) = i2tparams->time; + *((uint16_t*)(&frame->data[3])) = i2tparams->tresh; + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_OPENLOOP_PARAMS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_setpoint_current_t *setpoint = (icubCanProto_setpoint_current_t*)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 3, ICUBCANPROTO_POL_MC_CMD__SET_OPENLOOP_PARAMS); + *((int16_t*)(&frame->data[1])) = setpoint->value; + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_OPENLOOP_PARAMS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_OPENLOOP_PARAMS); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_OPENLOOP_PARAMS(eOcanframe_t *frame, eOcanport_t port) +{ + eOmc_joint_t *joint = NULL; + + if(NULL == (joint = (eOmc_joint_t*) s_eocanprotMCpolling_get_entity(eoprot_entity_mc_joint, frame, port, NULL))) + { + return(eores_OK); + } + + //it is: joint->status.ofpid.openloop.refolo = *((int16_t*)&frame->data[1]); + joint->status.target.trgt_pwm = *((int16_t*)&frame->data[1]); + + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_INTERACTION_MODE(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + icubCanProto_interactionmode_t *imode = (icubCanProto_interactionmode_t*)descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_INTERACTION_MODE); + *((icubCanProto_interactionmode_t*)(&frame->data[1])) = *imode; + return(eores_OK); +} + +#define USE_2FOC_PROT_1dot6 +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_MOTOR_CONFIG(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ +#if defined(USE_2FOC_PROT_1dot6) + s_former_POL_MC_prepare_frame(descriptor, frame, 8, ICUBCANPROTO_POL_MC_CMD__SET_MOTOR_CONFIG); + memcpy(frame->data+1,descriptor->cmd.value,7); +#else + s_former_POL_MC_prepare_frame(descriptor, frame, 7, ICUBCANPROTO_POL_MC_CMD__SET_MOTOR_CONFIG); + memcpy(frame->data+1,descriptor->cmd.value,6); +#endif + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_POS_STICTION_PARAMS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + eOmc_PID_t *pid = (eOmc_PID_t*) descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_POS_STICTION_PARAMS); + *((int16_t*)(&frame->data[1])) = (int16_t) pid->stiction_up_val; + *((int16_t*)(&frame->data[3])) = (int16_t) pid->stiction_down_val; + return(eores_OK); + +} +//extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__SET_POS_STICTION_PARAMS(eOcanframe_t *frame, eOcanport_t port);//unused + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_POS_STICTION_PARAMS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_POS_STICTION_PARAMS); + return(eores_OK); +} + + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_POS_STICTION_PARAMS(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getpid_etc(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_POS_STICTION_PARAMS)); +} + + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__SET_TORQUE_STICTION_PARAMS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + eOmc_PID_t *pid = (eOmc_PID_t*) descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 5, ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_STICTION_PARAMS); + *((int16_t*)(&frame->data[1])) = (int16_t) pid->stiction_up_val; + *((int16_t*)(&frame->data[3])) = (int16_t) pid->stiction_down_val; + return(eores_OK); +} +//extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__SET_TORQUE_STICTION_PARAMS(eOcanframe_t *frame, eOcanport_t port);//unused + +extern eOresult_t eocanprotMCpolling_former_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame) +{ + s_former_POL_MC_prepare_frame(descriptor, frame, 1, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS); + return(eores_OK); +} + +extern eOresult_t eocanprotMCpolling_parser_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS(eOcanframe_t *frame, eOcanport_t port) +{ + return(s_parser_POL_MC_CMD_getpid_etc(frame, port, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS)); +} + + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- + +static void* s_eocanprotMCpolling_get_entity(eOprot_entity_t entity, eOcanframe_t *frame, eOcanport_t port, uint8_t *index) +{ + void * ret = NULL; + uint8_t ii = 0; + eObrd_canlocation_t loc = {0}; + + loc.port = port; + loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); + loc.insideindex = EOCANPROT_FRAME_POLLING_MC_GET_INTERNALINDEX(frame); + + ii = eo_canmap_GetEntityIndex(eo_canmap_GetHandle(), loc, eoprot_endpoint_motioncontrol, entity); + + if(EOK_uint08dummy == ii) + { + //#warning -> TODO: add diagnostics about not found board as in s_eo_icubCanProto_mb_send_runtime_error_diagnostics() + return(NULL); + } + + ret = eoprot_entity_ramof_get(eoprot_board_localboard, eoprot_endpoint_motioncontrol, entity, ii); + + if(NULL != index) + { + *index = ii; + } + + return(ret); +} + + + +// - parser helper funtions + + + + +// former helper funtions + +static void s_former_POL_MC_prepare_frame(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t len, uint8_t type) +{ // every message coming from the ems has actually id 0 + frame->id = EOCANPROT_CREATE_CANID(eocanprot_msgclass_pollingMotorControl, 0, descriptor->loc.addr); + frame->id_type = eocanframeID_std11bits; + frame->frame_type = eocanframetype_data; + frame->size = len; + // marco.accame on 26 apr 2021: the mc control uses only inside index 0 or 1. it is better to ensure this rule to avoid wrongly formed can frames + uint8_t canlegalinsideindex = (eobrd_caninsideindex_second == descriptor->loc.insideindex) ? 1 : 0; // only 0 or 1. 1 only if we want the second index + frame->data[0] = EOCANPROT_CREATE_POLLING_MC_DATA0(canlegalinsideindex, type); +} + + +static void s_former_POL_MC_CMD_setpid(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t type) +{ + eOmc_PID_t *pid = (eOmc_PID_t*) descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 8, type); + // now i prepare data[1] -> data[7] + *((int16_t*)(&frame->data[1])) = (int16_t) pid->kp; + *((int16_t*)(&frame->data[3])) = (int16_t) pid->ki; + *((int16_t*)(&frame->data[5])) = (int16_t) pid->kd; + frame->data[7] = (int8_t) pid->scale; +} + +static void s_former_POL_MC_CMD_setpid_limits(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t type) +{ + // Currently in messages.h there is a check on frame_len equal to 8, else frame is discarded + // so i left size = 8 even if correct size is 7 + eOmc_PID_t *pid = (eOmc_PID_t*) descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 8, type); + // now i prepare data[1] -> data[7] + *((int16_t*)(&frame->data[1])) = (int16_t) pid->offset; + *((int16_t*)(&frame->data[3])) = (int16_t) pid->limitonoutput; + *((int16_t*)(&frame->data[5])) = (int16_t) pid->limitonintegral; +} + +/* +static void s_former_POL_MC_CMD_setpid_7(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t type) +{ + eOmc_PID_t *pid = (eOmc_PID_t*) descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 7, type); + // now i prepare data[1] -> data[6] + *((int16_t*)(&frame->data[1])) = (int16_t) pid->kp; + *((int16_t*)(&frame->data[3])) = (int16_t) pid->ki; + *((int16_t*)(&frame->data[5])) = (int16_t) pid->kd; +} +*/ + +static void s_former_POL_MC_CMD_setpid_limits_7(eOcanprot_descriptor_t *descriptor, eOcanframe_t *frame, uint8_t type) +{ + eOmc_PID_t *pid = (eOmc_PID_t*) descriptor->cmd.value; + s_former_POL_MC_prepare_frame(descriptor, frame, 7, type); + // now i prepare data[1] -> data[6] + *((int16_t*)(&frame->data[1])) = (int16_t) pid->offset; + *((int16_t*)(&frame->data[3])) = (int16_t) pid->limitonoutput; + *((int16_t*)(&frame->data[5])) = (int16_t) pid->limitonintegral; +} + + +// cmd can be: ICUBCANPROTO_POL_MC_CMD__GET_MIN_POSITION, ICUBCANPROTO_POL_MC_CMD__GET_MAX_POSITION +static eOresult_t s_parser_POL_MC_CMD_getposition(eOcanframe_t *frame, eOcanport_t port, uint8_t type) +//{ +// eOprotIndex_t index = EOK_uint08dummy; + +// eObrd_canlocation_t loc = {0}; +// loc.port = port; +// loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); +// loc.insideindex = EOCANPROT_FRAME_POLLING_MC_GET_INTERNALINDEX(frame); +// +// index = eo_canmap_GetEntityIndex(eo_canmap_GetHandle(), loc, eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint); +// if(EOK_uint08dummy == index) +// { +// //s_eo_icubCanProto_mb_send_runtime_error_diagnostics(6); +// return(eores_OK); +// } +// +// +// eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, index, eoprot_tag_mc_joint_config_userlimits); +// +// eOerrmanDescriptor_t errdes = {0}; +// EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); +// eOproxy_params_t *param = eo_proxy_Params_Get(proxy, id32); +// if(NULL == param) +// { +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_ropdes_notfound); +// errdes.par16 = 0; +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); +// +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_canservices_parsingfailure); +// errdes.par16 = (frame->id & 0x0fff) | ((frame->size & 0x000f) << 12); +// errdes.par64 = eo_common_canframe_data2u64((eOcanframe_t*)frame); +// errdes.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame); +// errdes.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2); +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +// +// return(eores_OK); +// } + +// eOmeas_position_limits_t *limits = (eOmeas_position_limits_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); +// +// icubCanProto_position_t icub_pos = *((icubCanProto_position_t*)(&frame->data[1])); +// // ok, now i must convert from one to another ... +// eOmeas_position_t pos = eo_mc4boards_Convert_Position_fromCAN(eo_mc4boards_GetHandle(), index, icub_pos); +// +// if(ICUBCANPROTO_POL_MC_CMD__GET_MIN_POSITION == type) +// { +// limits->min = pos; +// } +// else if(ICUBCANPROTO_POL_MC_CMD__GET_MAX_POSITION == type) +// { +// limits->max = pos; +// } +// else +// { // i must have called it badly +// return(eores_NOK_generic); +// } + +// param->p08_2 ++; +// +// if(param->p08_1 == param->p08_2) +// { +// // send back response +// if(limits->max < limits->min) +// { // exchange them +// eOmeas_position_t tmp = limits->min; +// limits->min = limits->max; +// limits->max = tmp; +// } +// +// eOresult_t res = eo_proxy_ReplyROP_Load(proxy, id32, NULL); +// eom_emsappl_SendTXRequest(eom_emsappl_GetHandle()); +// } + +//#if defined(DEBUG_LOG_PROXY_ACTIVITY) +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_reply_ok); +// errdes.par16 = (param->p08_2 << 8) | (param->p08_1); +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +//#endif +// +// return(eores_OK); +//} +{ + static volatile uint32_t xxx = 0; + for(;;) + { // i dont think trhat the foc control mode uses it, but we never know + xxx = 0; + } + return(eores_NOK_generic); +} + + +// cmd can be: ICUBCANPROTO_POL_MC_CMD__GET_POS_PID, ICUBCANPROTO_POL_MC_CMD__GET_POS_PIDLIMITS, ICUBCANPROTO_POL_MC_CMD__GET_POS_STICTION_PARAMS +// ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PID, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PIDLIMITS, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS +static eOresult_t s_parser_POL_MC_CMD_getpid_etc(eOcanframe_t *frame, eOcanport_t port, uint8_t type) +//{ +// eOprotIndex_t index = EOK_uint08dummy; + +// eObrd_canlocation_t loc = {0}; +// loc.port = port; +// loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); +// loc.insideindex = EOCANPROT_FRAME_POLLING_MC_GET_INTERNALINDEX(frame); +// +// index = eo_canmap_GetEntityIndex(eo_canmap_GetHandle(), loc, eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint); +// if(EOK_uint08dummy == index) +// { +// //s_eo_icubCanProto_mb_send_runtime_error_diagnostics(6); +// return(eores_OK); +// } +// +// eOprotTag_t tag = eoprot_tag_none; +// +// switch(type) +// { +// case ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PID: +// case ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PIDLIMITS: +// case ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS: tag = eoprot_tag_mc_joint_config_pidtorque; +// break; + +// case ICUBCANPROTO_POL_MC_CMD__GET_POS_PID: +// case ICUBCANPROTO_POL_MC_CMD__GET_POS_PIDLIMITS: +// case ICUBCANPROTO_POL_MC_CMD__GET_POS_STICTION_PARAMS: tag = eoprot_tag_mc_joint_config_pidposition; +// break; + +// default: tag = eoprot_tag_none; +// break; +// } +// +// if(eoprot_tag_none == tag) +// { // returns NOK because it is something i cannot parse. i must have called this function badly ... +// return(eores_NOK_generic); +// } +// +// eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, index, tag); +// +// eOerrmanDescriptor_t errdes = {0}; +// EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); +// eOproxy_params_t *param = eo_proxy_Params_Get(proxy, id32); +// if(NULL == param) +// { +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_ropdes_notfound); +// errdes.par16 = 0; +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); +// +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_canservices_parsingfailure); +// errdes.par16 = (frame->id & 0x0fff) | ((frame->size & 0x000f) << 12); +// errdes.par64 = eo_common_canframe_data2u64((eOcanframe_t*)frame); +// errdes.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame); +// errdes.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2); +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +// +// return(eores_OK); +// } +// +// eOmc_PID_t* pid = (eOmc_PID_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); +// +// if((ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PID == type) || (ICUBCANPROTO_POL_MC_CMD__GET_POS_PID == type)) +// { +// pid->kp = *((int16_t*)&frame->data[1]); +// pid->ki = *((int16_t*)&frame->data[3]); +// pid->kd = *((int16_t*)&frame->data[5]); +// } +// else if((ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PIDLIMITS == type) || (ICUBCANPROTO_POL_MC_CMD__GET_POS_PIDLIMITS == type)) +// { +// pid->offset = *((int16_t*)(&frame->data[1])); +// pid->limitonoutput = *((int16_t*)(&frame->data[3])); +// pid->limitonintegral = *((int16_t*)(&frame->data[5])); +// } +// else // no need to verify that type value is a LIMITS i already did it in the switch-case part. +// { +// pid->stiction_up_val = *((int16_t*)&frame->data[1]); +// pid->stiction_down_val = *((int16_t*)&frame->data[3]); +// } +// +// param->p08_2 ++; +// +// if(param->p08_1 == param->p08_2) +// { +// // send back response +// //marco.accame -> can eo_proxy_ReplyROP_Load() use the same memory as the nv? much be better using NULL. think of using memmove. +// eOresult_t res = eo_proxy_ReplyROP_Load(proxy, id32, NULL); // if NULL it does not copy dat into the nv and uses ram inside the netvar +// eom_emsappl_SendTXRequest(eom_emsappl_GetHandle()); +// } + +//#if defined(DEBUG_LOG_PROXY_ACTIVITY) +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_reply_ok); +// errdes.par16 = (param->p08_2 << 8) | (param->p08_1); +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +//#endif +// +// return(eores_OK); +//} +{ + static volatile uint32_t xxx = 0; + for(;;) + { // i dont think trhat the foc control mode uses it, but we never know + xxx = 0; + } + return(eores_NOK_generic); +} + + +// cmd can be: ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_PARAMS, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_OFFSET +static eOresult_t s_parser_POL_MC_CMD_getimpedance(eOcanframe_t *frame, eOcanport_t port, uint8_t type) +//{ +// +// eOprotIndex_t index = EOK_uint08dummy; + +// eObrd_canlocation_t loc = {0}; +// loc.port = port; +// loc.addr = EOCANPROT_FRAME_GET_SOURCE(frame); +// loc.insideindex = EOCANPROT_FRAME_POLLING_MC_GET_INTERNALINDEX(frame); +// +// index = eo_canmap_GetEntityIndex(eo_canmap_GetHandle(), loc, eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint); +// if(EOK_uint08dummy == index) +// { +// //s_eo_icubCanProto_mb_send_runtime_error_diagnostics(6); +// return(eores_OK); +// } +// +// +// eOprotID32_t id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, index, eoprot_tag_mc_joint_config_impedance); +// +// eOerrmanDescriptor_t errdes = {0}; +// EOproxy * proxy = eo_transceiver_GetProxy(eo_boardtransceiver_GetTransceiver(eo_boardtransceiver_GetHandle())); +// eOproxy_params_t *param = eo_proxy_Params_Get(proxy, id32); +// if(NULL == param) +// { +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_ropdes_notfound); +// errdes.par16 = 0; +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_error, NULL, NULL, &errdes); +// +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_canservices_parsingfailure); +// errdes.par16 = (frame->id & 0x0fff) | ((frame->size & 0x000f) << 12); +// errdes.par64 = eo_common_canframe_data2u64((eOcanframe_t*)frame); +// errdes.sourceaddress = EOCANPROT_FRAME_GET_SOURCE(frame); +// errdes.sourcedevice = (eOcanport1 == port) ? (eo_errman_sourcedevice_canbus1) : (eo_errman_sourcedevice_canbus2); +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +// +// return(eores_OK); +// } + +// eOmc_impedance_t *impedance = (eOmc_impedance_t*)eoprot_variable_ramof_get(eoprot_board_localboard, id32); +// +// if(ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_PARAMS == type) +// { +// icubCanProto_stiffness_t icub_stiff = *((icubCanProto_stiffness_t*)(&frame->data[1])); +// icubCanProto_damping_t icub_dump = *((icubCanProto_damping_t*)(&frame->data[3])); +// impedance->stiffness = eo_mc4boards_Convert_impedanceStiffness_S2I(eo_mc4boards_GetHandle(), index, icub_stiff); +// impedance->damping = eo_mc4boards_Convert_impedanceDamping_S2I(eo_mc4boards_GetHandle(), index, icub_dump); +// +// } +// else if(ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_OFFSET == type) +// { +// icubCanProto_torque_t icub_trq = *((icubCanProto_torque_t*)(&frame->data[1])); +// impedance->offset = eo_mc4boards_Convert_torque_S2I(eo_mc4boards_GetHandle(), index, icub_trq); +// } +// else +// { // i must have called it badly +// return(eores_NOK_generic); +// } + +// param->p08_2 ++; +// +// if(param->p08_1 == param->p08_2) +// { +// // send back response +// eOresult_t res = eo_proxy_ReplyROP_Load(proxy, id32, NULL); +// eom_emsappl_SendTXRequest(eom_emsappl_GetHandle()); +// } + +//#if defined(DEBUG_LOG_PROXY_ACTIVITY) +// errdes.sourcedevice = eo_errman_sourcedevice_localboard; +// errdes.sourceaddress = 0; +// errdes.code = eoerror_code_get(eoerror_category_System, eoerror_value_SYS_proxy_reply_ok); +// errdes.par16 = (param->p08_2 << 8) | (param->p08_1); +// errdes.par64 = id32; +// eo_errman_Error(eo_errman_GetHandle(), eo_errortype_debug, NULL, NULL, &errdes); +//#endif +// +// return(eores_OK); +//} +{ + static volatile uint32_t xxx = 0; + for(;;) + { // i dont think trhat the foc control mode uses it, but we never know + xxx = 0; + } + return(eores_NOK_generic); +} + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotSKperiodic_amc.c b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotSKperiodic_amc.c new file mode 100644 index 0000000000..107adbfac9 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/protocol/can/EoCANprotSKperiodic_amc.c @@ -0,0 +1,89 @@ + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + +// -------------------------------------------------------------------------------------------------------------------- +// - external dependencies +// -------------------------------------------------------------------------------------------------------------------- + + +#include "EoCommon.h" +//#include "EOtheSKIN.h" + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern public interface +// -------------------------------------------------------------------------------------------------------------------- + +#include "EOtheCANprotocol_functions.h" + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of extern hidden interface +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - #define with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of extern variables, but better using _get(), _set() +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - typedef with internal scope +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - declaration of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition (and initialisation) of static variables +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern public functions +// -------------------------------------------------------------------------------------------------------------------- + + +extern eOresult_t eocanprotSKperiodic_parser_ANY_PERIODIC_SKIN_MSG(eOcanframe_t *frame, eOcanport_t port) +{ + return(eores_NOK_generic); +} + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of extern hidden functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - definition of static functions +// -------------------------------------------------------------------------------------------------------------------- +// empty-section + + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + + + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/startup_stm32h745xx_CM7.s b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/startup_stm32h745xx_CM7.s new file mode 100644 index 0000000000..262b4b0944 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/startup_stm32h745xx_CM7.s @@ -0,0 +1,621 @@ +;******************** (C) COPYRIGHT 2019 STMicroelectronics ******************** +;* File Name : startup_stm32h745xx.s +;* @author MCD Application Team +;* Description : STM32H7xx devices vector table for MDK-ARM toolchain. +;* This module performs: +;* - Set the initial SP +;* - Set the initial PC == Reset_Handler +;* - Set the vector table entries with the exceptions ISR address +;* - Branches to __main in the C library (which eventually +;* calls main()). +;* After Reset the Cortex-M processor is in Thread mode, +;* priority is Privileged, and the Stack is set to Main. +;* <<< Use Configuration Wizard in Context Menu >>> +;****************************************************************************** +;* @attention +;* +;* Copyright (c) 2019 STMicroelectronics. +;* All rights reserved. +;* +;* This software component is licensed by ST under BSD 3-Clause license, +;* the "License"; You may not use this file except in compliance with the +;* License. You may obtain a copy of the License at: +;* opensource.org/licenses/BSD-3-Clause +;* +;****************************************************************************** + +; Amount of memory (in bytes) allocated for Stack +; Tailor this value to your application needs +; Stack Configuration +; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8> +; + +Stack_Size EQU 0x1000 + + AREA STACK, NOINIT, READWRITE, ALIGN=3 +Stack_Mem SPACE Stack_Size +__initial_sp + + +; Heap Configuration +; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> +; + +Heap_Size EQU 0x20000 + + AREA HEAP, NOINIT, READWRITE, ALIGN=3 +__heap_base +Heap_Mem SPACE Heap_Size +__heap_limit + + PRESERVE8 + THUMB + + +; Vector Table Mapped to Address 0 at Reset + AREA RESET, DATA, READONLY + EXPORT __Vectors + EXPORT __Vectors_End + EXPORT __Vectors_Size + +__Vectors DCD __initial_sp ; Top of Stack + DCD Reset_Handler ; Reset Handler + DCD NMI_Handler ; NMI Handler + DCD HardFault_Handler ; Hard Fault Handler + DCD MemManage_Handler ; MPU Fault Handler + DCD BusFault_Handler ; Bus Fault Handler + DCD UsageFault_Handler ; Usage Fault Handler + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD SVC_Handler ; SVCall Handler + DCD DebugMon_Handler ; Debug Monitor Handler + DCD 0 ; Reserved + DCD PendSV_Handler ; PendSV Handler + DCD SysTick_Handler ; SysTick Handler + + ; External Interrupts + DCD WWDG_IRQHandler ; Window WatchDog interrupt ( wwdg1_it, wwdg2_it) + DCD PVD_AVD_IRQHandler ; PVD/AVD through EXTI Line detection + DCD TAMP_STAMP_IRQHandler ; Tamper and TimeStamps through the EXTI line + DCD RTC_WKUP_IRQHandler ; RTC Wakeup through the EXTI line + DCD FLASH_IRQHandler ; FLASH + DCD RCC_IRQHandler ; RCC + DCD EXTI0_IRQHandler ; EXTI Line0 + DCD EXTI1_IRQHandler ; EXTI Line1 + DCD EXTI2_IRQHandler ; EXTI Line2 + DCD EXTI3_IRQHandler ; EXTI Line3 + DCD EXTI4_IRQHandler ; EXTI Line4 + DCD DMA1_Stream0_IRQHandler ; DMA1 Stream 0 + DCD DMA1_Stream1_IRQHandler ; DMA1 Stream 1 + DCD DMA1_Stream2_IRQHandler ; DMA1 Stream 2 + DCD DMA1_Stream3_IRQHandler ; DMA1 Stream 3 + DCD DMA1_Stream4_IRQHandler ; DMA1 Stream 4 + DCD DMA1_Stream5_IRQHandler ; DMA1 Stream 5 + DCD DMA1_Stream6_IRQHandler ; DMA1 Stream 6 + DCD ADC_IRQHandler ; ADC1, ADC2 + DCD FDCAN1_IT0_IRQHandler ; FDCAN1 interrupt line 0 + DCD FDCAN2_IT0_IRQHandler ; FDCAN2 interrupt line 0 + DCD FDCAN1_IT1_IRQHandler ; FDCAN1 interrupt line 1 + DCD FDCAN2_IT1_IRQHandler ; FDCAN2 interrupt line 1 + DCD EXTI9_5_IRQHandler ; External Line[9:5]s + DCD TIM1_BRK_IRQHandler ; TIM1 Break interrupt + DCD TIM1_UP_IRQHandler ; TIM1 Update Interrupt + DCD TIM1_TRG_COM_IRQHandler ; TIM1 Trigger and Commutation Interrupt + DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare + DCD TIM2_IRQHandler ; TIM2 + DCD TIM3_IRQHandler ; TIM3 + DCD TIM4_IRQHandler ; TIM4 + DCD I2C1_EV_IRQHandler ; I2C1 Event + DCD I2C1_ER_IRQHandler ; I2C1 Error + DCD I2C2_EV_IRQHandler ; I2C2 Event + DCD I2C2_ER_IRQHandler ; I2C2 Error + DCD SPI1_IRQHandler ; SPI1 + DCD SPI2_IRQHandler ; SPI2 + DCD USART1_IRQHandler ; USART1 + DCD USART2_IRQHandler ; USART2 + DCD USART3_IRQHandler ; USART3 + DCD EXTI15_10_IRQHandler ; External Line[15:10] + DCD RTC_Alarm_IRQHandler ; RTC Alarm (A and B) through EXTI Line + DCD 0 ; Reserved + DCD TIM8_BRK_TIM12_IRQHandler ; TIM8 Break Interrupt and TIM12 global interrupt + DCD TIM8_UP_TIM13_IRQHandler ; TIM8 Update Interrupt and TIM13 global interrupt + DCD TIM8_TRG_COM_TIM14_IRQHandler ; TIM8 Trigger and Commutation Interrupt and TIM14 global interrupt + DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare Interrupt + DCD DMA1_Stream7_IRQHandler ; DMA1 Stream7 + DCD FMC_IRQHandler ; FMC + DCD SDMMC1_IRQHandler ; SDMMC1 + DCD TIM5_IRQHandler ; TIM5 + DCD SPI3_IRQHandler ; SPI3 + DCD UART4_IRQHandler ; UART4 + DCD UART5_IRQHandler ; UART5 + DCD TIM6_DAC_IRQHandler ; TIM6 and DAC1&2 underrun errors + DCD TIM7_IRQHandler ; TIM7 + DCD DMA2_Stream0_IRQHandler ; DMA2 Stream 0 + DCD DMA2_Stream1_IRQHandler ; DMA2 Stream 1 + DCD DMA2_Stream2_IRQHandler ; DMA2 Stream 2 + DCD DMA2_Stream3_IRQHandler ; DMA2 Stream 3 + DCD DMA2_Stream4_IRQHandler ; DMA2 Stream 4 + DCD ETH_IRQHandler ; Ethernet + DCD ETH_WKUP_IRQHandler ; Ethernet Wakeup through EXTI line + DCD FDCAN_CAL_IRQHandler ; FDCAN calibration unit interrupt + DCD CM7_SEV_IRQHandler ; CM7 Send event interrupt for CM4 + DCD CM4_SEV_IRQHandler ; CM4 Send event interrupt for CM7 + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD DMA2_Stream5_IRQHandler ; DMA2 Stream 5 + DCD DMA2_Stream6_IRQHandler ; DMA2 Stream 6 + DCD DMA2_Stream7_IRQHandler ; DMA2 Stream 7 + DCD USART6_IRQHandler ; USART6 + DCD I2C3_EV_IRQHandler ; I2C3 event + DCD I2C3_ER_IRQHandler ; I2C3 error + DCD OTG_HS_EP1_OUT_IRQHandler ; USB OTG HS End Point 1 Out + DCD OTG_HS_EP1_IN_IRQHandler ; USB OTG HS End Point 1 In + DCD OTG_HS_WKUP_IRQHandler ; USB OTG HS Wakeup through EXTI + DCD OTG_HS_IRQHandler ; USB OTG HS + DCD DCMI_IRQHandler ; DCMI + DCD 0 ; Reserved + DCD RNG_IRQHandler ; Rng + DCD FPU_IRQHandler ; FPU + DCD UART7_IRQHandler ; UART7 + DCD UART8_IRQHandler ; UART8 + DCD SPI4_IRQHandler ; SPI4 + DCD SPI5_IRQHandler ; SPI5 + DCD SPI6_IRQHandler ; SPI6 + DCD SAI1_IRQHandler ; SAI1 + DCD LTDC_IRQHandler ; LTDC + DCD LTDC_ER_IRQHandler ; LTDC error + DCD DMA2D_IRQHandler ; DMA2D + DCD SAI2_IRQHandler ; SAI2 + DCD QUADSPI_IRQHandler ; QUADSPI + DCD LPTIM1_IRQHandler ; LPTIM1 + DCD CEC_IRQHandler ; HDMI_CEC + DCD I2C4_EV_IRQHandler ; I2C4 Event + DCD I2C4_ER_IRQHandler ; I2C4 Error + DCD SPDIF_RX_IRQHandler ; SPDIF_RX + DCD OTG_FS_EP1_OUT_IRQHandler ; USB OTG FS End Point 1 Out + DCD OTG_FS_EP1_IN_IRQHandler ; USB OTG FS End Point 1 In + DCD OTG_FS_WKUP_IRQHandler ; USB OTG FS Wakeup through EXTI + DCD OTG_FS_IRQHandler ; USB OTG FS + DCD DMAMUX1_OVR_IRQHandler ; DMAMUX1 Overrun interrupt + DCD HRTIM1_Master_IRQHandler ; HRTIM Master Timer global Interrupts + DCD HRTIM1_TIMA_IRQHandler ; HRTIM Timer A global Interrupt + DCD HRTIM1_TIMB_IRQHandler ; HRTIM Timer B global Interrupt + DCD HRTIM1_TIMC_IRQHandler ; HRTIM Timer C global Interrupt + DCD HRTIM1_TIMD_IRQHandler ; HRTIM Timer D global Interrupt + DCD HRTIM1_TIME_IRQHandler ; HRTIM Timer E global Interrupt + DCD HRTIM1_FLT_IRQHandler ; HRTIM Fault global Interrupt + DCD DFSDM1_FLT0_IRQHandler ; DFSDM Filter0 Interrupt + DCD DFSDM1_FLT1_IRQHandler ; DFSDM Filter1 Interrupt + DCD DFSDM1_FLT2_IRQHandler ; DFSDM Filter2 Interrupt + DCD DFSDM1_FLT3_IRQHandler ; DFSDM Filter3 Interrupt + DCD SAI3_IRQHandler ; SAI3 global Interrupt + DCD SWPMI1_IRQHandler ; Serial Wire Interface 1 global interrupt + DCD TIM15_IRQHandler ; TIM15 global Interrupt + DCD TIM16_IRQHandler ; TIM16 global Interrupt + DCD TIM17_IRQHandler ; TIM17 global Interrupt + DCD MDIOS_WKUP_IRQHandler ; MDIOS Wakeup Interrupt + DCD MDIOS_IRQHandler ; MDIOS global Interrupt + DCD JPEG_IRQHandler ; JPEG global Interrupt + DCD MDMA_IRQHandler ; MDMA global Interrupt + DCD 0 ; Reserved + DCD SDMMC2_IRQHandler ; SDMMC2 global Interrupt + DCD HSEM1_IRQHandler ; HSEM1 global Interrupt + DCD HSEM2_IRQHandler ; HSEM2 global Interrupt + DCD ADC3_IRQHandler ; ADC3 global Interrupt + DCD DMAMUX2_OVR_IRQHandler ; DMAMUX Overrun interrupt + DCD BDMA_Channel0_IRQHandler ; BDMA Channel 0 global Interrupt + DCD BDMA_Channel1_IRQHandler ; BDMA Channel 1 global Interrupt + DCD BDMA_Channel2_IRQHandler ; BDMA Channel 2 global Interrupt + DCD BDMA_Channel3_IRQHandler ; BDMA Channel 3 global Interrupt + DCD BDMA_Channel4_IRQHandler ; BDMA Channel 4 global Interrupt + DCD BDMA_Channel5_IRQHandler ; BDMA Channel 5 global Interrupt + DCD BDMA_Channel6_IRQHandler ; BDMA Channel 6 global Interrupt + DCD BDMA_Channel7_IRQHandler ; BDMA Channel 7 global Interrupt + DCD COMP1_IRQHandler ; COMP1 global Interrupt + DCD LPTIM2_IRQHandler ; LP TIM2 global interrupt + DCD LPTIM3_IRQHandler ; LP TIM3 global interrupt + DCD LPTIM4_IRQHandler ; LP TIM4 global interrupt + DCD LPTIM5_IRQHandler ; LP TIM5 global interrupt + DCD LPUART1_IRQHandler ; LP UART1 interrupt + DCD WWDG_RST_IRQHandler ; Window Watchdog reset interrupt (exti_d2_wwdg_it, exti_d1_wwdg_it) + DCD CRS_IRQHandler ; Clock Recovery Global Interrupt + DCD ECC_IRQHandler ; ECC diagnostic Global Interrupt + DCD SAI4_IRQHandler ; SAI4 global interrupt + DCD 0 ; Reserved + DCD HOLD_CORE_IRQHandler ; Hold core interrupt + DCD WAKEUP_PIN_IRQHandler ; Interrupt for all 6 wake-up pins + + +__Vectors_End + +__Vectors_Size EQU __Vectors_End - __Vectors + + AREA |.text|, CODE, READONLY + +; Reset handler +Reset_Handler PROC + EXPORT Reset_Handler [WEAK] + IMPORT SystemInit + IMPORT __main + + LDR R0, =SystemInit + BLX R0 + LDR R0, =__main + BX R0 + ENDP + +; Dummy Exception Handlers (infinite loops which can be modified) + +NMI_Handler PROC + EXPORT NMI_Handler [WEAK] + B . + ENDP +HardFault_Handler\ + PROC + EXPORT HardFault_Handler [WEAK] + B . + ENDP +MemManage_Handler\ + PROC + EXPORT MemManage_Handler [WEAK] + B . + ENDP +BusFault_Handler\ + PROC + EXPORT BusFault_Handler [WEAK] + B . + ENDP +UsageFault_Handler\ + PROC + EXPORT UsageFault_Handler [WEAK] + B . + ENDP +SVC_Handler PROC + EXPORT SVC_Handler [WEAK] + B . + ENDP +DebugMon_Handler\ + PROC + EXPORT DebugMon_Handler [WEAK] + B . + ENDP +PendSV_Handler PROC + EXPORT PendSV_Handler [WEAK] + B . + ENDP +SysTick_Handler PROC + EXPORT SysTick_Handler [WEAK] + B . + ENDP + +Default_Handler PROC + + EXPORT WWDG_IRQHandler [WEAK] + EXPORT PVD_AVD_IRQHandler [WEAK] + EXPORT TAMP_STAMP_IRQHandler [WEAK] + EXPORT RTC_WKUP_IRQHandler [WEAK] + EXPORT FLASH_IRQHandler [WEAK] + EXPORT RCC_IRQHandler [WEAK] + EXPORT EXTI0_IRQHandler [WEAK] + EXPORT EXTI1_IRQHandler [WEAK] + EXPORT EXTI2_IRQHandler [WEAK] + EXPORT EXTI3_IRQHandler [WEAK] + EXPORT EXTI4_IRQHandler [WEAK] + EXPORT DMA1_Stream0_IRQHandler [WEAK] + EXPORT DMA1_Stream1_IRQHandler [WEAK] + EXPORT DMA1_Stream2_IRQHandler [WEAK] + EXPORT DMA1_Stream3_IRQHandler [WEAK] + EXPORT DMA1_Stream4_IRQHandler [WEAK] + EXPORT DMA1_Stream5_IRQHandler [WEAK] + EXPORT DMA1_Stream6_IRQHandler [WEAK] + EXPORT DMA1_Stream7_IRQHandler [WEAK] + EXPORT ADC_IRQHandler [WEAK] + EXPORT FDCAN1_IT0_IRQHandler [WEAK] + EXPORT FDCAN2_IT0_IRQHandler [WEAK] + EXPORT FDCAN1_IT1_IRQHandler [WEAK] + EXPORT FDCAN2_IT1_IRQHandler [WEAK] + EXPORT EXTI9_5_IRQHandler [WEAK] + EXPORT TIM1_BRK_IRQHandler [WEAK] + EXPORT TIM1_UP_IRQHandler [WEAK] + EXPORT TIM1_TRG_COM_IRQHandler [WEAK] + EXPORT TIM1_CC_IRQHandler [WEAK] + EXPORT TIM2_IRQHandler [WEAK] + EXPORT TIM3_IRQHandler [WEAK] + EXPORT TIM4_IRQHandler [WEAK] + EXPORT I2C1_EV_IRQHandler [WEAK] + EXPORT I2C1_ER_IRQHandler [WEAK] + EXPORT I2C2_EV_IRQHandler [WEAK] + EXPORT I2C2_ER_IRQHandler [WEAK] + EXPORT SPI1_IRQHandler [WEAK] + EXPORT SPI2_IRQHandler [WEAK] + EXPORT USART1_IRQHandler [WEAK] + EXPORT USART2_IRQHandler [WEAK] + EXPORT USART3_IRQHandler [WEAK] + EXPORT EXTI15_10_IRQHandler [WEAK] + EXPORT RTC_Alarm_IRQHandler [WEAK] + EXPORT TIM8_BRK_TIM12_IRQHandler [WEAK] + EXPORT TIM8_UP_TIM13_IRQHandler [WEAK] + EXPORT TIM8_TRG_COM_TIM14_IRQHandler [WEAK] + EXPORT TIM8_CC_IRQHandler [WEAK] + EXPORT DMA1_Stream7_IRQHandler [WEAK] + EXPORT FMC_IRQHandler [WEAK] + EXPORT SDMMC1_IRQHandler [WEAK] + EXPORT TIM5_IRQHandler [WEAK] + EXPORT SPI3_IRQHandler [WEAK] + EXPORT UART4_IRQHandler [WEAK] + EXPORT UART5_IRQHandler [WEAK] + EXPORT TIM6_DAC_IRQHandler [WEAK] + EXPORT TIM7_IRQHandler [WEAK] + EXPORT DMA2_Stream0_IRQHandler [WEAK] + EXPORT DMA2_Stream1_IRQHandler [WEAK] + EXPORT DMA2_Stream2_IRQHandler [WEAK] + EXPORT DMA2_Stream3_IRQHandler [WEAK] + EXPORT DMA2_Stream4_IRQHandler [WEAK] + EXPORT ETH_IRQHandler [WEAK] + EXPORT ETH_WKUP_IRQHandler [WEAK] + EXPORT FDCAN_CAL_IRQHandler [WEAK] + EXPORT CM7_SEV_IRQHandler [WEAK] + EXPORT CM4_SEV_IRQHandler [WEAK] + EXPORT DMA2_Stream5_IRQHandler [WEAK] + EXPORT DMA2_Stream6_IRQHandler [WEAK] + EXPORT DMA2_Stream7_IRQHandler [WEAK] + EXPORT USART6_IRQHandler [WEAK] + EXPORT I2C3_EV_IRQHandler [WEAK] + EXPORT I2C3_ER_IRQHandler [WEAK] + EXPORT OTG_HS_EP1_OUT_IRQHandler [WEAK] + EXPORT OTG_HS_EP1_IN_IRQHandler [WEAK] + EXPORT OTG_HS_WKUP_IRQHandler [WEAK] + EXPORT OTG_HS_IRQHandler [WEAK] + EXPORT DCMI_IRQHandler [WEAK] + EXPORT RNG_IRQHandler [WEAK] + EXPORT FPU_IRQHandler [WEAK] + EXPORT UART7_IRQHandler [WEAK] + EXPORT UART8_IRQHandler [WEAK] + EXPORT SPI4_IRQHandler [WEAK] + EXPORT SPI5_IRQHandler [WEAK] + EXPORT SPI6_IRQHandler [WEAK] + EXPORT SAI1_IRQHandler [WEAK] + EXPORT LTDC_IRQHandler [WEAK] + EXPORT LTDC_ER_IRQHandler [WEAK] + EXPORT DMA2D_IRQHandler [WEAK] + EXPORT SAI2_IRQHandler [WEAK] + EXPORT QUADSPI_IRQHandler [WEAK] + EXPORT LPTIM1_IRQHandler [WEAK] + EXPORT CEC_IRQHandler [WEAK] + EXPORT I2C4_EV_IRQHandler [WEAK] + EXPORT I2C4_ER_IRQHandler [WEAK] + EXPORT SPDIF_RX_IRQHandler [WEAK] + EXPORT OTG_FS_EP1_OUT_IRQHandler [WEAK] + EXPORT OTG_FS_EP1_IN_IRQHandler [WEAK] + EXPORT OTG_FS_WKUP_IRQHandler [WEAK] + EXPORT OTG_FS_IRQHandler [WEAK] + EXPORT DMAMUX1_OVR_IRQHandler [WEAK] + EXPORT HRTIM1_Master_IRQHandler [WEAK] + EXPORT HRTIM1_TIMA_IRQHandler [WEAK] + EXPORT HRTIM1_TIMB_IRQHandler [WEAK] + EXPORT HRTIM1_TIMC_IRQHandler [WEAK] + EXPORT HRTIM1_TIMD_IRQHandler [WEAK] + EXPORT HRTIM1_TIME_IRQHandler [WEAK] + EXPORT HRTIM1_FLT_IRQHandler [WEAK] + EXPORT DFSDM1_FLT0_IRQHandler [WEAK] + EXPORT DFSDM1_FLT1_IRQHandler [WEAK] + EXPORT DFSDM1_FLT2_IRQHandler [WEAK] + EXPORT DFSDM1_FLT3_IRQHandler [WEAK] + EXPORT SAI3_IRQHandler [WEAK] + EXPORT SWPMI1_IRQHandler [WEAK] + EXPORT TIM15_IRQHandler [WEAK] + EXPORT TIM16_IRQHandler [WEAK] + EXPORT TIM17_IRQHandler [WEAK] + EXPORT MDIOS_WKUP_IRQHandler [WEAK] + EXPORT MDIOS_IRQHandler [WEAK] + EXPORT JPEG_IRQHandler [WEAK] + EXPORT MDMA_IRQHandler [WEAK] + EXPORT SDMMC2_IRQHandler [WEAK] + EXPORT HSEM1_IRQHandler [WEAK] + EXPORT HSEM2_IRQHandler [WEAK] + EXPORT ADC3_IRQHandler [WEAK] + EXPORT DMAMUX2_OVR_IRQHandler [WEAK] + EXPORT BDMA_Channel0_IRQHandler [WEAK] + EXPORT BDMA_Channel1_IRQHandler [WEAK] + EXPORT BDMA_Channel2_IRQHandler [WEAK] + EXPORT BDMA_Channel3_IRQHandler [WEAK] + EXPORT BDMA_Channel4_IRQHandler [WEAK] + EXPORT BDMA_Channel5_IRQHandler [WEAK] + EXPORT BDMA_Channel6_IRQHandler [WEAK] + EXPORT BDMA_Channel7_IRQHandler [WEAK] + EXPORT COMP1_IRQHandler [WEAK] + EXPORT LPTIM2_IRQHandler [WEAK] + EXPORT LPTIM3_IRQHandler [WEAK] + EXPORT LPTIM4_IRQHandler [WEAK] + EXPORT LPTIM5_IRQHandler [WEAK] + EXPORT LPUART1_IRQHandler [WEAK] + EXPORT WWDG_RST_IRQHandler [WEAK] + EXPORT CRS_IRQHandler [WEAK] + EXPORT ECC_IRQHandler [WEAK] + EXPORT SAI4_IRQHandler [WEAK] + EXPORT HOLD_CORE_IRQHandler [WEAK] + EXPORT WAKEUP_PIN_IRQHandler [WEAK] + + +WWDG_IRQHandler +PVD_AVD_IRQHandler +TAMP_STAMP_IRQHandler +RTC_WKUP_IRQHandler +FLASH_IRQHandler +RCC_IRQHandler +EXTI0_IRQHandler +EXTI1_IRQHandler +EXTI2_IRQHandler +EXTI3_IRQHandler +EXTI4_IRQHandler +DMA1_Stream0_IRQHandler +DMA1_Stream1_IRQHandler +DMA1_Stream2_IRQHandler +DMA1_Stream3_IRQHandler +DMA1_Stream4_IRQHandler +DMA1_Stream5_IRQHandler +DMA1_Stream6_IRQHandler +ADC_IRQHandler +FDCAN1_IT0_IRQHandler +FDCAN2_IT0_IRQHandler +FDCAN1_IT1_IRQHandler +FDCAN2_IT1_IRQHandler +EXTI9_5_IRQHandler +TIM1_BRK_IRQHandler +TIM1_UP_IRQHandler +TIM1_TRG_COM_IRQHandler +TIM1_CC_IRQHandler +TIM2_IRQHandler +TIM3_IRQHandler +TIM4_IRQHandler +I2C1_EV_IRQHandler +I2C1_ER_IRQHandler +I2C2_EV_IRQHandler +I2C2_ER_IRQHandler +SPI1_IRQHandler +SPI2_IRQHandler +USART1_IRQHandler +USART2_IRQHandler +USART3_IRQHandler +EXTI15_10_IRQHandler +RTC_Alarm_IRQHandler +TIM8_BRK_TIM12_IRQHandler +TIM8_UP_TIM13_IRQHandler +TIM8_TRG_COM_TIM14_IRQHandler +TIM8_CC_IRQHandler +DMA1_Stream7_IRQHandler +FMC_IRQHandler +SDMMC1_IRQHandler +TIM5_IRQHandler +SPI3_IRQHandler +UART4_IRQHandler +UART5_IRQHandler +TIM6_DAC_IRQHandler +TIM7_IRQHandler +DMA2_Stream0_IRQHandler +DMA2_Stream1_IRQHandler +DMA2_Stream2_IRQHandler +DMA2_Stream3_IRQHandler +DMA2_Stream4_IRQHandler +ETH_IRQHandler +ETH_WKUP_IRQHandler +FDCAN_CAL_IRQHandler +CM7_SEV_IRQHandler +CM4_SEV_IRQHandler +DMA2_Stream5_IRQHandler +DMA2_Stream6_IRQHandler +DMA2_Stream7_IRQHandler +USART6_IRQHandler +I2C3_EV_IRQHandler +I2C3_ER_IRQHandler +OTG_HS_EP1_OUT_IRQHandler +OTG_HS_EP1_IN_IRQHandler +OTG_HS_WKUP_IRQHandler +OTG_HS_IRQHandler +DCMI_IRQHandler +RNG_IRQHandler +FPU_IRQHandler +UART7_IRQHandler +UART8_IRQHandler +SPI4_IRQHandler +SPI5_IRQHandler +SPI6_IRQHandler +SAI1_IRQHandler +LTDC_IRQHandler +LTDC_ER_IRQHandler +DMA2D_IRQHandler +SAI2_IRQHandler +QUADSPI_IRQHandler +LPTIM1_IRQHandler +CEC_IRQHandler +I2C4_EV_IRQHandler +I2C4_ER_IRQHandler +SPDIF_RX_IRQHandler +OTG_FS_EP1_OUT_IRQHandler +OTG_FS_EP1_IN_IRQHandler +OTG_FS_WKUP_IRQHandler +OTG_FS_IRQHandler +DMAMUX1_OVR_IRQHandler +HRTIM1_Master_IRQHandler +HRTIM1_TIMA_IRQHandler +HRTIM1_TIMB_IRQHandler +HRTIM1_TIMC_IRQHandler +HRTIM1_TIMD_IRQHandler +HRTIM1_TIME_IRQHandler +HRTIM1_FLT_IRQHandler +DFSDM1_FLT0_IRQHandler +DFSDM1_FLT1_IRQHandler +DFSDM1_FLT2_IRQHandler +DFSDM1_FLT3_IRQHandler +SAI3_IRQHandler +SWPMI1_IRQHandler +TIM15_IRQHandler +TIM16_IRQHandler +TIM17_IRQHandler +MDIOS_WKUP_IRQHandler +MDIOS_IRQHandler +JPEG_IRQHandler +MDMA_IRQHandler +SDMMC2_IRQHandler +HSEM1_IRQHandler +HSEM2_IRQHandler +ADC3_IRQHandler +DMAMUX2_OVR_IRQHandler +BDMA_Channel0_IRQHandler +BDMA_Channel1_IRQHandler +BDMA_Channel2_IRQHandler +BDMA_Channel3_IRQHandler +BDMA_Channel4_IRQHandler +BDMA_Channel5_IRQHandler +BDMA_Channel6_IRQHandler +BDMA_Channel7_IRQHandler +COMP1_IRQHandler +LPTIM2_IRQHandler +LPTIM3_IRQHandler +LPTIM4_IRQHandler +LPTIM5_IRQHandler +LPUART1_IRQHandler +WWDG_RST_IRQHandler +CRS_IRQHandler +ECC_IRQHandler +SAI4_IRQHandler +HOLD_CORE_IRQHandler +WAKEUP_PIN_IRQHandler + + B . + + ENDP + + ALIGN + +;******************************************************************************* +; User Stack and Heap initialization +;******************************************************************************* + IF :DEF:__MICROLIB + + EXPORT __initial_sp + EXPORT __heap_base + EXPORT __heap_limit + + ELSE + + IMPORT __use_two_region_memory + EXPORT __user_initial_stackheap + +__user_initial_stackheap + + LDR R0, = Heap_Mem + LDR R1, =(Stack_Mem + Stack_Size) + LDR R2, = (Heap_Mem + Heap_Size) + LDR R3, = Stack_Mem + BX LR + + ALIGN + + ENDIF + + END + +;************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE***** diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.cpp b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.cpp new file mode 100644 index 0000000000..8e728103fd --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.cpp @@ -0,0 +1,284 @@ + + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + + + +#include "theApplication_config.h" + +//#define REDIRECT_ERRORMANAGER_OVER_BACKDOOR + +#include "embot_app_eth_theBackdoor.h" + + +namespace embot { namespace app { namespace eth { + + // user defined onidle + void theApplication_Config_onIdle(embot::os::Thread *t, void* idleparam) + { + static volatile uint32_t cnt {0}; + cnt++; + } + + +// embot::os::EventThread *thr {nullptr}; +// +// void startup(embot::os::Thread *t, void *p) +// { +// embot::core::print("startup: ...."); +// } +// +// void onevent(embot::os::Thread *t, embot::os::EventMask eventmask, void *p) +// { +// if(0 == eventmask) +// { // timeout ... + +// embot::core::TimeFormatter tf(embot::core::now()); +// std::string str = "onevent: timeout @ time = " + tf.to_string(); +// //embot::core::print(str); +// +// embot::app::eth::emit(sevINFO, {"testThread", t}, {}, str); +// +// return; +// } + +// // eval other events +// +// } +// +// void tTEST(void *p) { reinterpret_cast(p)->run(); } + + void emit_over_backdoor(theErrorManager::Severity sev, const theErrorManager::Caller &caller, const theErrorManager::Descriptor &des, const std::string &str); + +// void bkdoor_onrx(EOpacket *rxp) +// { +// uint8_t *data {nullptr}; +// uint16_t size {0}; +// eo_packet_Payload_Get(rxp, &data, &size); +// embot::core::print("theBackdoor-> " + std::string(reinterpret_cast(data))); +// } +// + + void addservice(); + + // user defined worker called by INIT thread just before start of scheduling + void theApplication_Config_inituserdefined(void *p) + { + // add what yout want + volatile uint32_t ciao {0}; + ciao++; + embot::core::print("hello world... you can ping me now"); + + + // in here i start whatever i want + + // for example: + // - the object theBackdoor + // - an EventThread periodically activated by its timeout + +// // add the backdoor +// constexpr embot::app::eth::theBackdoor::Config bkdconfig = +// { +// { embot::os::Priority::belownorm23, 2*1024 }, // thread +// { {2, 64, 2, 512}, 6666 }, // socket.size, socket.localport +// {embot::app::eth::IPlocalhost, 6666}, // hostaddress +// bkdoor_onrx, // onreception does ... nothing so far +// }; +// +// embot::app::eth::theBackdoor::getInstance().initialise(bkdconfig); + + +// constexpr embot::core::relTime timeout {5*1000*embot::core::time1millisec}; + +// embot::os::EventThread::Config tCfg { +// 6*1024, +// embot::os::Priority::belownorm23, +// startup, nullptr, +// timeout, +// onevent, +// "tTEST" +// }; +// +// +// // create the test thread +// thr = new embot::os::EventThread; +// // and start it +// thr->start(tCfg, tTEST); +// +//#if defined(REDIRECT_ERRORMANAGER_OVER_BACKDOOR) +// // now link theErrorManager to send onto the theBackdoor +// embot::app::eth::theErrorManager::getInstance().set(emit_over_backdoor); +//#endif + + embot::app::eth::addservice(); + + } + + void theApplication_Config_errorman_onemit(theErrorManager::Severity sev, const theErrorManager::Caller &caller, const theErrorManager::Descriptor &des, const std::string &str) + { + std::string timenow = embot::core::TimeFormatter(embot::core::now()).to_string(); + std::string eobjstr = (true == caller.isvalid()) ? caller.objectname : "OBJ"; + std::string threadname = (true == caller.isvalid()) ? caller.owner->getName() : "THR"; + std::string severity = theErrorManager::to_cstring(sev); + + embot::core::print(std::string("[") + severity + "] @" + timenow + " (" + eobjstr + ", " + threadname + "): " + str); + + if(theErrorManager::Severity::trace == sev) + { + return; + } + + // you may in here send the diagnostics message + if(true == des.isvalid()) + { + + } + + if(theErrorManager::Severity::fatal == sev) + { + for(;;); + } + + } + +// void emit_over_backdoor(theErrorManager::Severity sev, const theErrorManager::Caller &caller, const theErrorManager::Descriptor &des, const std::string &str) +// { +// // form a string and then send it to the ... + +// std::string timenow = embot::core::TimeFormatter(embot::core::now()).to_string(); +// std::string eobjstr = (true == caller.isvalid()) ? caller.objectname : "OBJ"; +// std::string threadname = (true == caller.isvalid()) ? caller.owner->getName() : "THR"; +// std::string severity = theErrorManager::to_cstring(sev); +// +// std::string out = std::string("[") + severity + "] @" + timenow + " (" + eobjstr + ", " + threadname + "): " + str + "\n"; +// +// static EOpacket *pkt {nullptr}; +// +// if(nullptr == pkt) +// { +// pkt = eo_packet_New(500); // max size +// } +// +// uint8_t *data = reinterpret_cast(const_cast(out.c_str())); +// eo_packet_Payload_Set(pkt, data, strlen(out.c_str())); +// eo_packet_Addressing_Set(pkt, EO_COMMON_IPV4ADDR(10, 0, 1, 104), 6666); +// +// embot::app::eth::theBackdoor::getInstance().transmit(pkt); +// +//// if(theErrorManager::Severity::fatal == sev) +//// { +//// for(;;); +//// } +// +// } + + +}}} + +//#include "embot_app_eth_theServices.h" +#include "embot_app_eth_theHandler.h" + +namespace embot { namespace app { namespace eth { + + +//void init_ctrl_socket(); +//void init_ctrl_transceiver(); +//void init_configurator(); +//void init_runner(); +//void init_err(); +//void init_sm(); + +void addservice() +{ + embot::app::eth::theHandler::getInstance().initialise({}); + +//// init_ctrl_socket(); +// init_ctrl_transceiver(); +// init_configurator(); +// init_runner(); +// init_err(); +// init_sm(); +// embot::app::eth::theServices::getInstance().initialise({}); +} + + +//#include "EOMtheEMSsocket.h" +//void init_ctrl_socket() +//{ +// +// eOemssocket_cfg_t socketcfg {}; // to be specified +// +// eom_emssocket_Initialise(&socketcfg); + +// eOipv4addr_t hostipv4addr {0}; + +// // i try connection now, so that if the hostaddress does not change, then during transmission we dont do a connect anymore +// eOresult_t res = eom_emssocket_Connect(eom_emssocket_GetHandle(), hostipv4addr, 5*EOK_reltime100ms); +//} + +//#include "EOMtheEMStransceiver.h" +//void init_ctrl_transceiver() +//{ +// eOemstransceiver_cfg_t transcfg; // to be specified +// eom_emstransceiver_Initialise(&transcfg); + +// eOipv4addr_t hostipv4addr {0}; // to be specified + +// // i try connection now, so that if the hostaddress does not change, then during transmission we dont do a connect anymore +// eOresult_t res = eom_emssocket_Connect(eom_emssocket_GetHandle(), hostipv4addr, 5*EOK_reltime100ms); + +//// +//// if(eores_OK == res) +//// { +//// // set led0 to ON +//// eo_ledpulser_On(eo_ledpulser_GetHandle(), eo_ledpulser_led_zero); +//// } +//// else +//// { +//// eo_ledpulser_Off(eo_ledpulser_GetHandle(), eo_ledpulser_led_zero); +//// } +//// +//// snprintf(str, sizeof(str), "eom_emssocket_Connect is over. Link w/ host %s is %s", ipv4str, (eores_OK == res) ? "ON" : "OFF"); +//// eo_errman_Trace(eo_errman_GetHandle(), str, s_eobj_ownname); +//} + + +//#include "EOMtheEMSconfigurator.h" +//void init_configurator() +//{ +// eOemsconfigurator_cfg_t cfgobjcfg {}; +// eom_emsconfigurator_Initialise(&cfgobjcfg); +//} + + +//#include "EOMtheEMSrunner.h" +//void init_runner() +//{ +// eOemsrunner_cfg_t cfgobjrun {}; +// eom_emsrunner_Initialise(&cfgobjrun); +//} + +//#include "EOMtheEMSerror.h" +//void init_err() +//{ +// eOemserror_cfg_t cfgobj {}; +// eom_emserror_Initialise(&cfgobj); +//} + +//#include "EOsm.h" +//#include "eOcfg_sm_EMSappl.h" +//EOsm *thesm {nullptr}; + +//void init_sm() +//{ +// thesm = eo_sm_New(eo_cfg_sm_EMSappl_Get()); +// eo_sm_Start(thesm); +//} + +}}} // namespace embot { namespace app { namespace eth { + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h new file mode 100644 index 0000000000..ae6d8fcb4e --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theApplication_config.h @@ -0,0 +1,90 @@ + + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef __THEAPPLICATION_CONFIG_H_ +#define __THEAPPLICATION_CONFIG_H_ + +#include "embot_app_eth_theApplication.h" + +#include "embot_os_theTimerManager.h" +#include "embot_os_theCallbackManager.h" +#include "embot_app_eth_theErrorManager.h" +#include "embot_app_theLEDmanager.h" + +#include "ipal_cfg2.h" + +namespace embot { namespace app { namespace eth { + + void theApplication_Config_onIdle(embot::os::Thread *t, void* idleparam); + + // user defined worker + void theApplication_Config_inituserdefined(void *p); + + // errors + void theApplication_Config_errorman_onemit(theErrorManager::Severity sev, const theErrorManager::Caller &caller, const theErrorManager::Descriptor &des, const std::string &str); + + // NOTE: theApplication_Config.property MUST be used in theEnvironment_config.h to as follow + // constexpr theEnvironment::Config theEnvironment_Config {theApplication_Config.property}; + + constexpr theApplication::Config theApplication_Config + { + .property = + { + Process::eApplication, +//#if defined(WRIST_MK2) +// #if defined(WRIST_MK2_RIGHT) +// {101, 5}, +// #else +// {102, 5}, +// #endif +//#else + {1, 1}, +//#endif + {2023, Month::Oct, Day::ten, 21, 00} + }, + .OStick = 1000*embot::core::time1microsec, + .OSstacksizeinit = 10*1024, + .OSuserdefinit = {theApplication_Config_inituserdefined, nullptr}, + .OSstacksizeidle = 8*1024, + .OSonidle = theApplication_Config_onIdle, + .TMRMANconfig = {1024, 8, embot::os::Priority::system50}, + .CBKMANconfig = {2*1024, 8, embot::os::Priority::system49}, + .ERRMANconfig = { theApplication_Config_errorman_onemit }, + .allLEDs = + { + embot::hw::LED::one, embot::hw::LED::two, embot::hw::LED::three, + embot::hw::LED::four + }, + .pulseLED = embot::hw::LED::three, + .pulseFREQ = embot::core::time1second, + .IPNETconfig = + { + {&ipal_cfg2, true}, // ipal + {embot::os::Priority::high44, 4*1024}, // proc + {embot::os::Priority::high43, 4*1024}, // tick + 25*embot::core::time1millisec, // maxidletime + {4, 4} // sockets ... {numberofsockets, maxdatagramenqueuedintx} + }, + .LISTENERconfig = + { + { embot::os::Priority::belownorm22, 4*1024 }, // thread + {{2, 32, 1, 128}, 3333}, // socket.size, socket.localport + {embot::app::eth::IPlocalhost, 3333} // hostaddress + } + }; + +}}} + + + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theEnvironment_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theEnvironment_config.h new file mode 100644 index 0000000000..6b8f454bd0 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theEnvironment_config.h @@ -0,0 +1,28 @@ + + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef __THEENVIRONMENT_CONFIG_H_ +#define __THEENVIRONMENT_CONFIG_H_ + +#include "embot_app_eth_theEnvironment.h" +#include "theApplication_config.h" + +namespace embot { namespace app { namespace eth { + + constexpr theEnvironment::Config theEnvironment_Config {theApplication_Config.property}; + +}}} + + + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theErrorManager_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theErrorManager_config.h new file mode 100644 index 0000000000..22ddbd539f --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theErrorManager_config.h @@ -0,0 +1,43 @@ + + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef __THEERRORMANAGER_CONFIG_H_ +#define __THEERRORMANAGER_CONFIG_H_ + +#include "embot_app_eth_theErrorManager.h" + +#include "theApplication_config.h" + + +namespace embot { namespace app { namespace eth { + +// void onemit(theErrorManager::Severity sev, const theErrorManager::Descriptor &des, const std::string &str) +// { +// embot::core::print("HI: "+ str); +// +// if(theErrorManager::Severity::fatal == sev) +// { +// for(;;); +// } +// } + + constexpr theErrorManager::Config theErrorManager_Config = theApplication_Config.ERRMANconfig; +// { +// theApplication_Config.ERRMANconfig.onemit +// }; + +}}} + + + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theHandler_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theHandler_config.h new file mode 100644 index 0000000000..f4eb8bd34c --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theHandler_config.h @@ -0,0 +1,157 @@ + + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef __THEHANDLER_CONFIG_H_ +#define __THEHANDLER_CONFIG_H_ + +#include "embot_app_eth.h" +#include "embot_app_eth_theHandler.h" + +#include "EOMtheEMSsocket.h" + + +#include "EOMtheEMSconfigurator.h" +#include "EOMtheEMSrunner.h" +#include "EOMtheEMSerror.h" + +#include "embot_os_common.h" +#include "embot_hw_timer.h" +#include "embot_hw_bsp_amc_config.h" +#include "theApplication_config.h" + + +#include "embot_app_eth_theETHmonitor.h" + +namespace embot { namespace app { namespace eth { + + constexpr eOemssocket_cfg_t theHandler_EOMtheEMSsocket_Config + { + .inpdatagramnumber = 3, + .outdatagramnumber = 2, + .inpdatagramsizeof = 768, + .outdatagramsizeof = 1416, //EOMTHEEMSAPPLCFG_SOCKET_OUTDGRAMSIZEOF + .localport = 12345, + .usemutex = eobool_true + }; + + constexpr eOemstransceiver_cfg_t theHandler_EOMtheEMStransceiver_Config + { + .nvsetbrdcfg = &eonvset_BRDcfgBasic, + .hostipv4addr = IPaddress(10, 0, 1, 104).v, + .hostipv4port = 12345, + .sizes = + { + .capacityoftxpacket = theHandler_EOMtheEMSsocket_Config.outdatagramsizeof, //EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMECAPACITY, + .capacityofrop = 384,// EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPCAPACITY, + .capacityofropframeregulars = 1024,// EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREGULARSCAPACITY, + .capacityofropframeoccasionals = 128, //EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEOCCASIONALSCAPACITY, + .capacityofropframereplies = 400, // EOMTHEEMSAPPLCFG_TRANSCEIVER_ROPFRAMEREPLIESCAPACITY, + .maxnumberofregularrops = 32 // EOMTHEEMSAPPLCFG_TRANSCEIVER_MAXNUMOFREGULARROPS + }, + .transprotection = eo_trans_protection_none, + .nvsetprotection = eo_nvset_protection_none, + .proxycfg = + { + .mode = eoproxy_mode_enabled, + .capacityoflistofropdes = 24, + .replyroptimeout = 1000*embot::core::time1millisec, + .mutex_fn_new = nullptr, + .transceiver = nullptr + } + }; + + constexpr eOemsconfigurator_cfg_t theHandler_EOMtheEMSconfigurator_Config + { + .taskpriority = embot::core::tointegral(embot::os::Priority::abovenorm36), + .taskstacksize = 8*1024 + }; + + constexpr eOemserror_cfg_t theHandler_EOMtheEMSerror_Config + { + .taskpriority = embot::core::tointegral(embot::os::Priority::abovenorm35), + .taskstacksize = 4*1024 + }; + + +#if defined(EMBOT_ENABLE_hw_timer_emulated) + + // 10: period (1ms) -> 10 ms, dostart (0.4ms) -> 4ms, safetx (0.25ms) -> 2.5 ms + // 100: period (1ms) -> 100 ms, dostart (0.4ms) -> 40ms, safetx (0.25ms) -> 25 ms + constexpr uint32_t sc {(theApplication_Config.OStick == 500*embot::core::time1microsec) ? 10 : 100}; + +#else + constexpr uint32_t sc {1}; +#endif + + constexpr embot::core::relTime runner_period = embot::core::time1millisec; + constexpr embot::core::relTime runner_execRXafter = 0; // executes at beginning of period + constexpr embot::core::relTime runner_safeRXexecutiontime = 300*embot::core::time1microsec; + constexpr embot::core::relTime runner_execDOafter = 400*embot::core::time1microsec; + constexpr embot::core::relTime runner_safeDOexecutiontime = 200*embot::core::time1microsec; + constexpr embot::core::relTime runner_execTXafter = 700*embot::core::time1microsec; + constexpr embot::core::relTime runner_safeTXexecutiontime = 250*embot::core::time1microsec; + + constexpr eOemsrunner_cfg_t theHandler_EOMtheEMSrunner_Config + { + .taskpriority = + { + embot::core::tointegral(embot::os::Priority::abovenorm39), // rx + embot::core::tointegral(embot::os::Priority::abovenorm38), // do + embot::core::tointegral(embot::os::Priority::abovenorm37) // tx + + }, + .taskstacksize = + { + 4*1024, // rx + 4*1024, // do + 4*1024 // tx + }, + .haltimerstart = + { + static_cast(embot::hw::TIMER::one), + static_cast(embot::hw::TIMER::two), + static_cast(embot::hw::TIMER::three) + }, + .haltimeralert = + { + hal_timerNONE, + hal_timerNONE, + hal_timerNONE + }, + .period = sc*runner_period, + .execRXafter = sc*runner_execRXafter, + .safeRXexecutiontime = sc*runner_safeRXexecutiontime, + .execDOafter = sc*runner_execDOafter, + .safeDOexecutiontime = sc*runner_safeDOexecutiontime, + .execTXafter = sc*runner_execTXafter, + .safeTXexecutiontime = sc*runner_safeTXexecutiontime, + .maxnumofRXpackets = 3, + .maxnumofTXpackets = 1, + .modeatstartup = eo_emsrunner_mode_besteffort, + .defaultTXdecimationfactor = 1 + }; + +// constexpr eOipv4addr_t theHandler_EOMtheEMSsocket_hostIPaddress = IPaddress(10, 0, 1, 104).v;// EO_COMMON_IPV4ADDR(10, 0, 1, 104); + + constexpr embot::app::eth::theETHmonitor::Config theHandler_theETHmonitor_Config + { + {embot::os::Priority::belownorm22, 2*1024}, + 100*embot::core::time1millisec, + 60*embot::core::time1second, + {} + }; + +}}} + + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theIPnet_config.h b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theIPnet_config.h new file mode 100644 index 0000000000..f380cd0b3f --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/cfg/theIPnet_config.h @@ -0,0 +1,40 @@ + + +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + +// - include guard ---------------------------------------------------------------------------------------------------- + +#ifndef __THEIPNET_CONFIG_H_ +#define __THEIPNET_CONFIG_H_ + +#include "embot_app_eth_theIPnet.h" + +#include "theApplication_config.h" + +// for the ipal config we use (variable ipal_cfg2) we need: +//#include "ipal_cfg2.h" + +namespace embot { namespace app { namespace eth { + + constexpr theIPnet::Config theIPnet_Config = theApplication_Config.IPNETconfig; +// { +//// {&ipal_cfg2, true}, // ipal +//// {embot::os::Priority::high44, 4*1024}, // proc +//// {embot::os::Priority::high43, 4*1024}, // tick +//// 25*embot::core::time1millisec, // maxidletime +//// {4, 4} // sockets +// theApplication_Config.IPNETconfig +// }; + +}}} + + + +#endif // include-guard + + +// - end-of-file (leave a blank line after)---------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01-wrist.uvoptx b/emBODY/eBcode/arch-arm/board/amc/application/v1/proj/appl-01.uvoptx similarity index 99% rename from emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01-wrist.uvoptx rename to emBODY/eBcode/arch-arm/board/amc/application/v1/proj/appl-01.uvoptx index c6083c5aac..9744183ca3 100644 --- a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01-wrist.uvoptx +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/proj/appl-01.uvoptx @@ -1049,7 +1049,7 @@ embot::app::eth::config - 0 + 1 0 0 0 @@ -3217,7 +3217,7 @@ mc-controller - 0 + 1 0 0 0 @@ -3341,18 +3341,6 @@ 0 0 - - 34 - 181 - 8 - 0 - 0 - 0 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - wrist_decoupler_data.cpp - 0 - 0 - @@ -3363,7 +3351,7 @@ 0 35 - 182 + 181 8 0 0 @@ -3375,7 +3363,7 @@ 35 - 183 + 182 8 0 0 diff --git a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01-wrist.uvprojx b/emBODY/eBcode/arch-arm/board/amc/application/v1/proj/appl-01.uvprojx similarity index 99% rename from emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01-wrist.uvprojx rename to emBODY/eBcode/arch-arm/board/amc/application/v1/proj/appl-01.uvprojx index 25b9f0abae..1e20eca387 100644 --- a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01-wrist.uvprojx +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/proj/appl-01.uvprojx @@ -10,13 +10,13 @@ amc-appl-01-osal-stlink 0x4 ARM-ADS - 6180000::V6.18::ARMCLANG + 6190000::V6.19::ARMCLANG 1 STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -2507,11 +2507,6 @@ 8 ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp - - wrist_decoupler_data.cpp - 8 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - @@ -2541,7 +2536,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -4860,11 +4855,6 @@ 8 ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp - - wrist_decoupler_data.cpp - 8 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - @@ -4894,7 +4884,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -7723,11 +7713,6 @@ 8 ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp - - wrist_decoupler_data.cpp - 8 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - @@ -7757,7 +7742,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -9362,11 +9347,6 @@ 8 ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp - - wrist_decoupler_data.cpp - 8 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - @@ -9396,7 +9376,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -11001,11 +10981,6 @@ 8 ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp - - wrist_decoupler_data.cpp - 8 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - @@ -11035,7 +11010,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -12997,11 +12972,6 @@ 8 ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp - - wrist_decoupler_data.cpp - 8 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v1/src/amc-appl-01.cpp b/emBODY/eBcode/arch-arm/board/amc/application/v1/src/amc-appl-01.cpp new file mode 100644 index 0000000000..0323964533 --- /dev/null +++ b/emBODY/eBcode/arch-arm/board/amc/application/v1/src/amc-appl-01.cpp @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2022 iCub Tech - Istituto Italiano di Tecnologia + * Author: Marco Accame + * email: marco.accame@iit.it +*/ + +#if 0 + +### How to use `embot::app::eth::theApplication` + +Just ... + +```c++ +#include "embot_app_eth_theApplication.h" + +int main(void) +{ + embot::app::eth::theApplication::getInstance().start({}); +} +``` + +and ... produce a file named `theApplication_config.h` w/ following definition: + +```C++ + constexpr theApplication::Config theApplication_Config + { + .property = + { + Process::eApplication, + {3, 0}, + {2022, Month::Apr, Day::fourteen, 15, 16} + }, + .OStick = 1000*embot::core::time1microsec, + ... + etc +``` + + +#endif + + +#include "embot_app_eth_theApplication.h" + +#include "embot_hw_bsp_amc.h" + +int main(void) +{ + embot::hw::bsp::amc::set(embot::hw::bsp::amc::OnSpecApplication); + embot::app::eth::theApplication::getInstance().start({}); +} + +// -------------------------------------------------------------------------------------------------------------------- +// - end-of-file (leave a blank line after) +// -------------------------------------------------------------------------------------------------------------------- + diff --git a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h b/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h index 69478db673..68a7730b65 100644 --- a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h +++ b/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/cfg/theApplication_config.h @@ -38,16 +38,16 @@ namespace embot { namespace app { namespace eth { .property = { Process::eApplication, -#if defined(WRIST_MK2) - #if defined(WRIST_MK2_RIGHT) - {101, 11}, - #else - {102, 11}, - #endif -#else - {103, 11}, -#endif - {2023, Month::Sep, Day::fourteen, 11, 18} +//#if defined(WRIST_MK2) +// #if defined(WRIST_MK2_RIGHT) +// {101, 5}, +// #else +// {102, 5}, +// #endif +//#else + {104, 7}, +//#endif + {2023, Month::Oct, Day::three, 21, 00} }, .OStick = 1000*embot::core::time1microsec, .OSstacksizeinit = 10*1024, diff --git a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01.uvoptx b/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01.uvoptx index 6f11c8453a..36332ebbdf 100644 --- a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01.uvoptx +++ b/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01.uvoptx @@ -12,7 +12,7 @@ *.lib *.txt; *.h; *.inc; *.md *.plm - *.cpp + *.cpp; *.cc; *.cxx 0 @@ -75,7 +75,7 @@ 1 0 - 0 + 1 18 @@ -103,7 +103,7 @@ 1 0 0 - 6 + 1 @@ -113,91 +113,54 @@ - .\eventviewer-stm32-cfg.ini - STLink\ST-LINKIII-KEIL_SWO.dll + + BIN\ULP2CM3.DLL 0 ULP2CM3 - -UP0948199 -O206 -S12 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65539 -TC400000000 -TT10000000 -TP11 -TDX0 -TDD0 -TDS8001 -TDT0 -TDC10 -TIE80000001 -TIP9 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + -UP0948193 -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65555 -TC400000000 -TT10000000 -TP18 -TDX0 -TDD0 -TDS8001 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) 0 - JL2CM3 - -U752001923 -O78 -S8 -ZTIFSpeedSel50000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(4) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - UL2CM3 - UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD20000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + ARMRTXEVENTFLAGS + -L70 -Z18 -C0 -M0 -T1 0 - ST-LINKIII-KEIL_SWO - -U002E00303137510A39383538 -O206 -SF50000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(6BA02477) -L00(0) -TO131091 -TC400000000 -TT10000000 -TP21 -TDS8021 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) + DLGTARM + (1010=-1,-1,-1,-1,0)(6017=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(6016=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) 0 - ARMRTXEVENTFLAGS - -L196 -Z36 -C0 -M1 -T1 + ARMDBGFLAGS + 0 - DLGTARM - (1010=-1,-1,-1,-1,0)(6017=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(6016=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) + ST-LINKIII-KEIL_SWO + -U -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(6BA02477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) 0 - ARMDBGFLAGS + DLGUARM 0 - DLGUARM - (105=-1,-1,-1,-1,0) + UL2CM3 + UL2CM3(-S0 -C0 -P0 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048 -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM)) - - - 0 - 1 - cfg,0x0A - - - 1 - 1 - referencespeed,0x0A - - - 2 - 1 - prop - - - - - 0 - 2 - osRtxInfo - - - - - 1 - 0 - 0x240180A0 - 0 - - 0 0 0 - 1 + 0 0 0 0 @@ -215,7 +178,7 @@ 0 0 1 - 1 + 0 0 0 0 @@ -238,7 +201,7 @@ 0 0 2 - 50000000 + 10000000 @@ -250,10 +213,10 @@ 12000000 - 1 + 0 1 - 0 - 1 + 1 + 0 0 @@ -295,14 +258,14 @@ 0 - 1 + 0 0 0 18 - 0 - 1 + 1 + 0 1 1 1 @@ -311,7 +274,7 @@ 1 1 1 - 1 + 0 1 1 1 @@ -325,7 +288,7 @@ 1 0 0 - 4 + -1 @@ -336,73 +299,9 @@ - Segger\JL2CM3.dll + - - - 0 - JL2CM3 - -U752001923 -O78 -S8 -ZTIFSpeedSel50000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(4) -TO12306 -TC400000000 -TP4 -TDS8007 -TDT0 -TDC1F -TIEFF0000FF -TIPF -TB5 -TFE1 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - UL2CM3 - UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD20000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ST-LINKIII-KEIL_SWO - -U002E00303137510A39383538 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(6BA02477) -L00(0) -TO131091 -TC400000000 -TT10000000 -TP21 -TDS8021 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ARMRTXEVENTFLAGS - -L200 -Z0 -C0 -M1 -T1 - - - 0 - DLGTARM - (1010=-1,-1,-1,-1,0)(6017=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(6016=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) - - - 0 - ARMDBGFLAGS - - - - 0 - DLGUARM - d - - - - - 0 - 1 - s - - - 1 - 1 - s_theemsrunner - - - - - 0 - 2 - osRtxInfo - - - - - 1 - 0 - 0x240180A0 - 0 - - 0 @@ -414,7 +313,7 @@ 0 0 0 - 1 + 0 0 0 0 @@ -426,8 +325,8 @@ 0 0 0 - 1 - 1 + 0 + 0 0 0 0 @@ -445,13 +344,6 @@ - - 1 - 0 - 0 - 2 - 10000000 - @@ -462,10 +354,10 @@ 12000000 - 1 + 0 1 - 0 - 1 + 1 + 0 0 @@ -507,14 +399,14 @@ 0 - 1 + 0 0 - 1 + 0 18 - 0 - 1 + 1 + 0 1 1 1 @@ -523,7 +415,7 @@ 1 1 1 - 1 + 0 1 1 1 @@ -537,7 +429,7 @@ 1 0 0 - 6 + -1 @@ -548,120 +440,21 @@ - STLink\ST-LINKIII-KEIL_SWO.dll + - - - 0 - ULP2CM3 - -UP0948193 -O206 -S12 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65555 -TC400000000 -TT10000000 -TP18 -TDX0 -TDD0 -TDS8001 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - JL2CM3 - -U752001923 -O78 -S8 -ZTIFSpeedSel50000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(4) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - UL2CM3 - UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD20000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ST-LINKIII-KEIL_SWO - -U005700373137510539383538 -O206 -SF50000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(6BA02477) -L00(0) -TO131075 -TC400000000 -TT10000000 -TP21 -TDS8021 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ARMRTXEVENTFLAGS - -L200 -Z0 -C0 -M1 -T1 - - - 0 - DLGTARM - (1010=-1,-1,-1,-1,0)(6017=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(6016=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) - - - 0 - ARMDBGFLAGS - - - - 0 - DLGUARM - (105=-1,-1,-1,-1,0) - - - - - 0 - 1 - epcfg - - - 1 - 1 - data - - - 2 - 1 - startofdata - - - 3 - 1 - _mnservice - - - 4 - 1 - s_eo_theentities - - - 5 - 1 - g - - - 6 - 1 - _config.numofjomos - - - 7 - 1 - o - - - - - 0 - 2 - osRtxInfo - - - - - 1 - 0 - 0x0809B6B8 6761 - 0 - - 0 0 0 - 1 + 0 0 0 0 0 - 1 + 0 0 0 0 @@ -673,8 +466,8 @@ 0 0 0 - 1 - 1 + 0 + 0 0 0 0 @@ -692,13 +485,6 @@ - - 1 - 0 - 0 - 2 - 50000000 - @@ -709,10 +495,10 @@ 12000000 - 1 + 0 1 - 0 - 1 + 1 + 0 0 @@ -754,14 +540,14 @@ 0 - 1 + 0 0 0 18 - 0 - 1 + 1 + 0 1 1 1 @@ -770,7 +556,7 @@ 1 1 1 - 1 + 0 1 1 1 @@ -784,7 +570,7 @@ 1 0 0 - 4 + -1 @@ -795,110 +581,21 @@ - Segger\JL2CM3.dll + - - - 0 - JL2CM3 - -U752001923 -O78 -S8 -ZTIFSpeedSel50000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(4) -TO19 -TC400000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIE80000001 -TIP9 -TB1 -TFE0 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - UL2CM3 - UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD20000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ST-LINKIII-KEIL_SWO - UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD20000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ARMRTXEVENTFLAGS - -L200 -Z19 -C0 -M1 -T1 - - - 0 - DLGTARM - (1010=-1,-1,-1,-1,0)(6017=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(6016=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) - - - 0 - ARMDBGFLAGS - - - - 0 - DLGUARM - - - - - - 0 - 1 - osthread - - - 1 - 1 - tmr - - - 2 - 1 - _internals.cfg->isr_queue,0x10 - - - 3 - 1 - errstr - - - 4 - 1 - ttt - - - 5 - 1 - priority,0x0A - - - 6 - 1 - c,0x0A - - - - - 0 - 2 - osRtxInfo - - - - - 1 - 0 - 0x20003134 - 0 - - 0 0 0 - 1 + 0 0 0 0 0 - 1 + 0 0 0 0 @@ -929,13 +626,6 @@ - - 1 - 0 - 0 - 2 - 10000000 - @@ -946,10 +636,10 @@ 12000000 - 1 + 0 1 - 0 - 1 + 1 + 0 0 @@ -991,14 +681,14 @@ 0 - 1 + 0 0 0 18 - 0 - 1 + 1 + 0 1 1 1 @@ -1007,7 +697,7 @@ 1 1 1 - 1 + 0 1 1 1 @@ -1021,7 +711,7 @@ 1 0 0 - 6 + -1 @@ -1032,98 +722,9 @@ - STLink\ST-LINKIII-KEIL_SWO.dll + - - - 0 - JL2CM3 - -U752001923 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(4) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - UL2CM3 - UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD20000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ST-LINKIII-KEIL_SWO - -U005700373137510539383538 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(6BA02477) -L00(0) -TO131089 -TC400000000 -TT10000000 -TP21 -TDS8021 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ARMRTXEVENTFLAGS - -L200 -Z19 -C0 -M1 -T1 - - - 0 - DLGTARM - (1010=-1,-1,-1,-1,0)(6017=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(6016=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) - - - 0 - ARMDBGFLAGS - - - - 0 - DLGUARM - (105=-1,-1,-1,-1,0) - - - - - 0 - 1 - osthread - - - 1 - 1 - tmr - - - 2 - 1 - _internals.cfg->isr_queue,0x10 - - - 3 - 1 - errstr - - - 4 - 1 - ttt - - - 5 - 1 - priority,0x0A - - - 6 - 1 - c,0x0A - - - - - 0 - 2 - osRtxInfo - - - - - 1 - 0 - 0x20003134 - 0 - - 0 @@ -1135,7 +736,7 @@ 0 0 0 - 1 + 0 0 0 0 @@ -1147,7 +748,7 @@ 0 0 0 - 1 + 0 0 0 0 @@ -1166,13 +767,6 @@ - - 1 - 0 - 0 - 2 - 10000000 - @@ -1183,10 +777,10 @@ 12000000 - 1 + 0 1 - 0 - 1 + 1 + 0 0 @@ -1228,14 +822,14 @@ 0 - 1 + 0 0 0 18 - 0 - 1 + 1 + 0 1 1 1 @@ -1244,7 +838,7 @@ 1 1 1 - 1 + 0 1 1 1 @@ -1258,7 +852,7 @@ 1 0 0 - 1 + -1 @@ -1269,85 +863,21 @@ - BIN\ULP2CM3.DLL + - - - 0 - ULP2CM3 - -UP1445368 -O206 -S14 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO69635 -TC400000000 -TT10000000 -TP8 -TDX0 -TDD0 -TDS8001 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - JL2CM3 - -U752001923 -O78 -S8 -ZTIFSpeedSel50000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(4) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - UL2CM3 - UL2CM3(-S0 -C0 -P0 ) -FN1 -FC8000 -FD20000000 -FF0STM32H7x_2048 -FL0200000 -FS08000000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ST-LINKIII-KEIL_SWO - -U005700373137510539383538 -O206 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(6BA02477) -L00(0) -TO131091 -TC400000000 -TT10000000 -TP21 -TDS8021 -TDT0 -TDC1F -TIE80000001 -TIP9 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H7x_2048.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H745IIKx$CMSIS\Flash\STM32H7x_2048.FLM) - - - 0 - ARMRTXEVENTFLAGS - -L200 -Z0 -C0 -M1 -T1 - - - 0 - DLGTARM - (1010=-1,-1,-1,-1,0)(6017=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(6016=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0) - - - 0 - ARMDBGFLAGS - - - - 0 - DLGUARM - - - - - - 0 - 1 - this - - - - - 0 - 2 - osRtxInfo - - - - - 1 - 0 - 0x24000dc8 - 0 - - 0 0 0 - 1 + 0 0 0 0 0 - 1 + 0 0 0 0 @@ -1359,8 +889,8 @@ 0 0 0 - 1 - 1 + 0 + 0 0 0 0 @@ -1378,19 +908,6 @@ - - - OS Support\Event Viewer - 35905 - - - - 1 - 0 - 0 - 2 - 25000000 - @@ -1416,7 +933,7 @@ embot::app::eth - 0 + 1 0 0 0 @@ -1532,7 +1049,7 @@ embot::app::eth::config - 0 + 1 0 0 0 @@ -1728,7 +1245,7 @@ embot::hw - 0 + 1 0 0 0 @@ -1763,18 +1280,6 @@ 0 0 0 - ..\..\..\..\..\embot\hw\embot_hw_led.cpp - embot_hw_led.cpp - 0 - 0 - - - 8 - 27 - 8 - 0 - 0 - 0 ..\..\..\..\..\embot\hw\embot_hw_gpio.cpp embot_hw_gpio.cpp 0 @@ -1782,19 +1287,19 @@ 8 - 28 + 27 8 0 0 0 - ..\..\..\..\..\embot\hw\embot_hw_testpoint.cpp - embot_hw_testpoint.cpp + ..\..\..\..\..\embot\hw\embot_hw_led.cpp + embot_hw_led.cpp 0 0 8 - 29 + 28 8 0 0 @@ -1806,7 +1311,7 @@ 8 - 30 + 29 8 0 0 @@ -1818,7 +1323,7 @@ 8 - 31 + 30 8 0 0 @@ -1830,7 +1335,7 @@ 8 - 32 + 31 8 0 0 @@ -1842,7 +1347,7 @@ 8 - 33 + 32 8 0 0 @@ -1854,7 +1359,7 @@ 8 - 34 + 33 8 0 0 @@ -1866,7 +1371,7 @@ 8 - 35 + 34 8 0 0 @@ -1878,7 +1383,7 @@ 8 - 36 + 35 8 0 0 @@ -1890,7 +1395,7 @@ 8 - 37 + 36 8 0 0 @@ -1902,7 +1407,7 @@ 8 - 38 + 37 8 0 0 @@ -1914,7 +1419,7 @@ 8 - 39 + 38 8 0 0 @@ -1926,7 +1431,7 @@ 8 - 40 + 39 8 0 0 @@ -1938,7 +1443,7 @@ 8 - 41 + 40 8 0 0 @@ -1948,6 +1453,18 @@ 0 0 + + 8 + 41 + 8 + 0 + 0 + 0 + ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + embot_hw_chip_MA730.cpp + 0 + 0 + 8 42 @@ -1967,8 +1484,56 @@ 0 0 0 - ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp - embot_hw_chip_MA730.cpp + ..\..\..\..\..\embot\hw\embot_hw_mtx.cpp + embot_hw_mtx.cpp + 0 + 0 + + + 8 + 44 + 8 + 0 + 0 + 0 + ..\..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + embot_hw_icc_sig.cpp + 0 + 0 + + + 8 + 45 + 8 + 0 + 0 + 0 + ..\..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + embot_hw_icc_mem.cpp + 0 + 0 + + + 8 + 46 + 8 + 0 + 0 + 0 + ..\..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + embot_hw_icc_ltr.cpp + 0 + 0 + + + 8 + 47 + 8 + 0 + 0 + 0 + ..\..\..\..\..\embot\hw\embot_hw_icc_printer.cpp + embot_hw_icc_printer.cpp 0 0 @@ -1982,7 +1547,7 @@ 0 9 - 44 + 48 8 0 0 @@ -1994,7 +1559,7 @@ 9 - 45 + 49 8 0 0 @@ -2006,7 +1571,7 @@ 9 - 46 + 50 1 0 0 @@ -2018,7 +1583,7 @@ 9 - 47 + 51 1 0 0 @@ -2030,7 +1595,7 @@ 9 - 48 + 52 1 0 0 @@ -2040,6 +1605,30 @@ 0 0 + + 9 + 53 + 8 + 0 + 0 + 0 + ..\..\..\bsp\embot_hw_mtx_bsp_amc.cpp + embot_hw_mtx_bsp_amc.cpp + 0 + 0 + + + 9 + 54 + 8 + 0 + 0 + 0 + ..\..\..\bsp\embot_hw_icc_bsp_amc.cpp + embot_hw_icc_bsp_amc.cpp + 0 + 0 + @@ -2050,7 +1639,7 @@ 0 10 - 49 + 55 8 0 0 @@ -2062,7 +1651,7 @@ 10 - 50 + 56 8 0 0 @@ -2074,7 +1663,7 @@ 10 - 51 + 57 8 0 0 @@ -2086,7 +1675,7 @@ 10 - 52 + 58 8 0 0 @@ -2098,7 +1687,7 @@ 10 - 53 + 59 8 0 0 @@ -2110,7 +1699,7 @@ 10 - 54 + 60 8 0 0 @@ -2122,7 +1711,7 @@ 10 - 55 + 61 8 0 0 @@ -2134,7 +1723,7 @@ 10 - 56 + 62 8 0 0 @@ -2154,7 +1743,7 @@ 0 11 - 57 + 63 8 0 0 @@ -2166,7 +1755,7 @@ 11 - 58 + 64 8 0 0 @@ -2178,7 +1767,7 @@ 11 - 59 + 65 8 0 0 @@ -2198,7 +1787,7 @@ 0 12 - 60 + 66 8 0 0 @@ -2210,7 +1799,7 @@ 12 - 61 + 67 8 0 0 @@ -2222,7 +1811,7 @@ 12 - 62 + 68 8 0 0 @@ -2234,7 +1823,7 @@ 12 - 63 + 69 1 0 0 @@ -2246,7 +1835,7 @@ 12 - 64 + 70 1 0 0 @@ -2258,7 +1847,7 @@ 12 - 65 + 71 1 0 0 @@ -2270,7 +1859,7 @@ 12 - 66 + 72 1 0 0 @@ -2282,7 +1871,7 @@ 12 - 67 + 73 1 0 0 @@ -2294,7 +1883,7 @@ 12 - 68 + 74 1 0 0 @@ -2306,7 +1895,7 @@ 12 - 69 + 75 1 0 0 @@ -2318,7 +1907,7 @@ 12 - 70 + 76 1 0 0 @@ -2330,7 +1919,7 @@ 12 - 71 + 77 1 0 0 @@ -2342,7 +1931,7 @@ 12 - 72 + 78 1 0 0 @@ -2354,7 +1943,7 @@ 12 - 73 + 79 1 0 0 @@ -2366,7 +1955,7 @@ 12 - 74 + 80 1 0 0 @@ -2378,7 +1967,7 @@ 12 - 75 + 81 1 0 0 @@ -2390,7 +1979,7 @@ 12 - 76 + 82 1 0 0 @@ -2402,7 +1991,7 @@ 12 - 77 + 83 1 0 0 @@ -2414,7 +2003,7 @@ 12 - 78 + 84 1 0 0 @@ -2426,7 +2015,7 @@ 12 - 79 + 85 1 0 0 @@ -2438,7 +2027,7 @@ 12 - 80 + 86 1 0 0 @@ -2450,7 +2039,7 @@ 12 - 81 + 87 1 0 0 @@ -2462,7 +2051,7 @@ 12 - 82 + 88 1 0 0 @@ -2482,7 +2071,7 @@ 0 13 - 83 + 89 8 0 0 @@ -2494,7 +2083,7 @@ 13 - 84 + 90 8 0 0 @@ -2506,7 +2095,7 @@ 13 - 85 + 91 8 0 0 @@ -2518,7 +2107,7 @@ 13 - 86 + 92 8 0 0 @@ -2538,7 +2127,7 @@ 0 14 - 87 + 93 8 0 0 @@ -2550,7 +2139,7 @@ 14 - 88 + 94 8 0 0 @@ -2562,7 +2151,7 @@ 14 - 89 + 95 1 0 0 @@ -2574,7 +2163,7 @@ 14 - 90 + 96 8 0 0 @@ -2586,7 +2175,7 @@ 14 - 91 + 97 8 0 0 @@ -2598,7 +2187,7 @@ 14 - 92 + 98 8 0 0 @@ -2610,7 +2199,7 @@ 14 - 93 + 99 8 0 0 @@ -2630,7 +2219,7 @@ 0 15 - 94 + 100 8 0 0 @@ -2650,7 +2239,7 @@ 0 16 - 95 + 101 1 0 0 @@ -2662,7 +2251,7 @@ 16 - 96 + 102 1 0 0 @@ -2682,7 +2271,7 @@ 0 17 - 97 + 103 1 0 0 @@ -2694,7 +2283,7 @@ 17 - 98 + 104 1 0 0 @@ -2706,7 +2295,7 @@ 17 - 99 + 105 1 0 0 @@ -2718,7 +2307,7 @@ 17 - 100 + 106 1 0 0 @@ -2730,7 +2319,7 @@ 17 - 101 + 107 1 0 0 @@ -2742,7 +2331,7 @@ 17 - 102 + 108 1 0 0 @@ -2754,7 +2343,7 @@ 17 - 103 + 109 1 0 0 @@ -2766,7 +2355,7 @@ 17 - 104 + 110 1 0 0 @@ -2778,7 +2367,7 @@ 17 - 105 + 111 1 0 0 @@ -2790,7 +2379,7 @@ 17 - 106 + 112 1 0 0 @@ -2802,7 +2391,7 @@ 17 - 107 + 113 1 0 0 @@ -2814,7 +2403,7 @@ 17 - 108 + 114 1 0 0 @@ -2826,7 +2415,7 @@ 17 - 109 + 115 1 0 0 @@ -2838,7 +2427,7 @@ 17 - 110 + 116 1 0 0 @@ -2850,7 +2439,7 @@ 17 - 111 + 117 1 0 0 @@ -2870,7 +2459,7 @@ 0 18 - 112 + 118 1 0 0 @@ -2882,7 +2471,7 @@ 18 - 113 + 119 1 0 0 @@ -2894,7 +2483,7 @@ 18 - 114 + 120 1 0 0 @@ -2906,7 +2495,7 @@ 18 - 115 + 121 1 0 0 @@ -2918,7 +2507,7 @@ 18 - 116 + 122 1 0 0 @@ -2930,7 +2519,7 @@ 18 - 117 + 123 1 0 0 @@ -2942,7 +2531,7 @@ 18 - 118 + 124 1 0 0 @@ -2954,7 +2543,7 @@ 18 - 119 + 125 1 0 0 @@ -2966,7 +2555,7 @@ 18 - 120 + 126 1 0 0 @@ -2978,7 +2567,7 @@ 18 - 121 + 127 1 0 0 @@ -2998,7 +2587,7 @@ 0 19 - 122 + 128 8 0 0 @@ -3010,7 +2599,7 @@ 19 - 123 + 129 8 0 0 @@ -3022,8 +2611,8 @@ 19 - 124 - 1 + 130 + 8 0 0 0 @@ -3042,7 +2631,7 @@ 0 20 - 125 + 131 8 0 0 @@ -3062,7 +2651,7 @@ 0 21 - 126 + 132 8 0 0 @@ -3082,7 +2671,7 @@ 0 22 - 127 + 133 8 0 0 @@ -3102,7 +2691,7 @@ 0 23 - 128 + 134 1 0 0 @@ -3114,7 +2703,7 @@ 23 - 129 + 135 1 0 0 @@ -3126,7 +2715,7 @@ 23 - 130 + 136 8 0 0 @@ -3146,7 +2735,7 @@ 0 24 - 131 + 137 8 0 0 @@ -3158,7 +2747,7 @@ 24 - 132 + 138 8 0 0 @@ -3170,7 +2759,7 @@ 24 - 133 + 139 1 0 0 @@ -3182,8 +2771,8 @@ 24 - 134 - 1 + 140 + 8 0 0 0 @@ -3194,7 +2783,7 @@ 24 - 135 + 141 1 0 0 @@ -3206,7 +2795,7 @@ 24 - 136 + 142 1 0 0 @@ -3218,7 +2807,7 @@ 24 - 137 + 143 8 0 0 @@ -3238,7 +2827,7 @@ 0 25 - 138 + 144 8 0 0 @@ -3250,7 +2839,7 @@ 25 - 139 + 145 8 0 0 @@ -3262,7 +2851,7 @@ 25 - 140 + 146 8 0 0 @@ -3274,7 +2863,7 @@ 25 - 141 + 147 8 0 0 @@ -3294,7 +2883,7 @@ 0 26 - 142 + 148 1 0 0 @@ -3314,7 +2903,7 @@ 0 27 - 143 + 149 1 0 0 @@ -3326,7 +2915,7 @@ 27 - 144 + 150 1 0 0 @@ -3338,7 +2927,7 @@ 27 - 145 + 151 1 0 0 @@ -3350,7 +2939,7 @@ 27 - 146 + 152 1 0 0 @@ -3370,7 +2959,7 @@ 0 28 - 147 + 153 8 0 0 @@ -3382,7 +2971,7 @@ 28 - 148 + 154 8 0 0 @@ -3394,7 +2983,7 @@ 28 - 149 + 155 8 0 0 @@ -3414,7 +3003,7 @@ 0 29 - 150 + 156 8 0 0 @@ -3434,7 +3023,7 @@ 0 30 - 151 + 157 8 0 0 @@ -3446,7 +3035,7 @@ 30 - 152 + 158 5 0 0 @@ -3466,7 +3055,7 @@ 0 31 - 153 + 159 8 0 0 @@ -3478,7 +3067,7 @@ 31 - 154 + 160 8 0 0 @@ -3490,7 +3079,7 @@ 31 - 155 + 161 8 0 0 @@ -3502,7 +3091,7 @@ 31 - 156 + 162 8 0 0 @@ -3514,7 +3103,7 @@ 31 - 157 + 163 8 0 0 @@ -3534,7 +3123,7 @@ 0 32 - 158 + 164 8 0 0 @@ -3546,7 +3135,7 @@ 32 - 159 + 165 8 0 0 @@ -3566,7 +3155,7 @@ 0 33 - 160 + 166 8 0 0 @@ -3578,7 +3167,7 @@ 33 - 161 + 167 8 0 0 @@ -3590,7 +3179,7 @@ 33 - 162 + 168 8 0 0 @@ -3602,7 +3191,7 @@ 33 - 163 + 169 8 0 0 @@ -3614,7 +3203,7 @@ 33 - 164 + 170 1 0 0 @@ -3634,7 +3223,7 @@ 0 34 - 165 + 171 8 0 0 @@ -3646,7 +3235,7 @@ 34 - 166 + 172 8 0 0 @@ -3658,7 +3247,7 @@ 34 - 167 + 173 8 0 0 @@ -3670,7 +3259,7 @@ 34 - 168 + 174 8 0 0 @@ -3682,7 +3271,7 @@ 34 - 169 + 175 8 0 0 @@ -3694,7 +3283,7 @@ 34 - 170 + 176 8 0 0 @@ -3706,7 +3295,7 @@ 34 - 171 + 177 8 0 0 @@ -3718,7 +3307,7 @@ 34 - 172 + 178 8 0 0 @@ -3730,7 +3319,7 @@ 34 - 173 + 179 8 0 0 @@ -3740,6 +3329,18 @@ 0 0 + + 34 + 180 + 8 + 0 + 0 + 0 + ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp + wrist_decoupler.cpp + 0 + 0 + @@ -3750,7 +3351,7 @@ 0 35 - 174 + 181 8 0 0 @@ -3762,7 +3363,7 @@ 35 - 175 + 182 8 0 0 diff --git a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01.uvprojx b/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01.uvprojx index 6b391bc836..1e20eca387 100644 --- a/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01.uvprojx +++ b/emBODY/eBcode/arch-arm/board/amc/examples/appl-01/proj/appl-01.uvprojx @@ -10,13 +10,13 @@ amc-appl-01-osal-stlink 0x4 ARM-ADS - 6180000::V6.18::ARMCLANG + 6190000::V6.19::ARMCLANG 1 STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -49,7 +49,7 @@ 1 .\obj\ - h7eth + amc 1 0 1 @@ -80,9 +80,9 @@ 0 - 0 + 1 0 - + cmd.exe /C copy .\obj\amc.hex ..\bin\amc.hex 0 0 @@ -338,7 +338,7 @@ 0 -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal -DIPAL_use_cfg2 -DUSE_EMBOT_runner -DUSE_EMBOT_theHandler -DUSE_EMBOT_theServices -DEOTHESERVICES_disable_theMotionController -DUSE_EOtheMCamc -DUSE_EMBOT_theServicesMC - USE_EMBOT_HW EMBOBJ_USE_EMBOT USE_STM32HAL STM32HAL_BOARD_AMC STM32HAL_DRIVER_V1A0 + USE_EMBOT_HW EMBOBJ_USE_EMBOT USE_STM32HAL STM32HAL_BOARD_AMC STM32HAL_DRIVER_V1A0 WRIST_MK2 ..\..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\embot\hw;..\..\..\..\..\embot\os;..\..\..\..\..\embot\app;..\..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\..\libs\highlevel\abslayer\ipal\api;--..\..\..\..\..\libs\highlevel\abslayer\hal2\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\core\core;..\..\..\..\..\embobj\core\exec\multitask;..\..\..\..\..\embobj\plus\ipnet;..\src\emb-env;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\prot\eth;..\..\..\bsp;..\cfg;..\..\..\bsp\ethdriver;..\..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\..\embobj\plus\embenv;..\..\..\..\..\libs\highlevel\abslayer\hal2\api;..\..\..\..\..\embobj\plus\ctrloop;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\icub;..\..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\embot\app\eth;.;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\transport;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\protocol\api;..\cfg\protocol;..\..\..\..\..\embobj\plus\can;..\..\..\..\ems004\appl\v2\src\eoappservices;..\..\..\..\..\embobj\plus\board;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\..\..\..\mbd\kalman_filter;..\..\..\..\..\embobj\plus\mc;..\..\..\..\..\libs\midware\hl-plus\api @@ -582,76 +582,20 @@ 8 ..\..\..\..\..\embot\hw\embot_hw.cpp - - embot_hw_led.cpp - 8 - ..\..\..\..\..\embot\hw\embot_hw_led.cpp - embot_hw_gpio.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_gpio.cpp - embot_hw_testpoint.cpp + embot_hw_led.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_testpoint.cpp + ..\..\..\..\..\embot\hw\embot_hw_led.cpp embot_hw_lowlevel.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_lowlevel.cpp - - - 2 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 1 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 0 - 0 - 2 - 2 - 2 - 2 - 2 - - - - - - - - - embot_hw_sys.cpp @@ -672,57 +616,6 @@ embot_hw_flash.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_flash.cpp - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 2 - 0 - 2 - 2 - 2 - 2 - 2 - 0 - 0 - 2 - 2 - 2 - 2 - 2 - - - - - - - - - embot_hw_spi.cpp @@ -815,15 +708,40 @@ 8 ..\..\..\..\..\embot\hw\embot_hw_chip_AS5045.cpp + + embot_hw_chip_MA730.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + embot_hw_chip_MB049.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_chip_MB049.cpp - embot_hw_chip_MA730.cpp + embot_hw_mtx.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + ..\..\..\..\..\embot\hw\embot_hw_mtx.cpp + + + embot_hw_icc_sig.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + + + embot_hw_icc_mem.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + + + embot_hw_icc_ltr.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + + + embot_hw_icc_printer.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_printer.cpp @@ -855,6 +773,16 @@ 1 ..\..\..\bsp\ethdriver\ipal_hal_eth_stm32h7.c + + embot_hw_mtx_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_mtx_bsp_amc.cpp + + + embot_hw_icc_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_icc_bsp_amc.cpp + @@ -1588,7 +1516,7 @@ EoProtocolMC_fun_amc.c - 1 + 8 ..\cfg\protocol\EoProtocolMC_fun_amc.c @@ -1816,7 +1744,7 @@ EoCANprotMCperiodic_amc.c - 1 + 8 ..\cfg\protocol\can\EoCANprotMCperiodic_amc.c @@ -2574,6 +2502,11 @@ 8 ..\..\..\..\..\embobj\plus\mc\WatchDog.c + + wrist_decoupler.cpp + 8 + ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp + @@ -2603,7 +2536,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -3150,20 +3083,15 @@ 8 ..\..\..\..\..\embot\hw\embot_hw.cpp - - embot_hw_led.cpp - 8 - ..\..\..\..\..\embot\hw\embot_hw_led.cpp - embot_hw_gpio.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_gpio.cpp - embot_hw_testpoint.cpp + embot_hw_led.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_testpoint.cpp + ..\..\..\..\..\embot\hw\embot_hw_led.cpp embot_hw_lowlevel.cpp @@ -3383,15 +3311,40 @@ 8 ..\..\..\..\..\embot\hw\embot_hw_chip_AS5045.cpp + + embot_hw_chip_MA730.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + embot_hw_chip_MB049.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_chip_MB049.cpp - embot_hw_chip_MA730.cpp + embot_hw_mtx.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + ..\..\..\..\..\embot\hw\embot_hw_mtx.cpp + + + embot_hw_icc_sig.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + + + embot_hw_icc_mem.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + + + embot_hw_icc_ltr.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + + + embot_hw_icc_printer.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_printer.cpp @@ -3423,6 +3376,16 @@ 1 ..\..\..\bsp\ethdriver\ipal_hal_eth_stm32h7.c + + embot_hw_mtx_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_mtx_bsp_amc.cpp + + + embot_hw_icc_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_icc_bsp_amc.cpp + @@ -4156,7 +4119,7 @@ EoProtocolMC_fun_amc.c - 1 + 8 ..\cfg\protocol\EoProtocolMC_fun_amc.c @@ -4333,7 +4296,7 @@ EoCANprotMCperiodic_amc.c - 1 + 8 ..\cfg\protocol\can\EoCANprotMCperiodic_amc.c @@ -4887,6 +4850,11 @@ 8 ..\..\..\..\..\embobj\plus\mc\WatchDog.c + + wrist_decoupler.cpp + 8 + ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp + @@ -4916,7 +4884,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -5214,7 +5182,7 @@ 1 - 6 + 2 0 0 1 @@ -5238,7 +5206,7 @@ 0 -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal -DIPAL_use_cfg2 -DUSE_EMBOT_runner -DUSE_EMBOT_theHandler -DUSE_EMBOT_theServices -DEOTHESERVICES_disable_theMotionController -DUSE_EOtheMCamc -DUSE_EMBOT_theServicesMC - USE_EMBOT_HW EMBOBJ_USE_EMBOT USE_STM32HAL STM32HAL_BOARD_AMC STM32HAL_DRIVER_V1A0 + USE_EMBOT_HW EMBOBJ_USE_EMBOT USE_STM32HAL STM32HAL_BOARD_AMC STM32HAL_DRIVER_V1A0 WRIST_MK2 ..\..\..\..\..\libs\lowlevel\stm32hal\api;..\..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\embot\hw;..\..\..\..\..\embot\os;..\..\..\..\..\embot\app;..\..\..\..\..\libs\midware\eventviewer\api;..\..\..\..\..\libs\highlevel\abslayer\ipal\api;--..\..\..\..\..\libs\highlevel\abslayer\hal2\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\core\core;..\..\..\..\..\embobj\core\exec\multitask;..\..\..\..\..\embobj\plus\ipnet;..\src\emb-env;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\prot\eth;..\..\..\bsp;..\cfg;..\..\..\bsp\ethdriver;..\..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\..\embobj\plus\embenv;..\..\..\..\..\libs\highlevel\abslayer\hal2\api;..\..\..\..\..\embobj\plus\ctrloop;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\icub;..\..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\..\embot\app\eth;.;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\transport;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\protocol\api;..\cfg\protocol;..\..\..\..\..\embobj\plus\can;..\..\..\..\ems004\appl\v2\src\eoappservices;..\..\..\..\..\embobj\plus\board;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\..\..\..\mbd\kalman_filter;..\..\..\..\..\embobj\plus\mc;..\..\..\..\..\libs\midware\hl-plus\api @@ -5402,25 +5370,6 @@ osal.cm4.dbg.lib 4 ..\..\..\..\..\libs\highlevel\abslayer\osal\lib\osal.cm4.dbg.lib - - - 2 - 0 - 0 - 0 - 0 - 0 - 2 - 2 - 2 - 2 - 11 - - - 1 - - - eventviewer.c @@ -5482,20 +5431,15 @@ 8 ..\..\..\..\..\embot\hw\embot_hw.cpp - - embot_hw_led.cpp - 8 - ..\..\..\..\..\embot\hw\embot_hw_led.cpp - embot_hw_gpio.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_gpio.cpp - embot_hw_testpoint.cpp + embot_hw_led.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_testpoint.cpp + ..\..\..\..\..\embot\hw\embot_hw_led.cpp embot_hw_lowlevel.cpp @@ -5579,7 +5523,7 @@ 0 0 0 - 1 + 0 2 2 2 @@ -5715,15 +5659,40 @@ 8 ..\..\..\..\..\embot\hw\embot_hw_chip_AS5045.cpp + + embot_hw_chip_MA730.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + embot_hw_chip_MB049.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_chip_MB049.cpp - embot_hw_chip_MA730.cpp + embot_hw_mtx.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + ..\..\..\..\..\embot\hw\embot_hw_mtx.cpp + + + embot_hw_icc_sig.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + + + embot_hw_icc_mem.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + + + embot_hw_icc_ltr.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + + + embot_hw_icc_printer.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_printer.cpp @@ -5755,6 +5724,16 @@ 1 ..\..\..\bsp\ethdriver\ipal_hal_eth_stm32h7.c + + embot_hw_mtx_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_mtx_bsp_amc.cpp + + + embot_hw_icc_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_icc_bsp_amc.cpp + @@ -6692,7 +6671,7 @@ EoProtocolMC_fun_amc.c - 1 + 8 ..\cfg\protocol\EoProtocolMC_fun_amc.c @@ -6869,7 +6848,7 @@ EoCANprotMCperiodic_amc.c - 1 + 8 ..\cfg\protocol\can\EoCANprotMCperiodic_amc.c @@ -7729,6 +7708,11 @@ 8 ..\..\..\..\..\embobj\plus\mc\WatchDog.c + + wrist_decoupler.cpp + 8 + ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp + @@ -7758,7 +7742,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -8356,20 +8340,15 @@ 8 ..\..\..\..\..\embot\hw\embot_hw.cpp - - embot_hw_led.cpp - 8 - ..\..\..\..\..\embot\hw\embot_hw_led.cpp - embot_hw_gpio.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_gpio.cpp - embot_hw_testpoint.cpp + embot_hw_led.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_testpoint.cpp + ..\..\..\..\..\embot\hw\embot_hw_led.cpp embot_hw_lowlevel.cpp @@ -8538,15 +8517,40 @@ 8 ..\..\..\..\..\embot\hw\embot_hw_chip_AS5045.cpp + + embot_hw_chip_MA730.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + embot_hw_chip_MB049.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_chip_MB049.cpp - embot_hw_chip_MA730.cpp + embot_hw_mtx.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + ..\..\..\..\..\embot\hw\embot_hw_mtx.cpp + + + embot_hw_icc_sig.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + + + embot_hw_icc_mem.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + + + embot_hw_icc_ltr.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + + + embot_hw_icc_printer.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_printer.cpp @@ -8578,6 +8582,16 @@ 1 ..\..\..\bsp\ethdriver\ipal_hal_eth_stm32h7.c + + embot_hw_mtx_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_mtx_bsp_amc.cpp + + + embot_hw_icc_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_icc_bsp_amc.cpp + @@ -9005,7 +9019,7 @@ EoProtocolMC_fun_amc.c - 1 + 8 ..\cfg\protocol\EoProtocolMC_fun_amc.c @@ -9080,7 +9094,7 @@ EoCANprotMCperiodic_amc.c - 1 + 8 ..\cfg\protocol\can\EoCANprotMCperiodic_amc.c @@ -9328,6 +9342,11 @@ 8 ..\..\..\..\..\embobj\plus\mc\WatchDog.c + + wrist_decoupler.cpp + 8 + ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp + @@ -9357,7 +9376,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -9955,20 +9974,15 @@ 8 ..\..\..\..\..\embot\hw\embot_hw.cpp - - embot_hw_led.cpp - 8 - ..\..\..\..\..\embot\hw\embot_hw_led.cpp - embot_hw_gpio.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_gpio.cpp - embot_hw_testpoint.cpp + embot_hw_led.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_testpoint.cpp + ..\..\..\..\..\embot\hw\embot_hw_led.cpp embot_hw_lowlevel.cpp @@ -10137,15 +10151,40 @@ 8 ..\..\..\..\..\embot\hw\embot_hw_chip_AS5045.cpp + + embot_hw_chip_MA730.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + embot_hw_chip_MB049.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_chip_MB049.cpp - embot_hw_chip_MA730.cpp + embot_hw_mtx.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + ..\..\..\..\..\embot\hw\embot_hw_mtx.cpp + + + embot_hw_icc_sig.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + + + embot_hw_icc_mem.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + + + embot_hw_icc_ltr.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + + + embot_hw_icc_printer.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_printer.cpp @@ -10177,6 +10216,16 @@ 1 ..\..\..\bsp\ethdriver\ipal_hal_eth_stm32h7.c + + embot_hw_mtx_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_mtx_bsp_amc.cpp + + + embot_hw_icc_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_icc_bsp_amc.cpp + @@ -10604,7 +10653,7 @@ EoProtocolMC_fun_amc.c - 1 + 8 ..\cfg\protocol\EoProtocolMC_fun_amc.c @@ -10679,7 +10728,7 @@ EoCANprotMCperiodic_amc.c - 1 + 8 ..\cfg\protocol\can\EoCANprotMCperiodic_amc.c @@ -10927,6 +10976,11 @@ 8 ..\..\..\..\..\embobj\plus\mc\WatchDog.c + + wrist_decoupler.cpp + 8 + ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp + @@ -10956,7 +11010,7 @@ STM32H745IIKx:CM7 STMicroelectronics - Keil.STM32H7xx_DFP.3.1.0 + Keil.STM32H7xx_DFP.3.1.1 https://www.keil.com/pack/ IRAM(0x38000000,0x00010000) IRAM2(0x24000000,0x00080000) IROM(0x08000000,0x00100000) XRAM(0x20000000,0x00020000) CPUTYPE("Cortex-M7") FPU3(DFPU) CLOCK(12000000) ELITTLE @@ -11503,20 +11557,15 @@ 8 ..\..\..\..\..\embot\hw\embot_hw.cpp - - embot_hw_led.cpp - 8 - ..\..\..\..\..\embot\hw\embot_hw_led.cpp - embot_hw_gpio.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_gpio.cpp - embot_hw_testpoint.cpp + embot_hw_led.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_testpoint.cpp + ..\..\..\..\..\embot\hw\embot_hw_led.cpp embot_hw_lowlevel.cpp @@ -11736,15 +11785,40 @@ 8 ..\..\..\..\..\embot\hw\embot_hw_chip_AS5045.cpp + + embot_hw_chip_MA730.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + embot_hw_chip_MB049.cpp 8 ..\..\..\..\..\embot\hw\embot_hw_chip_MB049.cpp - embot_hw_chip_MA730.cpp + embot_hw_mtx.cpp 8 - ..\..\..\..\..\embot\hw\embot_hw_chip_MA730.cpp + ..\..\..\..\..\embot\hw\embot_hw_mtx.cpp + + + embot_hw_icc_sig.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_sig.cpp + + + embot_hw_icc_mem.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_mem.cpp + + + embot_hw_icc_ltr.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_ltr.cpp + + + embot_hw_icc_printer.cpp + 8 + ..\..\..\..\..\embot\hw\embot_hw_icc_printer.cpp @@ -11776,6 +11850,16 @@ 1 ..\..\..\bsp\ethdriver\ipal_hal_eth_stm32h7.c + + embot_hw_mtx_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_mtx_bsp_amc.cpp + + + embot_hw_icc_bsp_amc.cpp + 8 + ..\..\..\bsp\embot_hw_icc_bsp_amc.cpp + @@ -12560,7 +12644,7 @@ EoProtocolMC_fun_amc.c - 1 + 8 ..\cfg\protocol\EoProtocolMC_fun_amc.c @@ -12635,7 +12719,7 @@ EoCANprotMCperiodic_amc.c - 1 + 8 ..\cfg\protocol\can\EoCANprotMCperiodic_amc.c @@ -12883,6 +12967,11 @@ 8 ..\..\..\..\..\embobj\plus\mc\WatchDog.c + + wrist_decoupler.cpp + 8 + ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp + diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index a1282ed72e..47cc8ee1c3 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -81,7 +81,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 75 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 76 // version @@ -89,13 +89,13 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 9 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 14 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 10 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 21 // minute <0-59> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 21 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 00 // build date // Info diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvoptx b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvoptx index 5760664336..11d93af3e3 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvoptx +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvoptx @@ -3010,7 +3010,7 @@ eo-emsappl-cfg - 0 + 1 0 0 0 diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvprojx b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvprojx index fb758a78cb..152bb6585d 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvprojx +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvprojx @@ -1417,8 +1417,8 @@ STM32F407IG STMicroelectronics - Keil.STM32F4xx_DFP.2.16.0 - http://www.keil.com/pack/ + Keil.STM32F4xx_DFP.2.17.0 + https://www.keil.com/pack/ IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) CPUTYPE("Cortex-M4") FPU2 "Startup\ST\STM32F4xx\startup_stm32f4xx.s" ("STM32F4xx Startup Code") @@ -2909,8 +2909,8 @@ STM32F407IG STMicroelectronics - Keil.STM32F4xx_DFP.2.16.0 - http://www.keil.com/pack/ + Keil.STM32F4xx_DFP.2.17.0 + https://www.keil.com/pack/ IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) CPUTYPE("Cortex-M4") FPU2 "Startup\ST\STM32F4xx\startup_stm32f4xx.s" ("STM32F4xx Startup Code") @@ -4503,8 +4503,8 @@ STM32F407IG STMicroelectronics - Keil.STM32F4xx_DFP.2.16.0 - http://www.keil.com/pack/ + Keil.STM32F4xx_DFP.2.17.0 + https://www.keil.com/pack/ IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) CPUTYPE("Cortex-M4") FPU2 "Startup\ST\STM32F4xx\startup_stm32f4xx.s" ("STM32F4xx Startup Code") diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.wristmk2.uvoptx b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.wristmk2.uvoptx index 17728d4104..cf3622507d 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.wristmk2.uvoptx +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.wristmk2.uvoptx @@ -1631,7 +1631,7 @@ 14 99 - 1 + 8 0 0 0 @@ -1679,7 +1679,7 @@ 14 103 - 1 + 8 0 0 0 @@ -1691,7 +1691,7 @@ 14 104 - 1 + 8 0 0 0 @@ -1703,7 +1703,7 @@ 14 105 - 1 + 8 0 0 0 @@ -1715,7 +1715,7 @@ 14 106 - 1 + 8 0 0 0 @@ -1739,18 +1739,6 @@ 14 108 - 8 - 0 - 0 - 0 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - wrist_decoupler_data.cpp - 0 - 0 - - - 14 - 109 5 0 0 @@ -1770,7 +1758,7 @@ 0 15 - 110 + 109 1 0 0 @@ -1782,7 +1770,7 @@ 15 - 111 + 110 1 0 0 @@ -1794,7 +1782,7 @@ 15 - 112 + 111 1 0 0 @@ -1806,7 +1794,7 @@ 15 - 113 + 112 1 0 0 @@ -1818,7 +1806,7 @@ 15 - 114 + 113 1 0 0 @@ -1830,7 +1818,7 @@ 15 - 115 + 114 1 0 0 @@ -1842,7 +1830,7 @@ 15 - 116 + 115 1 0 0 @@ -1854,7 +1842,7 @@ 15 - 117 + 116 1 0 0 @@ -1866,7 +1854,7 @@ 15 - 118 + 117 1 0 0 @@ -1878,7 +1866,7 @@ 15 - 119 + 118 1 0 0 @@ -1890,7 +1878,7 @@ 15 - 120 + 119 1 0 0 @@ -1902,7 +1890,7 @@ 15 - 121 + 120 1 0 0 @@ -1914,7 +1902,7 @@ 15 - 122 + 121 1 0 0 @@ -1926,7 +1914,7 @@ 15 - 123 + 122 1 0 0 @@ -1938,7 +1926,7 @@ 15 - 124 + 123 1 0 0 @@ -1958,7 +1946,7 @@ 0 16 - 125 + 124 1 0 0 @@ -1970,7 +1958,7 @@ 16 - 126 + 125 1 0 0 @@ -1982,7 +1970,7 @@ 16 - 127 + 126 1 0 0 @@ -1994,7 +1982,7 @@ 16 - 128 + 127 1 0 0 @@ -2006,7 +1994,7 @@ 16 - 129 + 128 1 0 0 @@ -2018,7 +2006,7 @@ 16 - 130 + 129 1 0 0 @@ -2030,7 +2018,7 @@ 16 - 131 + 130 1 0 0 @@ -2042,7 +2030,7 @@ 16 - 132 + 131 1 0 0 @@ -2054,7 +2042,7 @@ 16 - 133 + 132 1 0 0 @@ -2066,7 +2054,7 @@ 16 - 134 + 133 1 0 0 @@ -2078,7 +2066,7 @@ 16 - 135 + 134 1 0 0 @@ -2090,7 +2078,7 @@ 16 - 136 + 135 1 0 0 @@ -2102,7 +2090,7 @@ 16 - 137 + 136 1 0 0 @@ -2114,7 +2102,7 @@ 16 - 138 + 137 1 0 0 @@ -2126,7 +2114,7 @@ 16 - 139 + 138 1 0 0 @@ -2138,7 +2126,7 @@ 16 - 140 + 139 1 0 0 @@ -2158,7 +2146,7 @@ 0 17 - 141 + 140 1 0 0 @@ -2170,7 +2158,7 @@ 17 - 142 + 141 1 0 0 @@ -2182,7 +2170,7 @@ 17 - 143 + 142 1 0 0 @@ -2194,7 +2182,7 @@ 17 - 144 + 143 8 0 0 @@ -2214,7 +2202,7 @@ 0 18 - 145 + 144 1 0 0 @@ -2226,7 +2214,7 @@ 18 - 146 + 145 1 0 0 @@ -2238,7 +2226,7 @@ 18 - 147 + 146 1 0 0 @@ -2258,7 +2246,7 @@ 0 19 - 148 + 147 8 0 0 @@ -2270,7 +2258,7 @@ 19 - 149 + 148 8 0 0 @@ -2282,7 +2270,7 @@ 19 - 150 + 149 8 0 0 @@ -2294,7 +2282,7 @@ 19 - 151 + 150 8 0 0 @@ -2306,7 +2294,7 @@ 19 - 152 + 151 1 0 0 @@ -2318,7 +2306,7 @@ 19 - 153 + 152 1 0 0 @@ -2330,7 +2318,7 @@ 19 - 154 + 153 1 0 0 @@ -2350,7 +2338,7 @@ 0 20 - 155 + 154 1 0 0 @@ -2362,7 +2350,7 @@ 20 - 156 + 155 1 0 0 @@ -2382,7 +2370,7 @@ 0 21 - 157 + 156 8 0 0 @@ -2394,7 +2382,7 @@ 21 - 158 + 157 8 0 0 @@ -2406,7 +2394,7 @@ 21 - 159 + 158 8 0 0 @@ -2418,7 +2406,7 @@ 21 - 160 + 159 8 0 0 @@ -2438,7 +2426,7 @@ 0 22 - 161 + 160 8 0 0 @@ -2450,7 +2438,7 @@ 22 - 162 + 161 8 0 0 @@ -2462,7 +2450,7 @@ 22 - 163 + 162 8 0 0 @@ -2474,7 +2462,7 @@ 22 - 164 + 163 8 0 0 @@ -2486,7 +2474,7 @@ 22 - 165 + 164 8 0 0 @@ -2506,7 +2494,7 @@ 0 23 - 166 + 165 8 0 0 @@ -2518,7 +2506,7 @@ 23 - 167 + 166 8 0 0 diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.wristmk2.uvprojx b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.wristmk2.uvprojx index 0e69b66d50..2eaeb6bc33 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.wristmk2.uvprojx +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.wristmk2.uvprojx @@ -10,13 +10,13 @@ ems4rd 0x4 ARM-ADS - 6180000::V6.18::ARMCLANG + 6190000::V6.19::ARMCLANG 1 STM32F407IG STMicroelectronics - Keil.STM32F4xx_DFP.2.16.0 + Keil.STM32F4xx_DFP.2.15.0 http://www.keil.com/pack/ IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) CPUTYPE("Cortex-M4") FPU2 @@ -1010,7 +1010,7 @@ AbsEncoder.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\AbsEncoder.c @@ -1030,22 +1030,22 @@ Motor.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\Motor.c Pid.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\Pid.c Trajectory.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\Trajectory.c WatchDog.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\WatchDog.c @@ -1053,11 +1053,6 @@ 8 ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp - - wrist_decoupler_data.cpp - 8 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - Joint_hid.h 5 @@ -2406,7 +2401,7 @@ AbsEncoder.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\AbsEncoder.c @@ -2426,22 +2421,22 @@ Motor.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\Motor.c Pid.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\Pid.c Trajectory.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\Trajectory.c WatchDog.c - 1 + 8 ..\..\..\..\..\embobj\plus\mc\WatchDog.c @@ -2449,11 +2444,6 @@ 8 ..\..\..\..\..\embobj\plus\mc\wrist_decoupler.cpp - - wrist_decoupler_data.cpp - 8 - ..\..\..\..\..\embobj\plus\mc\wrist_decoupler_data.cpp - Joint_hid.h 5 diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheServices_hid.h b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheServices_hid.h index 0aebddb831..b5cddf80bb 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheServices_hid.h +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/src/eoappservices/EOtheServices_hid.h @@ -61,16 +61,18 @@ extern "C" { // so far, i write them in here. later on we think of a board file or else... #if 1 -#ifdef WRIST_MK2 -#define EOTHESERVICES_disable_theInertials2 -#define EOTHESERVICES_disable_theInertials3 -#define EOTHESERVICES_disable_theTemperatures -#endif +//#ifdef WRIST_MK2 +//#define EOTHESERVICES_disable_theInertials2 +//#define EOTHESERVICES_disable_theInertials3 +//#define EOTHESERVICES_disable_theTemperatures +//#endif + + //#define EOTHESERVICES_disable_theMAIS //#define EOTHESERVICES_disable_theSTRAIN -#ifdef WRIST_MK2 -#define EOTHESERVICES_disable_theSKIN -#endif +//#ifdef WRIST_MK2 +//#define EOTHESERVICES_disable_theSKIN +//#endif //#define EOTHESERVICES_disable_thePSC ////#define EOTHESERVICES_disable_thePOS //#define EOTHESERVICES_disable_theMC4boards diff --git a/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index 1381f63d36..a4a647abe5 100644 --- a/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/mc2plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -75,7 +75,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR 3 // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 57 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 58 // version @@ -83,11 +83,11 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 9 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 14 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 10 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 21 // minute <0-59> #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 23 // build date diff --git a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h index 7438a7af9c..a1d9966ffd 100644 --- a/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h +++ b/emBODY/eBcode/arch-arm/board/mc4plus/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h @@ -84,7 +84,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 78 +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 79 // version @@ -92,11 +92,11 @@ extern "C" { // year <2010-2030> #define EOMTHEEMSAPPLCFG_BUILDDATE_YEAR 2023 // month <1-12> -#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 9 +#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 10 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 14 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 10 // hour <0-23> -#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 11 +#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 21 // minute <0-59> #define EOMTHEEMSAPPLCFG_BUILDDATE_MIN 24 diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.h index ef5bdcd5f3..99948e98a8 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.h @@ -19,16 +19,16 @@ #ifndef MC_CALIBRATORS_H___ #define MC_CALIBRATORS_H___ -#ifdef WRIST_MK2 +//#ifdef WRIST_MK2 #include "JointSet.h" -#endif +//#endif #include "EOemsControllerCfg.h" -#ifndef WRIST_MK2 -#include "JointSet.h" -#endif +//#ifndef WRIST_MK2 +//#include "JointSet.h" +//#endif extern BOOL JointSet_do_wait_calibration_3(JointSet* o); extern BOOL JointSet_do_wait_calibration_5(JointSet* o); diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c index 2ffaafb8fe..0a9af82e0e 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c @@ -41,17 +41,17 @@ #include "hal_trace.h" #include "stdio.h" -#ifdef WRIST_MK2 +//#ifdef WRIST_MK2 #include "JointSet.h" -#endif +//#endif #include "Joint.h" #include "Motor.h" #include "AbsEncoder.h" #include "Pid.h" -#ifndef WRIST_MK2 -#include "JointSet.h" -#endif +//#ifndef WRIST_MK2 +//#include "JointSet.h" +//#endif // - OPAQUE STRUCT @@ -513,6 +513,52 @@ static void update_jointAndMotor_withJointset_constraints(void) o->joint[j].not_reversible = TRUE; } } + #ifdef WRIST_MK2 + if(o->jointSet[s].special_constraint == eomc_jsetconstraint_ergocubwrist) + { + JointSet_init_wrist_decoupler(&(o->jointSet[s])); + //init the joint of the set + /*Implementation note: I connot move the belong2WristMK2 init in the JointSet_init_wrist_decoupler function, + because the jointset_config functionis still not invoked and the joint_set structure has all pointer to null. */ + for(int i=0; iset_dim[s]; i++) + { + int j = o->jos[s][i]; + o->joint[j].belong2WristMK2 = TRUE; + } + } + #endif + +//#ifdef WRIST_MK2 moved to JointSet_init_wrist_decoupler function +// if(o->jointSet[s].special_constraint == eomc_jsetconstraint_ergocubwrist) +// { +// JointSet *jset_ptr = &(o->jointSet[s]); +// if(jset_ptr->is_right_wrist) +// jset_ptr->wristDecoupler.rtU.RL = true; +// else +// jset_ptr->wristDecoupler.rtU.RL = false; +// +// if(jset_ptr->mk_version == WRIST_MK_VER_2_1) +// { +// jset_ptr->wristDecoupler.rtU.plat_off[0] = 60.0f; +// jset_ptr->wristDecoupler.rtU.plat_off[1] = 180.0f; +// jset_ptr->wristDecoupler.rtU.plat_off[2] = 300.0f; + +// jset_ptr->wristDecoupler.rtU.theta_off[0] = 60.0f + 60.0f; +// jset_ptr->wristDecoupler.rtU.theta_off[1] = 180.0f + 60.0f; +// jset_ptr->wristDecoupler.rtU.theta_off[2] = 300.0f + 60.0f; +// } +// else +// { +// jset_ptr->wristDecoupler.rtU.plat_off[0] = 85.0f; +// jset_ptr->wristDecoupler.rtU.plat_off[1] = 185.0f; +// jset_ptr->wristDecoupler.rtU.plat_off[2] = 280.0f; + +// jset_ptr->wristDecoupler.rtU.theta_off[0] = 85.0f + 60.0f; +// jset_ptr->wristDecoupler.rtU.theta_off[1] = 185.0f + 85.0f; +// jset_ptr->wristDecoupler.rtU.theta_off[2] = 280.0f + 120.0f; +// } +// } +//#endif } } @@ -560,9 +606,13 @@ static void get_jomo_coupling_info(const eOmc_4jomo_coupling_t *jomoCouplingInfo o->jointSet[s].veldir_ctrl_out_type = jsetcfg[s].pid_output_types.veldir_ctrl_out_type; o->jointSet[s].USE_SPEED_FBK_FROM_MOTORS = jsetcfg[s].usespeedfeedbackfrommotors; - o->jointSet[s].special_constraint = (eOmc_jsetconstraint_t)jsetcfg[s].constraints.type; - o->jointSet[s].special_limit = jsetcfg[s].constraints.param1; - //NOTE: jsetcfg[s].constraints.param2 is not used currently. It is reserved for future use + + JointSet_set_constraints(&(o->jointSet[s]), &(jsetcfg[s].constraints)); + + //moved in JointSet_set_constraints(JointSet* o, eOmc_jointSet_constraints_t *constraints) +// o->jointSet[s].special_constraint = (eOmc_jsetconstraint_t)jsetcfg[s].constraints.type; +// o->jointSet[s].special_limit = jsetcfg[s].constraints.param1; +// //NOTE: jsetcfg[s].constraints.param2 is not used currently. It is reserved for future use #ifdef R1_HAND_MKII @@ -1973,17 +2023,21 @@ void MController_go_idle(void) void MController_get_joint_state(int j, eOmc_joint_status_t* joint_state) { -// static uint32_t count =0; -// -// count++; - - #ifndef WRIST_MK2 + Joint *j_ptr= smc->joint+j; - - Joint_get_state(j_ptr, joint_state); - #else - JointSet_get_state(&(smc->jointSet[0]), j, joint_state); - #endif +#ifdef WRIST_MK2 + if((!j_ptr->belong2WristMK2) || ((j_ptr->belong2WristMK2) && (j_ptr->control_mode == eomc_controlmode_notConfigured))) + { + Joint_get_state(j_ptr, joint_state); + } + else + { + JointSet_get_state(&(smc->jointSet[0]), j, joint_state); + } + +#else + Joint_get_state(j_ptr, joint_state); +#endif AbsEncoder* enc_ptr = smc->absEncoder + j*smc->multi_encs; @@ -1993,8 +2047,7 @@ void MController_get_joint_state(int j, eOmc_joint_status_t* joint_state) //joint_state->addinfo.multienc[k] = count; } -// if(count>10000) -// count = 0; + } @@ -2057,10 +2110,15 @@ void MController_config_joint_vel_ref_timeout(int j, int32_t timeout_ms) BOOL MController_set_joint_pos_ref(int j, CTRL_UNITS pos_ref, CTRL_UNITS vel_ref) { -#if !defined(WRIST_MK2) - return Joint_set_pos_ref(smc->joint+j, pos_ref, vel_ref); + Joint *j_ptr= smc->joint+j; + +#ifdef WRIST_MK2 + if(!j_ptr->belong2WristMK2) + return Joint_set_pos_ref(j_ptr, pos_ref, vel_ref); + else + return JointSet_set_pos_ref(&(smc->jointSet[0]), j, pos_ref, vel_ref); #else - return JointSet_set_pos_ref(&(smc->jointSet[0]), j, pos_ref, vel_ref); + return Joint_set_pos_ref(j_ptr, pos_ref, vel_ref); #endif } @@ -2071,10 +2129,16 @@ BOOL MController_set_joint_vel_ref(int j, CTRL_UNITS vel_ref, CTRL_UNITS acc_ref BOOL MController_set_joint_pos_raw(int j, CTRL_UNITS pos_ref) { -#if !defined(WRIST_MK2) - return Joint_set_pos_raw(smc->joint+j, pos_ref); + Joint *j_ptr= smc->joint+j; +#ifdef WRIST_MK2 + if(!j_ptr->belong2WristMK2) + return Joint_set_pos_raw(j_ptr, pos_ref); + + else + return JointSet_set_pos_ref(&(smc->jointSet[0]), j, pos_ref, 0.0f); #else - return JointSet_set_pos_ref(&(smc->jointSet[0]), j, pos_ref, 0.0f); + + return Joint_set_pos_raw(j_ptr, pos_ref); #endif } @@ -2100,10 +2164,14 @@ BOOL MController_set_joint_cur_ref(int j, CTRL_UNITS cur_ref) void MController_stop_joint(int j) { + Joint *j_ptr= smc->joint+j; #ifdef WRIST_MK2 - JointSet_stop(&(smc->jointSet[0]), j); + if(j_ptr->belong2WristMK2) + JointSet_stop(&(smc->jointSet[0]), j); + else + Joint_stop(j_ptr); #else - Joint_stop(smc->joint+j); + Joint_stop(j_ptr); #endif } diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c index 0f0034b28d..9e8a964df0 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c @@ -53,6 +53,7 @@ Joint* Joint_new(uint8_t n) void Joint_init(Joint* o) { o->dead_zone = ZERO; + o->belong2WristMK2 = FALSE; o->pos_min = ZERO; o->pos_max = ZERO; @@ -154,29 +155,34 @@ void Joint_reset_calibration_data(Joint* o) void Joint_config(Joint* o, uint8_t ID, eOmc_joint_config_t* config) { - o->ID = ID; + o->ID = ID; + if(o->belong2WristMK2) + { + const CTRL_UNITS lim = (150.0f/360.0f)*65536.0f; -#ifndef WRIST_MK2 - o->pos_min_soft = config->userlimits.min; - o->pos_max_soft = config->userlimits.max; - o->pos_min_hard = config->hardwarelimits.min; - o->pos_max_hard = config->hardwarelimits.max; - - o->pos_min = config->userlimits.min; - o->pos_max = config->userlimits.max; + o->pos_min = o->pos_min_soft = -lim; + o->pos_max = o->pos_max_soft = lim; + + o->pos_min_hard = -lim-400.0f; + o->pos_max_hard = lim+400.0f; + + o->vel_max = (90.0f/360.0f)*65536.0f; + } + else + { + - o->vel_max = config->maxvelocityofjoint; -#else - const CTRL_UNITS lim = (150.0f/360.0f)*65536.0f; - - o->pos_min = o->pos_min_soft = -lim; - o->pos_max = o->pos_max_soft = lim; - - o->pos_min_hard = -lim-400.0f; - o->pos_max_hard = lim+400.0f; + o->pos_min_soft = config->userlimits.min; + o->pos_max_soft = config->userlimits.max; + o->pos_min_hard = config->hardwarelimits.min; + o->pos_max_hard = config->hardwarelimits.max; + + o->pos_min = config->userlimits.min; + o->pos_max = config->userlimits.max; + + o->vel_max = config->maxvelocityofjoint; + } - o->vel_max = (90.0f/360.0f)*65536.0f; -#endif o->acc_max = 10000000.0f; @@ -484,13 +490,16 @@ int8_t Joint_pushing_limit(Joint* o) void Joint_set_limits(Joint* o, CTRL_UNITS pos_min, CTRL_UNITS pos_max) { -#ifndef WRIST_MK2 - o->pos_min = pos_min; - o->pos_max = pos_max; -#else - o->pos_min = -(150.0f/360.0f)*65536.0f; - o->pos_max = -o->pos_min; -#endif + if(o->belong2WristMK2) + { + o->pos_min = -(150.0f/360.0f)*65536.0f; + o->pos_max = -o->pos_min; + } + else + { + o->pos_min = pos_min; + o->pos_max = pos_max; + } Trajectory_config_limits(&o->trajectory, pos_min, pos_max, 0.0f, 0.0f); } @@ -651,11 +660,12 @@ CTRL_UNITS Joint_do_pwm_or_current_control(Joint* o) //CTRL_UNITS pos_err_old = o->pos_err; -#ifdef WRIST_MK2 - o->pos_err = wrap180(o->pos_ref - o->pos_fbk); -#else - o->pos_err = o->pos_ref - o->pos_fbk; -#endif + + if(o->belong2WristMK2) + o->pos_err = wrap180(o->pos_ref - o->pos_fbk); + else + o->pos_err = o->pos_ref - o->pos_fbk; + o->vel_err = o->vel_ref - o->vel_fbk; if (o->interaction_mode == eOmc_interactionmode_stiff) diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c index 6556608505..205f709570 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c @@ -44,25 +44,13 @@ #include "AbsEncoder.h" #include "Pid.h" #include "Calibrators.h" - +#include static void JointSet_set_inner_control_flags(JointSet* o); static const CTRL_UNITS DEG2ICUB = 65536.0f/360.0f; static const CTRL_UNITS ICUB2DEG = 360.0f/65536.0f; -#if defined(WRIST_MK2) - -#if defined(WRIST_MK2_RIGHT) - constexpr float prefactor = -1.0; - constexpr float postfactor = -1.0; -#else - constexpr float prefactor = +1.0; - constexpr float postfactor = +1.0; -#endif - -#endif - JointSet* JointSet_new(uint8_t n) // { JointSet* o = NEW(JointSet, n); @@ -116,36 +104,6 @@ void JointSet_init(JointSet* o) // o->calibration_in_progress = eomc_calibration_typeUndefined; -#ifdef WRIST_MK2 - o->is_parking = FALSE; - o->must_park = TRUE; - - o->wrist_decoupler.initialize(); - - Trajectory_init(&(o->ypr_trajectory[0]), 0, 0, 0); - Trajectory_init(&(o->ypr_trajectory[1]), 0, 0, 0); - Trajectory_init(&(o->ypr_trajectory[2]), 0, 0, 0); - - Trajectory_config_limits(&(o->ypr_trajectory[0]), -90.0f*DEG2ICUB, 90.0f*DEG2ICUB, 90.0f*DEG2ICUB, ZERO); - Trajectory_config_limits(&(o->ypr_trajectory[1]), -60.0f*DEG2ICUB, 50.0f*DEG2ICUB, 90.0f*DEG2ICUB, ZERO); - Trajectory_config_limits(&(o->ypr_trajectory[2]), -30.0f*DEG2ICUB, 30.0f*DEG2ICUB, 90.0f*DEG2ICUB, ZERO); - - o->ypr_pos_ref[0] = ZERO; - o->ypr_pos_ref[1] = ZERO; - o->ypr_pos_ref[2] = ZERO; - - o->ypr_vel_ref[0] = ZERO; - o->ypr_vel_ref[1] = ZERO; - o->ypr_vel_ref[2] = ZERO; - - o->ypr_pos_fbk[0] = ZERO; - o->ypr_pos_fbk[1] = ZERO; - o->ypr_pos_fbk[2] = ZERO; - - o->arm_pos_off[0] = 145.0f; - o->arm_pos_off[1] = 270.0f; - o->arm_pos_off[2] = 40.0f; -#endif } void JointSet_config // @@ -466,10 +424,30 @@ void JointSet_do(JointSet* o) } } +void JointSet_set_constraints(JointSet* o, const eOmc_jointSet_constraints_t *constraints) +{ + + o->special_constraint = (eOmc_jsetconstraint_t)constraints->type; + #ifdef WRIST_MK2 + if(eomc_jsetconstraint_ergocubwrist == o->special_constraint) + { + o->wristMK2.mk_version = (wrist_mk_version_t)constraints->param1; //can be WRIST_MK_VER_2_0 or WRIST_MK_VER_2_1 + o->wristMK2.is_right_wrist = constraints->param2; //0 is left, 1 is right + } + #else + { + o->special_limit = constraints->param1; + //NOTE: jsetcfg[s].constraints.param2 is not used currently. It is reserved for future use + } + #endif + +} + #ifdef WRIST_MK2 -static void JointSet_start_park(JointSet* o) + +static void JointSet_wristMK2_start_park(JointSet* o) { - o->is_parking = TRUE; + o->wristMK2.is_parking = TRUE; Joint_set_control_mode(&(o->joint[0]), eomc_controlmode_cmd_position); Joint_set_control_mode(&(o->joint[1]), eomc_controlmode_cmd_position); @@ -484,28 +462,25 @@ static void JointSet_start_park(JointSet* o) JointSet_send_debug_message(msg, 0, 0, 0); } -static void JointSet_stop_park(JointSet* o) +static void JointSet_wristMK2_stop_park(JointSet* o) { Joint_stop(&(o->joint[0])); Joint_stop(&(o->joint[1])); Joint_stop(&(o->joint[2])); - Trajectory_stop(&(o->ypr_trajectory[0]),ZERO); - Trajectory_stop(&(o->ypr_trajectory[1]),ZERO); - Trajectory_stop(&(o->ypr_trajectory[2]),ZERO); + Trajectory_stop(&(o->wristMK2.ypr_trajectory[0]),ZERO); + Trajectory_stop(&(o->wristMK2.ypr_trajectory[1]),ZERO); + Trajectory_stop(&(o->wristMK2.ypr_trajectory[2]),ZERO); JointSet_set_control_mode(o, (eOmc_controlmode_command_t)(o->control_mode)); - o->is_parking = FALSE; - - static char msg[] = "PARK DONE"; + o->wristMK2.is_parking = FALSE; - JointSet_send_debug_message(msg, 0, 0, 0); } -static BOOL JointSet_is_parked(JointSet* o) +static BOOL JointSet_wristMK2_is_parked(JointSet* o) { - if (!o->is_parking) return TRUE; + if (!o->wristMK2.is_parking) return TRUE; return (fabs(o->joint[0].pos_fbk) < 400.0f && fabs(o->joint[1].pos_fbk) < 400.0f && fabs(o->joint[2].pos_fbk) < 400.0f); } @@ -559,14 +534,20 @@ static int control_output_type(JointSet* o, int16_t control_mode) BOOL JointSet_set_control_mode(JointSet* o, eOmc_controlmode_command_t control_mode_cmd) { #ifdef WRIST_MK2 - Trajectory_stop(&(o->ypr_trajectory[0]), o->ypr_pos_fbk[0]); - Trajectory_stop(&(o->ypr_trajectory[1]), o->ypr_pos_fbk[1]); - Trajectory_stop(&(o->ypr_trajectory[2]), o->ypr_pos_fbk[2]); + if(eomc_jsetconstraint_ergocubwrist == o->special_constraint) + { + Trajectory_stop(&(o->wristMK2.ypr_trajectory[0]), o->wristMK2.ypr_pos_fbk[0]); + Trajectory_stop(&(o->wristMK2.ypr_trajectory[1]), o->wristMK2.ypr_pos_fbk[1]); + Trajectory_stop(&(o->wristMK2.ypr_trajectory[2]), o->wristMK2.ypr_pos_fbk[2]); + } #endif if (control_mode_cmd != eomc_controlmode_cmd_force_idle) { -#ifndef WRIST_MK2 - if ((eOmc_controlmode_t)control_mode_cmd == o->control_mode) return TRUE; +#ifdef WRIST_MK2 + if(eomc_jsetconstraint_ergocubwrist != o->special_constraint) + { + if ((eOmc_controlmode_t)control_mode_cmd == o->control_mode) return TRUE; + } #endif if (o->control_mode == eomc_controlmode_calib) return FALSE; @@ -583,7 +564,10 @@ BOOL JointSet_set_control_mode(JointSet* o, eOmc_controlmode_command_t control_m if (control_mode_cmd == eomc_controlmode_cmd_force_idle) { #ifdef WRIST_MK2 - o->must_park = TRUE; + if(eomc_jsetconstraint_ergocubwrist == o->special_constraint) + { + o->wristMK2.must_park = TRUE; + } #endif for (int k=0; kmotor+o->motors_of_set[k]); Joint_set_control_mode(o->joint+o->joints_of_set[k], control_mode_cmd); } - } - break; -#ifdef WRIST_MK2 + }break; + case eomc_controlmode_cmd_mixed: case eomc_controlmode_cmd_velocity_pos: case eomc_controlmode_cmd_position: @@ -644,23 +627,24 @@ BOOL JointSet_set_control_mode(JointSet* o, eOmc_controlmode_command_t control_m if (!Motor_set_run(o->motor+o->motors_of_set[k], o->motor_input_type)) return FALSE; } + eOmc_controlmode_command_t cmd = control_mode_cmd; + + if(eomc_jsetconstraint_ergocubwrist == o->special_constraint) + { + cmd=eomc_controlmode_cmd_direct; + } + + for (int k=0; kjoint+o->joints_of_set[k], eomc_controlmode_cmd_direct); + Joint_set_control_mode(o->joint+o->joints_of_set[k], cmd); } break; } -#endif + case eomc_controlmode_cmd_openloop: case eomc_controlmode_cmd_current: case eomc_controlmode_cmd_torque: -#ifndef WRIST_MK2 - case eomc_controlmode_cmd_direct: - case eomc_controlmode_cmd_mixed: - case eomc_controlmode_cmd_velocity_pos: - case eomc_controlmode_cmd_position: - case eomc_controlmode_cmd_velocity: -#endif case eomc_controlmode_cmd_vel_direct: { //if (o->external_fault) return FALSE; @@ -688,9 +672,6 @@ BOOL JointSet_set_control_mode(JointSet* o, eOmc_controlmode_command_t control_m case eomc_controlmode_cmd_velocity_pos: o->control_mode = (eOmc_controlmode_t)eomc_controlmode_cmd_mixed; break; - //case eomc_controlmode_cmd_velocity: - // o->control_mode = (eOmc_controlmode_t)eomc_controlmode_cmd_vel_direct; - // break; case eomc_controlmode_cmd_force_idle: o->control_mode = eomc_controlmode_idle; break; @@ -699,7 +680,6 @@ BOOL JointSet_set_control_mode(JointSet* o, eOmc_controlmode_command_t control_m break; } - //JointSet_send_debug_message("SET CONTROLMODE", 0, control_mode_cmd, o->control_mode); JointSet_set_inner_control_flags(o); @@ -859,97 +839,100 @@ static eoas_pos_ROT_t JointSet_calib14_ROT2pos_ROT(eOmc_calib14_ROT_t rot) void JointSet_do_pwm_control(JointSet* o) { int N = *(o->pN); + + CTRL_UNITS arm_pos_ref[3]; BOOL limits_torque_protection = FALSE; #ifdef WRIST_MK2 - - if (o->must_park) - { - JointSet_start_park(o); - - o->must_park = FALSE; - - static char msg[] = "BUONGIORNISSIMO"; - - JointSet_send_debug_message(msg, 0, 0, 0); - } - - if (o->is_parking) - { - if (JointSet_is_parked(o)) + if(eomc_jsetconstraint_ergocubwrist == o->special_constraint) + { + if (o->wristMK2.must_park) { - JointSet_stop_park(o); + JointSet_wristMK2_start_park(o); + + o->wristMK2.must_park = FALSE; + + static char msg[] = "pwm_ctrl: PARKING..."; + + JointSet_send_debug_message(msg, 0, 0, 0); } - } - - if (!o->is_parking) - { - Trajectory_do_step(o->ypr_trajectory, o->ypr_pos_ref, o->ypr_vel_ref, o->ypr_acc_ref ); - Trajectory_do_step(o->ypr_trajectory+1, o->ypr_pos_ref+1, o->ypr_vel_ref+1, o->ypr_acc_ref+1); - Trajectory_do_step(o->ypr_trajectory+2, o->ypr_pos_ref+2, o->ypr_vel_ref+2, o->ypr_acc_ref+2); - } - else - { - o->ypr_pos_ref[0] = o->ypr_vel_ref[0] = o->ypr_acc_ref[0] = ZERO; - o->ypr_pos_ref[1] = o->ypr_vel_ref[1] = o->ypr_acc_ref[1] = ZERO; - o->ypr_pos_ref[2] = o->ypr_vel_ref[2] = o->ypr_acc_ref[2] = ZERO; - } - -// static int noflood = 0; -// -// if (++noflood > 1000) -// { -// noflood = 0; -// int16_t par16 = (int16_t)(o->ypr_pos_ref[0]*ICUB2DEG); -// int32_t par32 = (int32_t)(Trajectory_get_target_position(o->ypr_trajectory)*ICUB2DEG); -// JointSet_send_debug_message(NULL, 0, par16, par32); -// } - - o->wrist_decoupler.rtU.ypr[0] = prefactor*ICUB2DEG*o->ypr_pos_ref[0]; - o->wrist_decoupler.rtU.ypr[1] = ICUB2DEG*o->ypr_pos_ref[1]; - o->wrist_decoupler.rtU.ypr[2] = prefactor*ICUB2DEG*o->ypr_pos_ref[2]; - o->wrist_decoupler.rtU.theta_meas[0] = ICUB2DEG*(o->joint[0]).pos_fbk + o->arm_pos_off[0]; - o->wrist_decoupler.rtU.theta_meas[1] = ICUB2DEG*(o->joint[1]).pos_fbk + o->arm_pos_off[1]; - o->wrist_decoupler.rtU.theta_meas[2] = ICUB2DEG*(o->joint[2]).pos_fbk + o->arm_pos_off[2]; - - ////////////////////////// - o->wrist_decoupler.step(); - ////////////////////////// - - if (o->wrist_decoupler.rtY.singularity) - { - if (!o->is_parking) + if (o->wristMK2.is_parking) { - JointSet_start_park(o); + if (JointSet_wristMK2_is_parked(o)) + { + static char msg[] = "pwm_ctrl:PARKING DONE."; + + JointSet_send_debug_message(msg, 0, 0, 0); + + JointSet_wristMK2_stop_park(o); + } } - } - - o->ypr_pos_fbk[0] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[0]); - o->ypr_pos_fbk[1] = DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[1]); - o->ypr_pos_fbk[2] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[2]); -// o->ypr_pos_fbk[0] = ZERO; -// o->ypr_pos_fbk[1] = ZERO; -// o->ypr_pos_fbk[2] = ZERO; - - CTRL_UNITS arm_pos_ref[3]; - - arm_pos_ref[0] = DEG2ICUB*wrap180(o->wrist_decoupler.rtY.theta_star[0] - o->arm_pos_off[0]); - arm_pos_ref[1] = DEG2ICUB*wrap180(o->wrist_decoupler.rtY.theta_star[1] - o->arm_pos_off[1]); - arm_pos_ref[2] = DEG2ICUB*wrap180(o->wrist_decoupler.rtY.theta_star[2] - o->arm_pos_off[2]); - -// arm_pos_ref[0] = ZERO; -// arm_pos_ref[1] = ZERO; -// arm_pos_ref[2] = ZERO; + if (!o->wristMK2.is_parking) + { + Trajectory_do_step(o->wristMK2.ypr_trajectory, o->wristMK2.ypr_pos_ref, o->wristMK2.ypr_vel_ref, o->wristMK2.ypr_acc_ref ); + Trajectory_do_step(o->wristMK2.ypr_trajectory+1, o->wristMK2.ypr_pos_ref+1, o->wristMK2.ypr_vel_ref+1, o->wristMK2.ypr_acc_ref+1); + Trajectory_do_step(o->wristMK2.ypr_trajectory+2, o->wristMK2.ypr_pos_ref+2, o->wristMK2.ypr_vel_ref+2, o->wristMK2.ypr_acc_ref+2); + } + else + { + o->wristMK2.ypr_pos_ref[0] = o->wristMK2.ypr_vel_ref[0] = o->wristMK2.ypr_acc_ref[0] = ZERO; + o->wristMK2.ypr_pos_ref[1] = o->wristMK2.ypr_vel_ref[1] = o->wristMK2.ypr_acc_ref[1] = ZERO; + o->wristMK2.ypr_pos_ref[2] = o->wristMK2.ypr_vel_ref[2] = o->wristMK2.ypr_acc_ref[2] = ZERO; + } + + // static int noflood = 0; + // + // if (++noflood > 1000) + // { + // noflood = 0; + // int16_t par16 = (int16_t)(o->ypr_pos_ref[0]*ICUB2DEG); + // int32_t par32 = (int32_t)(Trajectory_get_target_position(o->ypr_trajectory)*ICUB2DEG); + // JointSet_send_debug_message(NULL, 0, par16, par32); + // } + + o->wristMK2.wristDecoupler.rtU.ypr_star[0] = ICUB2DEG*o->wristMK2.ypr_pos_ref[0]; + o->wristMK2.wristDecoupler.rtU.ypr_star[1] = ICUB2DEG*o->wristMK2.ypr_pos_ref[1]; + o->wristMK2.wristDecoupler.rtU.ypr_star[2] = ICUB2DEG*o->wristMK2.ypr_pos_ref[2]; + + o->wristMK2.wristDecoupler.rtU.theta_diff[0] = ICUB2DEG*(o->joint[0]).pos_fbk; + o->wristMK2.wristDecoupler.rtU.theta_diff[1] = ICUB2DEG*(o->joint[1]).pos_fbk; + o->wristMK2.wristDecoupler.rtU.theta_diff[2] = ICUB2DEG*(o->joint[2]).pos_fbk; + + ////////////////////////// + o->wristMK2.wristDecoupler.step(); + ////////////////////////// + + if (o->wristMK2.wristDecoupler.rtY.singularity) + { + if (!o->wristMK2.is_parking) + { + JointSet_wristMK2_start_park(o); + } + } + + o->wristMK2.ypr_pos_fbk[0] = DEG2ICUB*(o->wristMK2.wristDecoupler.rtY.ypr_meas[0]); + o->wristMK2.ypr_pos_fbk[1] = DEG2ICUB*(o->wristMK2.wristDecoupler.rtY.ypr_meas[1]); + o->wristMK2.ypr_pos_fbk[2] = DEG2ICUB*(o->wristMK2.wristDecoupler.rtY.ypr_meas[2]); + + + arm_pos_ref[0] = DEG2ICUB*o->wristMK2.wristDecoupler.rtY.theta_star[0]; + arm_pos_ref[1] = DEG2ICUB*o->wristMK2.wristDecoupler.rtY.theta_star[1]; + arm_pos_ref[2] = DEG2ICUB*o->wristMK2.wristDecoupler.rtY.theta_star[2]; + + }//end if eroigocubwrist #endif for (int js=0; jsjoint+o->joints_of_set[js]; #ifdef WRIST_MK2 - if (!o->is_parking) + if(eomc_jsetconstraint_ergocubwrist == o->special_constraint) { - Joint_set_pos_raw(pJoint, arm_pos_ref[js]); + if (!o->wristMK2.is_parking) + { + Joint_set_pos_raw(pJoint, arm_pos_ref[js]); + } } #endif Joint_do_pwm_or_current_control(pJoint); @@ -1098,93 +1081,105 @@ void JointSet_do_pwm_control(JointSet* o) static void JointSet_do_current_control(JointSet* o) { int N = *(o->pN); + + CTRL_UNITS arm_pos_ref[3]; BOOL limits_torque_protection = FALSE; #ifdef WRIST_MK2 - if (o->must_park) + if(eomc_jsetconstraint_ergocubwrist == o->special_constraint) { - JointSet_start_park(o); + if (o->wristMK2.must_park) + { + JointSet_wristMK2_start_park(o); - o->must_park = FALSE; + o->wristMK2.must_park = FALSE; - static char msg[] = "BUONGIORNISSIMO 2"; + static char msg[] = "cur_ctrl:PARKING..."; - JointSet_send_debug_message(msg, 0, 0, 0); - } + JointSet_send_debug_message(msg, 0, 0, 0); + } - if (o->is_parking) - { - if (JointSet_is_parked(o)) + if (o->wristMK2.is_parking) { - JointSet_stop_park(o); + if (JointSet_wristMK2_is_parked(o)) + { + static char msg[] = "cur_ctrl:PARKING DONE."; + + JointSet_send_debug_message(msg, 0, 0, 0); + + JointSet_wristMK2_stop_park(o); + } } - } - if (!o->is_parking) - { - Trajectory_do_step(o->ypr_trajectory, o->ypr_pos_ref, o->ypr_vel_ref, o->ypr_acc_ref ); - Trajectory_do_step(o->ypr_trajectory+1, o->ypr_pos_ref+1, o->ypr_vel_ref+1, o->ypr_acc_ref+1); - Trajectory_do_step(o->ypr_trajectory+2, o->ypr_pos_ref+2, o->ypr_vel_ref+2, o->ypr_acc_ref+2); - } - else - { - o->ypr_pos_ref[0] = o->ypr_vel_ref[0] = o->ypr_acc_ref[0] = ZERO; - o->ypr_pos_ref[1] = o->ypr_vel_ref[1] = o->ypr_acc_ref[1] = ZERO; - o->ypr_pos_ref[2] = o->ypr_vel_ref[2] = o->ypr_acc_ref[2] = ZERO; - } - -// static int noflood = 0; -// -// if (++noflood > 1000) -// { -// noflood = 0; -// int16_t par16 = (int16_t)(o->ypr_pos_ref[0]*ICUB2DEG); -// int32_t par32 = (int32_t)(Trajectory_get_target_position(o->ypr_trajectory)*ICUB2DEG); -// JointSet_send_debug_message(NULL, 0, par16, par32); -// } - - o->wrist_decoupler.rtU.ypr[0] = prefactor*ICUB2DEG*o->ypr_pos_ref[0]; - o->wrist_decoupler.rtU.ypr[1] = ICUB2DEG*o->ypr_pos_ref[1]; - o->wrist_decoupler.rtU.ypr[2] = prefactor*ICUB2DEG*o->ypr_pos_ref[2]; - - o->wrist_decoupler.rtU.theta_meas[0] = ICUB2DEG*(o->joint[0]).pos_fbk + o->arm_pos_off[0]; - o->wrist_decoupler.rtU.theta_meas[1] = ICUB2DEG*(o->joint[1]).pos_fbk + o->arm_pos_off[1]; - o->wrist_decoupler.rtU.theta_meas[2] = ICUB2DEG*(o->joint[2]).pos_fbk + o->arm_pos_off[2]; - - ////////////////////////// - o->wrist_decoupler.step(); - ////////////////////////// + if (!o->wristMK2.is_parking) + { + Trajectory_do_step(o->wristMK2.ypr_trajectory, o->wristMK2.ypr_pos_ref, o->wristMK2.ypr_vel_ref, o->wristMK2.ypr_acc_ref ); + Trajectory_do_step(o->wristMK2.ypr_trajectory+1, o->wristMK2.ypr_pos_ref+1, o->wristMK2.ypr_vel_ref+1, o->wristMK2.ypr_acc_ref+1); + Trajectory_do_step(o->wristMK2.ypr_trajectory+2, o->wristMK2.ypr_pos_ref+2, o->wristMK2.ypr_vel_ref+2, o->wristMK2.ypr_acc_ref+2); + } + else + { + o->wristMK2.ypr_pos_ref[0] = o->wristMK2.ypr_vel_ref[0] = o->wristMK2.ypr_acc_ref[0] = ZERO; + o->wristMK2.ypr_pos_ref[1] = o->wristMK2.ypr_vel_ref[1] = o->wristMK2.ypr_acc_ref[1] = ZERO; + o->wristMK2.ypr_pos_ref[2] = o->wristMK2.ypr_vel_ref[2] = o->wristMK2.ypr_acc_ref[2] = ZERO; + } - if (o->wrist_decoupler.rtY.singularity) - { - if (!o->is_parking) + // static int noflood = 0; + // + // if (++noflood > 1000) + // { + // noflood = 0; + // int16_t par16 = (int16_t)(o->ypr_pos_ref[0]*ICUB2DEG); + // int32_t par32 = (int32_t)(Trajectory_get_target_position(o->ypr_trajectory)*ICUB2DEG); + // JointSet_send_debug_message(NULL, 0, par16, par32); + // } + + o->wristMK2.wristDecoupler.rtU.ypr_star[0] = ICUB2DEG*o->wristMK2.ypr_pos_ref[0]; + o->wristMK2.wristDecoupler.rtU.ypr_star[1] = ICUB2DEG*o->wristMK2.ypr_pos_ref[1]; + o->wristMK2.wristDecoupler.rtU.ypr_star[2] = ICUB2DEG*o->wristMK2.ypr_pos_ref[2]; + + o->wristMK2.wristDecoupler.rtU.theta_diff[0] = ICUB2DEG*(o->joint[0]).pos_fbk; + o->wristMK2.wristDecoupler.rtU.theta_diff[1] = ICUB2DEG*(o->joint[1]).pos_fbk; + o->wristMK2.wristDecoupler.rtU.theta_diff[2] = ICUB2DEG*(o->joint[2]).pos_fbk; + + ////////////////////////// + o->wristMK2.wristDecoupler.step(); + ////////////////////////// + + if (o->wristMK2.wristDecoupler.rtY.singularity) { - JointSet_start_park(o); + if (!o->wristMK2.is_parking) + { + JointSet_wristMK2_start_park(o); + } } - } - o->ypr_pos_fbk[0] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[0]); - o->ypr_pos_fbk[1] = DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[1]); - o->ypr_pos_fbk[2] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[2]); + o->wristMK2.ypr_pos_fbk[0] = DEG2ICUB*(o->wristMK2.wristDecoupler.rtY.ypr_meas[0]); + o->wristMK2.ypr_pos_fbk[1] = DEG2ICUB*(o->wristMK2.wristDecoupler.rtY.ypr_meas[1]); + o->wristMK2.ypr_pos_fbk[2] = DEG2ICUB*(o->wristMK2.wristDecoupler.rtY.ypr_meas[2]); - CTRL_UNITS arm_pos_ref[3]; + //CTRL_UNITS arm_pos_ref[3]; move out of this section to make it available to next scope - arm_pos_ref[0] = DEG2ICUB*wrap180(o->wrist_decoupler.rtY.theta_star[0] - o->arm_pos_off[0]); - arm_pos_ref[1] = DEG2ICUB*wrap180(o->wrist_decoupler.rtY.theta_star[1] - o->arm_pos_off[1]); - arm_pos_ref[2] = DEG2ICUB*wrap180(o->wrist_decoupler.rtY.theta_star[2] - o->arm_pos_off[2]); + arm_pos_ref[0] = DEG2ICUB*o->wristMK2.wristDecoupler.rtY.theta_star[0]; + arm_pos_ref[1] = DEG2ICUB*o->wristMK2.wristDecoupler.rtY.theta_star[1]; + arm_pos_ref[2] = DEG2ICUB*o->wristMK2.wristDecoupler.rtY.theta_star[2]; -// arm_pos_ref[0] = ZERO; -// arm_pos_ref[1] = ZERO; -// arm_pos_ref[2] = ZERO; + // arm_pos_ref[0] = ZERO; + // arm_pos_ref[1] = ZERO; + // arm_pos_ref[2] = ZERO; + }//end ergocubwrist #endif for (int js=0; jsjoint+o->joints_of_set[js]; #ifdef WRIST_MK2 - if (!o->is_parking) + if(eomc_jsetconstraint_ergocubwrist == o->special_constraint) { - Joint_set_pos_raw(pJoint, arm_pos_ref[js]); + if (!o->wristMK2.is_parking) + { + Joint_set_pos_raw(pJoint, arm_pos_ref[js]); + } } #endif @@ -1362,41 +1357,44 @@ static void JointSet_do_off(JointSet* o) } #ifdef WRIST_MK2 - // be sure to continue to report the encoder readings - o->wrist_decoupler.rtU.theta_meas[0] = ICUB2DEG*(o->joint[0]).pos_fbk + o->arm_pos_off[0]; - o->wrist_decoupler.rtU.theta_meas[1] = ICUB2DEG*(o->joint[1]).pos_fbk + o->arm_pos_off[1]; - o->wrist_decoupler.rtU.theta_meas[2] = ICUB2DEG*(o->joint[2]).pos_fbk + o->arm_pos_off[2]; - - o->wrist_decoupler.step(); - - if (o->wrist_decoupler.rtY.singularity) + if(eomc_jsetconstraint_ergocubwrist == o->special_constraint) { - if (!o->is_parking) + // be sure to continue to report the encoder readings + o->wristMK2.wristDecoupler.rtU.theta_diff[0] = ICUB2DEG*(o->joint[0]).pos_fbk; + o->wristMK2.wristDecoupler.rtU.theta_diff[1] = ICUB2DEG*(o->joint[1]).pos_fbk; + o->wristMK2.wristDecoupler.rtU.theta_diff[2] = ICUB2DEG*(o->joint[2]).pos_fbk; + + o->wristMK2.wristDecoupler.step(); + + if (o->wristMK2.wristDecoupler.rtY.singularity) { - JointSet_start_park(o); + if (!o->wristMK2.is_parking) + { + JointSet_wristMK2_start_park(o); + } } - } - - // update joint positions (from the yarpmotorgui they still appear as yaw, pitch and roll) - o->ypr_pos_fbk[0] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[0]); - o->ypr_pos_fbk[1] = DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[1]); - o->ypr_pos_fbk[2] = postfactor*DEG2ICUB*(o->wrist_decoupler.rtY.ypr_meas[2]); - - // be sure that when we go in RUN again the joint does not move from it current position - CTRL_UNITS arm_pos_ref[3]; - - arm_pos_ref[0] = DEG2ICUB*wrap180(o->wrist_decoupler.rtY.theta_star[0] - o->arm_pos_off[0]); - arm_pos_ref[1] = DEG2ICUB*wrap180(o->wrist_decoupler.rtY.theta_star[1] - o->arm_pos_off[1]); - arm_pos_ref[2] = DEG2ICUB*wrap180(o->wrist_decoupler.rtY.theta_star[2] - o->arm_pos_off[2]); - - for (int js=0; jsjoint+o->joints_of_set[js]; - if (!o->is_parking) + + // update joint positions (from the yarpmotorgui they still appear as yaw, pitch and roll) + o->wristMK2.ypr_pos_fbk[0] = DEG2ICUB*(o->wristMK2.wristDecoupler.rtY.ypr_meas[0]); + o->wristMK2.ypr_pos_fbk[1] = DEG2ICUB*(o->wristMK2.wristDecoupler.rtY.ypr_meas[1]); + o->wristMK2.ypr_pos_fbk[2] = DEG2ICUB*(o->wristMK2.wristDecoupler.rtY.ypr_meas[2]); + + // be sure that when we go in RUN again the joint does not move from it current position + CTRL_UNITS arm_pos_ref[3]; + + arm_pos_ref[0] = DEG2ICUB*o->wristMK2.wristDecoupler.rtY.theta_star[0]; + arm_pos_ref[1] = DEG2ICUB*o->wristMK2.wristDecoupler.rtY.theta_star[1]; + arm_pos_ref[2] = DEG2ICUB*o->wristMK2.wristDecoupler.rtY.theta_star[2]; + + for (int js=0; jsjoint+o->joints_of_set[js]; + if (!o->wristMK2.is_parking) + { + Joint_set_pos_raw(pJoint, arm_pos_ref[js]); + } } - } + } //end ergocubwrist #endif // WRIST_MK2 } @@ -2130,9 +2128,74 @@ void JointSet_send_debug_message(char *message, uint8_t jid, uint16_t par16, uin } #ifdef WRIST_MK2 -BOOL JointSet_set_pos_ref(JointSet* o, int j, CTRL_UNITS pos_ref, CTRL_UNITS vel_ref) +extern void JointSet_init_wrist_decoupler(JointSet* o) +{ + o->wristMK2.is_parking = FALSE; + o->wristMK2.must_park = TRUE; + + o->wristMK2.wristDecoupler.initialize(); + + o->wristMK2.wristDecoupler.rtU.arm_bend = 30.0f; + + + if(o->wristMK2.is_right_wrist) + o->wristMK2.wristDecoupler.rtU.RL = true; + else + o->wristMK2.wristDecoupler.rtU.RL = false; + + if(o->wristMK2.mk_version == WRIST_MK_VER_2_1) + { + o->wristMK2.wristDecoupler.rtU.plat_off[0] = 60.0f; + o->wristMK2.wristDecoupler.rtU.plat_off[1] = 180.0f; + o->wristMK2.wristDecoupler.rtU.plat_off[2] = 300.0f; + + o->wristMK2.wristDecoupler.rtU.theta_off[0] = 60.0f + 60.0f; + o->wristMK2.wristDecoupler.rtU.theta_off[1] = 180.0f + 60.0f; + o->wristMK2.wristDecoupler.rtU.theta_off[2] = 300.0f + 60.0f; + } + else + { + o->wristMK2.wristDecoupler.rtU.plat_off[0] = 85.0f; + o->wristMK2.wristDecoupler.rtU.plat_off[1] = 185.0f; + o->wristMK2.wristDecoupler.rtU.plat_off[2] = 280.0f; + + o->wristMK2.wristDecoupler.rtU.theta_off[0] = 85.0f + 60.0f; + o->wristMK2.wristDecoupler.rtU.theta_off[1] = 185.0f + 85.0f; + o->wristMK2.wristDecoupler.rtU.theta_off[2] = 280.0f + 120.0f; + } + + + + Trajectory_init(&(o->wristMK2.ypr_trajectory[0]), 0, 0, 0); + Trajectory_init(&(o->wristMK2.ypr_trajectory[1]), 0, 0, 0); + Trajectory_init(&(o->wristMK2.ypr_trajectory[2]), 0, 0, 0); + + Trajectory_config_limits(&(o->wristMK2.ypr_trajectory[0]), -90.0f*DEG2ICUB, 90.0f*DEG2ICUB, 90.0f*DEG2ICUB, ZERO); + Trajectory_config_limits(&(o->wristMK2.ypr_trajectory[1]), -60.0f*DEG2ICUB, 50.0f*DEG2ICUB, 90.0f*DEG2ICUB, ZERO); + Trajectory_config_limits(&(o->wristMK2.ypr_trajectory[2]), -30.0f*DEG2ICUB, 30.0f*DEG2ICUB, 90.0f*DEG2ICUB, ZERO); + + o->wristMK2.ypr_pos_ref[0] = ZERO; + o->wristMK2.ypr_pos_ref[1] = ZERO; + o->wristMK2.ypr_pos_ref[2] = ZERO; + + o->wristMK2.ypr_vel_ref[0] = ZERO; + o->wristMK2.ypr_vel_ref[1] = ZERO; + o->wristMK2.ypr_vel_ref[2] = ZERO; + + o->wristMK2.ypr_pos_fbk[0] = ZERO; + o->wristMK2.ypr_pos_fbk[1] = ZERO; + o->wristMK2.ypr_pos_fbk[2] = ZERO; + + //o->arm_pos_off[0] = 145.0f; + //o->arm_pos_off[1] = 270.0f; + //o->arm_pos_off[2] = 40.0f; + +} + + +BOOL JointSet_set_pos_ref(JointSet* o, int j, CTRL_UNITS pos_ref, CTRL_UNITS vel_ref) //use only in WRIST_MK2 case { - if (o->is_parking) return FALSE; + if (o->wristMK2.is_parking) return FALSE; if ((o->control_mode != eomc_controlmode_direct) && (o->control_mode != eomc_controlmode_position) && (o->control_mode != eomc_controlmode_mixed) && (o->control_mode != eomc_ctrlmval_velocity_pos)) { @@ -2141,28 +2204,28 @@ BOOL JointSet_set_pos_ref(JointSet* o, int j, CTRL_UNITS pos_ref, CTRL_UNITS vel if(eomc_controlmode_direct == o->control_mode) { - Trajectory_set_pos_raw(&(o->ypr_trajectory[j]), pos_ref); + Trajectory_set_pos_raw(&(o->wristMK2.ypr_trajectory[j]), pos_ref); return TRUE; } if (vel_ref == 0.0f) return TRUE; - Trajectory_set_pos_end(&(o->ypr_trajectory[j]), pos_ref, vel_ref); + Trajectory_set_pos_end(&(o->wristMK2.ypr_trajectory[j]), pos_ref, vel_ref); return TRUE; } -void JointSet_stop(JointSet* o, int j) +void JointSet_stop(JointSet* o, int j) //use only in WRIST_MK2 case { - Trajectory_stop(&(o->ypr_trajectory[j]), o->ypr_pos_fbk[j]); + Trajectory_stop(&(o->wristMK2.ypr_trajectory[j]), o->wristMK2.ypr_pos_fbk[j]); } -extern void JointSet_get_state(JointSet* o, int j, eOmc_joint_status_t* joint_state) +extern void JointSet_get_state(JointSet* o, int j, eOmc_joint_status_t* joint_state) //use only in WRIST_MK2 case { joint_state->core.modes.interactionmodestatus = o->interaction_mode; joint_state->core.modes.controlmodestatus = o->control_mode; - joint_state->core.modes.ismotiondone = Trajectory_is_done(&(o->ypr_trajectory[j])); - joint_state->core.measures.meas_position = o->ypr_pos_fbk[j]; + joint_state->core.modes.ismotiondone = Trajectory_is_done(&(o->wristMK2.ypr_trajectory[j])); + joint_state->core.measures.meas_position = o->wristMK2.ypr_pos_fbk[j]; joint_state->core.measures.meas_velocity = ZERO; joint_state->core.measures.meas_acceleration = ZERO; joint_state->core.measures.meas_torque = ZERO; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h index 64db37483e..af8e4be35b 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h @@ -36,6 +36,33 @@ #include "hal_led.h" +#ifdef WRIST_MK2 +enum wrist_mk_version_t {WRIST_MK_VER_2_0 = 20, WRIST_MK_VER_2_1 = 21}; + +/* The struct wristMk2_t contains all the data related to the wrist MK2*/ +typedef struct //wristMk2_t +{ + BOOL is_parking; + BOOL must_park; + + wrist_mk_version_t mk_version; + BOOL is_right_wrist; + + wrist_decoupler wristDecoupler; + + Trajectory ypr_trajectory[3]; + + CTRL_UNITS ypr_pos_ref[3]; + CTRL_UNITS ypr_vel_ref[3]; + CTRL_UNITS ypr_acc_ref[3]; + CTRL_UNITS ypr_pos_fbk[3]; + + //CTRL_UNITS arm_pos_off[3]; + +} wristMK2_t; + +#endif + typedef struct // JointSet { @@ -91,22 +118,9 @@ typedef struct // JointSet eOmc_calibration_type_t calibration_in_progress; int32_t calibration_timeout; - -#ifdef WRIST_MK2 - BOOL is_parking; - BOOL must_park; - - wrist_decouplerModelClass wrist_decoupler; - - Trajectory ypr_trajectory[3]; - - CTRL_UNITS ypr_pos_ref[3]; - CTRL_UNITS ypr_vel_ref[3]; - CTRL_UNITS ypr_acc_ref[3]; - CTRL_UNITS ypr_pos_fbk[3]; - - CTRL_UNITS arm_pos_off[3]; -#endif + #ifdef WRIST_MK2 + wristMK2_t wristMK2; + #endif TripodCalib tripod_calib; } JointSet; @@ -148,10 +162,12 @@ extern void JointSet_do_pwm_control(JointSet* o); extern void JointSet_send_debug_message(char *message, uint8_t jid, uint16_t par16, uint64_t par64); #ifdef WRIST_MK2 -extern BOOL JointSet_set_pos_ref(JointSet* o, int j, CTRL_UNITS pos_ref, CTRL_UNITS vel_ref); -extern void JointSet_get_state(JointSet* o, int j, eOmc_joint_status_t* joint_state); -extern void JointSet_stop(JointSet* o, int j); +extern BOOL JointSet_set_pos_ref(JointSet* o, int j, CTRL_UNITS pos_ref, CTRL_UNITS vel_ref); //used only for WRIST_MK2 +extern void JointSet_get_state(JointSet* o, int j, eOmc_joint_status_t* joint_state); //used only for WRIST_MK2 +extern void JointSet_stop(JointSet* o, int j); //used only for WRIST_MK2 #endif +extern void JointSet_set_constraints(JointSet* o, const eOmc_jointSet_constraints_t *constraints); +extern void JointSet_init_wrist_decoupler(JointSet* o); #endif // include-guard diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint_hid.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint_hid.h index 4f483fe477..4889fa4bec 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint_hid.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint_hid.h @@ -29,6 +29,8 @@ struct Joint_hid // Joint BOOL use_hard_limit; + BOOL belong2WristMK2; + CTRL_UNITS pos_min_soft; CTRL_UNITS pos_max_soft; CTRL_UNITS pos_min_hard; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/rtwtypes.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/rtwtypes.h index 8ecf5da44e..7ed827cc37 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/rtwtypes.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/rtwtypes.h @@ -1,15 +1,15 @@ // // Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, educational organizations only. Not for -// government, commercial, or other organizational use. +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. // // File: rtwtypes.h // // Code generated for Simulink model 'wrist_decoupler'. // -// Model version : 2.3 -// Simulink Coder version : 9.4 (R2020b) 29-Jul-2020 -// C/C++ source code generated on : Tue Nov 17 12:30:21 2020 +// Model version : 7.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Tue Jul 25 11:18:40 2023 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler.cpp b/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler.cpp index ac6e8d30d5..e4f3aa5144 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler.cpp +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler.cpp @@ -1,15 +1,15 @@ // // Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, educational organizations only. Not for -// government, commercial, or other organizational use. +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. // // File: wrist_decoupler.cpp // // Code generated for Simulink model 'wrist_decoupler'. // -// Model version : 2.3 -// Simulink Coder version : 9.4 (R2020b) 29-Jul-2020 -// C/C++ source code generated on : Tue Nov 17 12:30:21 2020 +// Model version : 7.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Tue Jul 25 11:18:40 2023 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -19,10 +19,15 @@ // Validation result: Not run // #include "wrist_decoupler.h" +#include "rtwtypes.h" +#include +#include +#include +#include #define NumBitsPerChar 8U -extern real_T rt_atan2d_snf(real_T u0, real_T u1); extern real_T rt_remd_snf(real_T u0, real_T u1); +extern real_T rt_atan2d_snf(real_T u0, real_T u1); //===========* // Constants * @@ -54,7 +59,8 @@ extern real_T rt_remd_snf(real_T u0, real_T u1); #endif #endif -extern "C" { +extern "C" +{ real_T rtInf; real_T rtMinusInf; real_T rtNaN; @@ -62,7 +68,8 @@ extern "C" { real32_T rtMinusInfF; real32_T rtNaNF; } - extern "C" + +extern "C" { // // Initialize rtNaN needed by the generated code. @@ -94,14 +101,15 @@ extern "C" { // static real32_T rtGetNaNF(void) { - IEEESingle nanF = { { 0 } }; + IEEESingle nanF = { { 0.0F } }; nanF.wordL.wordLuint = 0xFFC00000U; return nanF.wordL.wordLreal; } } -extern "C" { +extern "C" +{ // // Initialize the rtInf, rtMinusInf, and rtNaN needed by the // generated code. NaN is initialized as non-signaling. Assumes IEEE. @@ -161,7 +169,8 @@ extern "C" { (tmp.wordL.wordLuint & 0x007FFFFF) != 0 ); } } - extern "C" + +extern "C" { // // Initialize rtInf needed by the generated code. @@ -234,138 +243,24 @@ extern "C" { } } -real_T rt_atan2d_snf(real_T u0, real_T u1) -{ - real_T y; - int32_T u0_0; - int32_T u1_0; - if (rtIsNaN(u0) || rtIsNaN(u1)) { - y = (rtNaN); - } else if (rtIsInf(u0) && rtIsInf(u1)) { - if (u0 > 0.0) { - u0_0 = 1; - } else { - u0_0 = -1; - } - - if (u1 > 0.0) { - u1_0 = 1; - } else { - u1_0 = -1; - } - - y = std::atan2(static_cast(u0_0), static_cast(u1_0)); - } else if (u1 == 0.0) { - if (u0 > 0.0) { - y = RT_PI / 2.0; - } else if (u0 < 0.0) { - y = -(RT_PI / 2.0); - } else { - y = 0.0; - } - } else { - y = std::atan2(u0, u1); - } - - return y; -} - -// Function for MATLAB Function: '/motors2quat' -real_T wrist_decouplerModelClass::maximum(const real_T x[3]) -{ - real_T ex; - int32_T idx; - int32_T k; - boolean_T exitg1; - if (!rtIsNaN(x[0])) { - idx = 1; - } else { - idx = 0; - k = 2; - exitg1 = false; - while ((!exitg1) && (k < 4)) { - if (!rtIsNaN(x[k - 1])) { - idx = k; - exitg1 = true; - } else { - k++; - } - } - } - - if (idx == 0) { - ex = x[0]; - } else { - ex = x[idx - 1]; - while (idx + 1 <= 3) { - if (ex < x[idx]) { - ex = x[idx]; - } - - idx++; - } - } - - return ex; -} - -// Function for MATLAB Function: '/motors2quat' -real_T wrist_decouplerModelClass::minimum(const real_T x[3]) -{ - real_T ex; - int32_T idx; - int32_T k; - boolean_T exitg1; - if (!rtIsNaN(x[0])) { - idx = 1; - } else { - idx = 0; - k = 2; - exitg1 = false; - while ((!exitg1) && (k < 4)) { - if (!rtIsNaN(x[k - 1])) { - idx = k; - exitg1 = true; - } else { - k++; - } - } - } - - if (idx == 0) { - ex = x[0]; - } else { - ex = x[idx - 1]; - while (idx + 1 <= 3) { - if (ex > x[idx]) { - ex = x[idx]; - } - - idx++; - } - } - - return ex; -} - real_T rt_remd_snf(real_T u0, real_T u1) { - real_T u1_0; real_T y; if (rtIsNaN(u0) || rtIsNaN(u1) || rtIsInf(u0)) { y = (rtNaN); } else if (rtIsInf(u1)) { y = u0; } else { + real_T q; if (u1 < 0.0) { - u1_0 = std::ceil(u1); + q = std::ceil(u1); } else { - u1_0 = std::floor(u1); + q = std::floor(u1); } - if ((u1 != 0.0) && (u1 != u1_0)) { - u1_0 = std::abs(u0 / u1); - if (!(std::abs(u1_0 - std::floor(u1_0 + 0.5)) > DBL_EPSILON * u1_0)) { + if ((u1 != 0.0) && (u1 != q)) { + q = std::abs(u0 / u1); + if (!(std::abs(q - std::floor(q + 0.5)) > DBL_EPSILON * q)) { y = 0.0 * u0; } else { y = std::fmod(u0, u1); @@ -378,562 +273,731 @@ real_T rt_remd_snf(real_T u0, real_T u1) return y; } -// Function for MATLAB Function: '/motors2quat' -void wrist_decouplerModelClass::sind(real_T x[3]) +// Function for MATLAB Function: '/motors2ypr' +void wrist_decoupler::cosd(real_T x[3]) { real_T absx; - real_T c_x; + real_T b_x; int8_T n; if (rtIsInf(x[0]) || rtIsNaN(x[0])) { - c_x = (rtNaN); + x[0] = (rtNaN); } else { - c_x = rt_remd_snf(x[0], 360.0); - absx = std::abs(c_x); + b_x = rt_remd_snf(x[0], 360.0); + absx = std::abs(b_x); if (absx > 180.0) { - if (c_x > 0.0) { - c_x -= 360.0; + if (b_x > 0.0) { + b_x -= 360.0; } else { - c_x += 360.0; + b_x += 360.0; } - absx = std::abs(c_x); + absx = std::abs(b_x); } if (absx <= 45.0) { - c_x *= 0.017453292519943295; + b_x *= 0.017453292519943295; n = 0; } else if (absx <= 135.0) { - if (c_x > 0.0) { - c_x = (c_x - 90.0) * 0.017453292519943295; + if (b_x > 0.0) { + b_x = (b_x - 90.0) * 0.017453292519943295; n = 1; } else { - c_x = (c_x + 90.0) * 0.017453292519943295; + b_x = (b_x + 90.0) * 0.017453292519943295; n = -1; } - } else if (c_x > 0.0) { - c_x = (c_x - 180.0) * 0.017453292519943295; + } else if (b_x > 0.0) { + b_x = (b_x - 180.0) * 0.017453292519943295; n = 2; } else { - c_x = (c_x + 180.0) * 0.017453292519943295; + b_x = (b_x + 180.0) * 0.017453292519943295; n = -2; } switch (n) { case 0: - c_x = std::sin(c_x); + x[0] = std::cos(b_x); break; case 1: - c_x = std::cos(c_x); + x[0] = -std::sin(b_x); break; case -1: - c_x = -std::cos(c_x); + x[0] = std::sin(b_x); break; default: - c_x = -std::sin(c_x); + x[0] = -std::cos(b_x); break; } } - x[0] = c_x; if (rtIsInf(x[1]) || rtIsNaN(x[1])) { - c_x = (rtNaN); + x[1] = (rtNaN); } else { - c_x = rt_remd_snf(x[1], 360.0); - absx = std::abs(c_x); + b_x = rt_remd_snf(x[1], 360.0); + absx = std::abs(b_x); if (absx > 180.0) { - if (c_x > 0.0) { - c_x -= 360.0; + if (b_x > 0.0) { + b_x -= 360.0; } else { - c_x += 360.0; + b_x += 360.0; } - absx = std::abs(c_x); + absx = std::abs(b_x); } if (absx <= 45.0) { - c_x *= 0.017453292519943295; + b_x *= 0.017453292519943295; n = 0; } else if (absx <= 135.0) { - if (c_x > 0.0) { - c_x = (c_x - 90.0) * 0.017453292519943295; + if (b_x > 0.0) { + b_x = (b_x - 90.0) * 0.017453292519943295; n = 1; } else { - c_x = (c_x + 90.0) * 0.017453292519943295; + b_x = (b_x + 90.0) * 0.017453292519943295; n = -1; } - } else if (c_x > 0.0) { - c_x = (c_x - 180.0) * 0.017453292519943295; + } else if (b_x > 0.0) { + b_x = (b_x - 180.0) * 0.017453292519943295; n = 2; } else { - c_x = (c_x + 180.0) * 0.017453292519943295; + b_x = (b_x + 180.0) * 0.017453292519943295; n = -2; } switch (n) { case 0: - c_x = std::sin(c_x); + x[1] = std::cos(b_x); break; case 1: - c_x = std::cos(c_x); + x[1] = -std::sin(b_x); break; case -1: - c_x = -std::cos(c_x); + x[1] = std::sin(b_x); break; default: - c_x = -std::sin(c_x); + x[1] = -std::cos(b_x); break; } } - x[1] = c_x; if (rtIsInf(x[2]) || rtIsNaN(x[2])) { - c_x = (rtNaN); + x[2] = (rtNaN); } else { - c_x = rt_remd_snf(x[2], 360.0); - absx = std::abs(c_x); + b_x = rt_remd_snf(x[2], 360.0); + absx = std::abs(b_x); if (absx > 180.0) { - if (c_x > 0.0) { - c_x -= 360.0; + if (b_x > 0.0) { + b_x -= 360.0; } else { - c_x += 360.0; + b_x += 360.0; } - absx = std::abs(c_x); + absx = std::abs(b_x); } if (absx <= 45.0) { - c_x *= 0.017453292519943295; + b_x *= 0.017453292519943295; n = 0; } else if (absx <= 135.0) { - if (c_x > 0.0) { - c_x = (c_x - 90.0) * 0.017453292519943295; + if (b_x > 0.0) { + b_x = (b_x - 90.0) * 0.017453292519943295; n = 1; } else { - c_x = (c_x + 90.0) * 0.017453292519943295; + b_x = (b_x + 90.0) * 0.017453292519943295; n = -1; } - } else if (c_x > 0.0) { - c_x = (c_x - 180.0) * 0.017453292519943295; + } else if (b_x > 0.0) { + b_x = (b_x - 180.0) * 0.017453292519943295; n = 2; } else { - c_x = (c_x + 180.0) * 0.017453292519943295; + b_x = (b_x + 180.0) * 0.017453292519943295; n = -2; } switch (n) { case 0: - c_x = std::sin(c_x); + x[2] = std::cos(b_x); break; case 1: - c_x = std::cos(c_x); + x[2] = -std::sin(b_x); break; case -1: - c_x = -std::cos(c_x); + x[2] = std::sin(b_x); break; default: - c_x = -std::sin(c_x); + x[2] = -std::cos(b_x); break; } } - - x[2] = c_x; } -// Function for MATLAB Function: '/motors2quat' -real_T wrist_decouplerModelClass::det(const real_T x[9]) +// Function for MATLAB Function: '/motors2ypr' +void wrist_decoupler::sind(real_T x[3]) { - real_T A[9]; - real_T b_y; - real_T smax; - real_T y; - int32_T c; - int32_T c_ix; - int32_T c_k; - int32_T d; - int32_T ijA; - int32_T ix; - int32_T iy; - int32_T j; - int8_T ipiv[3]; - boolean_T isodd; - std::memcpy(&A[0], &x[0], 9U * sizeof(real_T)); - ipiv[0] = 1; - ipiv[1] = 2; - for (j = 0; j < 2; j++) { - c = j << 2; - iy = 0; - ix = c; - smax = std::abs(A[c]); - for (c_k = 2; c_k <= 3 - j; c_k++) { - ix++; - b_y = std::abs(A[ix]); - if (b_y > smax) { - iy = c_k - 1; - smax = b_y; - } - } - - if (A[c + iy] != 0.0) { - if (iy != 0) { - iy += j; - ipiv[j] = static_cast(iy + 1); - smax = A[j]; - A[j] = A[iy]; - A[iy] = smax; - smax = A[j + 3]; - A[j + 3] = A[iy + 3]; - A[iy + 3] = smax; - smax = A[j + 6]; - A[j + 6] = A[iy + 6]; - A[iy + 6] = smax; + real_T absx; + real_T b_x; + int8_T n; + if (rtIsInf(x[0]) || rtIsNaN(x[0])) { + x[0] = (rtNaN); + } else { + b_x = rt_remd_snf(x[0], 360.0); + absx = std::abs(b_x); + if (absx > 180.0) { + if (b_x > 0.0) { + b_x -= 360.0; + } else { + b_x += 360.0; } - iy = (c - j) + 3; - for (ix = c + 1; ix < iy; ix++) { - A[ix] /= A[c]; - } + absx = std::abs(b_x); } - iy = c; - ix = c + 3; - for (c_k = 0; c_k <= 1 - j; c_k++) { - if (A[ix] != 0.0) { - smax = -A[ix]; - c_ix = c + 1; - d = (iy - j) + 6; - for (ijA = iy + 4; ijA < d; ijA++) { - A[ijA] += A[c_ix] * smax; - c_ix++; - } + if (absx <= 45.0) { + b_x *= 0.017453292519943295; + n = 0; + } else if (absx <= 135.0) { + if (b_x > 0.0) { + b_x = (b_x - 90.0) * 0.017453292519943295; + n = 1; + } else { + b_x = (b_x + 90.0) * 0.017453292519943295; + n = -1; } - - ix += 3; - iy += 3; + } else if (b_x > 0.0) { + b_x = (b_x - 180.0) * 0.017453292519943295; + n = 2; + } else { + b_x = (b_x + 180.0) * 0.017453292519943295; + n = -2; } - } - - isodd = (ipiv[0] > 1); - y = A[0] * A[4] * A[8]; - if (ipiv[1] > 2) { - isodd = !isodd; - } - - if (isodd) { - y = -y; - } - return y; -} + switch (n) { + case 0: + x[0] = std::sin(b_x); + break; -// Function for MATLAB Function: '/motors2quat' -void wrist_decouplerModelClass::mldivide(const real_T A[9], const real_T B_0[3], - real_T Y[3]) -{ - real_T b_A[9]; - real_T maxval; - real_T y; - int32_T r1; - int32_T r2; - int32_T r3; - int32_T rtemp; - std::memcpy(&b_A[0], &A[0], 9U * sizeof(real_T)); - r1 = 0; - r2 = 1; - r3 = 2; - maxval = std::abs(A[0]); - y = std::abs(A[1]); - if (y > maxval) { - maxval = y; - r1 = 1; - r2 = 0; - } + case 1: + x[0] = std::cos(b_x); + break; - if (std::abs(A[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } + case -1: + x[0] = -std::cos(b_x); + break; - b_A[r2] = A[r2] / A[r1]; - b_A[r3] /= b_A[r1]; - b_A[r2 + 3] -= b_A[r1 + 3] * b_A[r2]; - b_A[r3 + 3] -= b_A[r1 + 3] * b_A[r3]; - b_A[r2 + 6] -= b_A[r1 + 6] * b_A[r2]; - b_A[r3 + 6] -= b_A[r1 + 6] * b_A[r3]; - if (std::abs(b_A[r3 + 3]) > std::abs(b_A[r2 + 3])) { - rtemp = r2; - r2 = r3; - r3 = rtemp; + default: + x[0] = -std::sin(b_x); + break; + } } - b_A[r3 + 3] /= b_A[r2 + 3]; - b_A[r3 + 6] -= b_A[r3 + 3] * b_A[r2 + 6]; - Y[1] = B_0[r2] - B_0[r1] * b_A[r2]; - Y[2] = (B_0[r3] - B_0[r1] * b_A[r3]) - b_A[r3 + 3] * Y[1]; - Y[2] /= b_A[r3 + 6]; - Y[0] = B_0[r1] - b_A[r1 + 6] * Y[2]; - Y[1] -= b_A[r2 + 6] * Y[2]; - Y[1] /= b_A[r2 + 3]; - Y[0] -= b_A[r1 + 3] * Y[1]; - Y[0] /= b_A[r1]; -} + if (rtIsInf(x[1]) || rtIsNaN(x[1])) { + x[1] = (rtNaN); + } else { + b_x = rt_remd_snf(x[1], 360.0); + absx = std::abs(b_x); + if (absx > 180.0) { + if (b_x > 0.0) { + b_x -= 360.0; + } else { + b_x += 360.0; + } -// Model step function -void wrist_decouplerModelClass::step() -{ - real_T oa[12]; - real_T up[12]; - real_T arg_0[9]; - real_T p[9]; - real_T q[9]; - real_T arg[3]; - real_T ct[3]; - real_T st[3]; - real_T absx; - real_T b_oc_idx_0; - real_T b_oc_idx_1; - real_T b_oc_idx_2; - real_T c_oc; - real_T oa_idx_0; - real_T od; - real_T rtb_Product1; - real_T rtb_Product2; - real_T rtb_Product3; - real_T st_0; - real_T theta; - int32_T b_cycles; - int32_T b_k; - int32_T exitg2; - int32_T idx; - int32_T q_tmp; - int8_T b_n; - boolean_T exitg1; - boolean_T guard1 = false; + absx = std::abs(b_x); + } - // Outputs for Atomic SubSystem: '/wrist_decoupler' - // Outport: '/theta_star' incorporates: - // MATLAB Function: '/ypr2motors' + if (absx <= 45.0) { + b_x *= 0.017453292519943295; + n = 0; + } else if (absx <= 135.0) { + if (b_x > 0.0) { + b_x = (b_x - 90.0) * 0.017453292519943295; + n = 1; + } else { + b_x = (b_x + 90.0) * 0.017453292519943295; + n = -1; + } + } else if (b_x > 0.0) { + b_x = (b_x - 180.0) * 0.017453292519943295; + n = 2; + } else { + b_x = (b_x + 180.0) * 0.017453292519943295; + n = -2; + } - // : theta = [0;0;0]; - // : q = Q*eul2rotm(deg2rad(ypr'))'; - rtY.theta_star[0] = 0.0; + switch (n) { + case 0: + x[1] = std::sin(b_x); + break; - // MATLAB Function: '/ypr2motors' incorporates: - // Inport: '/ypr_star' + case 1: + x[1] = std::cos(b_x); + break; - st_0 = 0.017453292519943295 * rtU.ypr[0]; - ct[0] = std::cos(st_0); - st[0] = std::sin(st_0); + case -1: + x[1] = -std::cos(b_x); + break; - // Outport: '/theta_star' incorporates: - // MATLAB Function: '/ypr2motors' + default: + x[1] = -std::sin(b_x); + break; + } + } - rtY.theta_star[1] = 0.0; + if (rtIsInf(x[2]) || rtIsNaN(x[2])) { + x[2] = (rtNaN); + } else { + b_x = rt_remd_snf(x[2], 360.0); + absx = std::abs(b_x); + if (absx > 180.0) { + if (b_x > 0.0) { + b_x -= 360.0; + } else { + b_x += 360.0; + } - // MATLAB Function: '/ypr2motors' incorporates: - // Inport: '/ypr_star' + absx = std::abs(b_x); + } - st_0 = 0.017453292519943295 * rtU.ypr[1]; - ct[1] = std::cos(st_0); - st[1] = std::sin(st_0); + if (absx <= 45.0) { + b_x *= 0.017453292519943295; + n = 0; + } else if (absx <= 135.0) { + if (b_x > 0.0) { + b_x = (b_x - 90.0) * 0.017453292519943295; + n = 1; + } else { + b_x = (b_x + 90.0) * 0.017453292519943295; + n = -1; + } + } else if (b_x > 0.0) { + b_x = (b_x - 180.0) * 0.017453292519943295; + n = 2; + } else { + b_x = (b_x + 180.0) * 0.017453292519943295; + n = -2; + } - // Outport: '/theta_star' incorporates: - // MATLAB Function: '/ypr2motors' + switch (n) { + case 0: + x[2] = std::sin(b_x); + break; - rtY.theta_star[2] = 0.0; + case 1: + x[2] = std::cos(b_x); + break; - // MATLAB Function: '/ypr2motors' incorporates: - // Inport: '/ypr_star' + case -1: + x[2] = -std::cos(b_x); + break; - st_0 = 0.017453292519943295 * rtU.ypr[2]; - c_oc = std::cos(st_0); - st_0 = std::sin(st_0); - p[0] = ct[1] * ct[0]; - rtb_Product3 = st_0 * st[1]; - p[3] = rtb_Product3 * ct[0] - c_oc * st[0]; - absx = c_oc * st[1]; - p[6] = absx * ct[0] + st_0 * st[0]; - p[1] = ct[1] * st[0]; - p[4] = rtb_Product3 * st[0] + c_oc * ct[0]; - p[7] = absx * st[0] - st_0 * ct[0]; - p[2] = -st[1]; - p[5] = st_0 * ct[1]; - p[8] = c_oc * ct[1]; - for (idx = 0; idx < 3; idx++) { - for (b_k = 0; b_k < 3; b_k++) { - q_tmp = idx + 3 * b_k; - q[q_tmp] = 0.0; - q[q_tmp] += rtConstP.pooled4[idx] * p[b_k]; - q[q_tmp] += rtConstP.pooled4[idx + 3] * p[b_k + 3]; - q[q_tmp] += rtConstP.pooled4[idx + 6] * p[b_k + 6]; + default: + x[2] = -std::sin(b_x); + break; } } +} - // : m = [sqrt(q(1,1)^2+q(1,2)^2) sqrt(q(2,1)^2+q(2,2)^2) sqrt(q(3,1)^2+q(3,2)^2)]; - absx = std::sqrt(q[0] * q[0] + q[3] * q[3]); - c_oc = std::sqrt(q[1] * q[1] + q[4] * q[4]); - b_oc_idx_1 = std::sqrt(q[2] * q[2] + q[5] * q[5]); - - // : if min(abs(m)) == 0 - st[0] = std::abs(absx); - st[1] = std::abs(c_oc); - st[2] = std::abs(b_oc_idx_1); - if (!rtIsNaN(st[0])) { +// Function for MATLAB Function: '/motors2ypr' +real_T wrist_decoupler::maximum(const real_T x[3]) +{ + real_T ex; + int32_T idx; + int32_T k; + if (!rtIsNaN(x[0])) { idx = 1; } else { + boolean_T exitg1; idx = 0; - b_k = 2; + k = 2; exitg1 = false; - while ((!exitg1) && (b_k < 4)) { - if (!rtIsNaN(st[b_k - 1])) { - idx = b_k; + while ((!exitg1) && (k < 4)) { + if (!rtIsNaN(x[k - 1])) { + idx = k; exitg1 = true; } else { - b_k++; + k++; } } } if (idx == 0) { - rtb_Product3 = st[0]; + ex = x[0]; } else { - rtb_Product3 = st[idx - 1]; - while (idx + 1 <= 3) { - if (rtb_Product3 > st[idx]) { - rtb_Product3 = st[idx]; + ex = x[idx - 1]; + for (k = idx + 1; k < 4; k++) { + real_T x_0; + x_0 = x[k - 1]; + if (ex < x_0) { + ex = x_0; } + } + } + + return ex; +} - idx++; +// Function for MATLAB Function: '/motors2ypr' +real_T wrist_decoupler::minimum(const real_T x[3]) +{ + real_T ex; + int32_T idx; + int32_T k; + if (!rtIsNaN(x[0])) { + idx = 1; + } else { + boolean_T exitg1; + idx = 0; + k = 2; + exitg1 = false; + while ((!exitg1) && (k < 4)) { + if (!rtIsNaN(x[k - 1])) { + idx = k; + exitg1 = true; + } else { + k++; + } } } - if (rtb_Product3 == 0.0) { - // Outport: '/out_of_range' - // : out_of_range = 1; - rtY.out_of_range = 1.0; + if (idx == 0) { + ex = x[0]; } else { - // : arg = [(PQ(1)-Pz(1)*q(1,3))/m(1); (PQ(2)-Pz(2)*q(2,3))/m(2); (PQ(3)-Pz(3)*q(3,3))/m(3)]; - arg[0] = (0.5 - -0.57735026918962573 * q[6]) / absx; - arg[1] = (0.087155742747658166 - -0.57735026918962573 * q[7]) / c_oc; - arg[2] = (-0.49999999999999994 - -0.57735026918962573 * q[8]) / b_oc_idx_1; + ex = x[idx - 1]; + for (k = idx + 1; k < 4; k++) { + real_T x_0; + x_0 = x[k - 1]; + if (ex > x_0) { + ex = x_0; + } + } + } - // : if max(abs(arg)) > 1 - st[0] = std::abs(arg[0]); - st[1] = std::abs(arg[1]); - st[2] = std::abs(arg[2]); - if (!rtIsNaN(st[0])) { - idx = 1; + return ex; +} + +real_T rt_atan2d_snf(real_T u0, real_T u1) +{ + real_T y; + if (rtIsNaN(u0) || rtIsNaN(u1)) { + y = (rtNaN); + } else if (rtIsInf(u0) && rtIsInf(u1)) { + int32_T tmp; + int32_T tmp_0; + if (u0 > 0.0) { + tmp = 1; } else { - idx = 0; - b_k = 2; - exitg1 = false; - while ((!exitg1) && (b_k < 4)) { - if (!rtIsNaN(st[b_k - 1])) { - idx = b_k; - exitg1 = true; - } else { - b_k++; - } - } + tmp = -1; } - if (idx == 0) { - rtb_Product3 = st[0]; + if (u1 > 0.0) { + tmp_0 = 1; } else { - rtb_Product3 = st[idx - 1]; - while (idx + 1 <= 3) { - if (rtb_Product3 < st[idx]) { - rtb_Product3 = st[idx]; - } + tmp_0 = -1; + } + + y = std::atan2(static_cast(tmp), static_cast(tmp_0)); + } else if (u1 == 0.0) { + if (u0 > 0.0) { + y = RT_PI / 2.0; + } else if (u0 < 0.0) { + y = -(RT_PI / 2.0); + } else { + y = 0.0; + } + } else { + y = std::atan2(u0, u1); + } + + return y; +} - idx++; +// Function for MATLAB Function: '/motors2ypr' +real_T wrist_decoupler::det(const real_T x[9]) +{ + real_T A[9]; + real_T y; + int8_T ipiv[3]; + boolean_T isodd; + std::memcpy(&A[0], &x[0], 9U * sizeof(real_T)); + ipiv[0] = 1; + ipiv[1] = 2; + for (int32_T j = 0; j < 2; j++) { + real_T smax; + int32_T b_ix; + int32_T iy; + int32_T jj; + jj = j << 2; + iy = 3 - j; + b_ix = 0; + smax = std::abs(A[jj]); + for (int32_T c_k = 2; c_k <= iy; c_k++) { + real_T s; + s = std::abs(A[(jj + c_k) - 1]); + if (s > smax) { + b_ix = c_k - 1; + smax = s; } } - if (rtb_Product3 > 1.0) { - // Outport: '/out_of_range' - // : out_of_range = 1; - rtY.out_of_range = 1.0; - } else { - // Outport: '/theta_star' - // : theta = atan2d(q(:,2),q(:,1))+[90; 90; 90;]-asind(arg); - rtY.theta_star[0] = (57.295779513082323 * rt_atan2d_snf(q[3], q[0]) + 90.0) - - 57.295779513082323 * std::asin(arg[0]); - rtY.theta_star[1] = (57.295779513082323 * rt_atan2d_snf(q[4], q[1]) + 90.0) - - 57.295779513082323 * std::asin(arg[1]); - rtY.theta_star[2] = (57.295779513082323 * rt_atan2d_snf(q[5], q[2]) + 90.0) - - 57.295779513082323 * std::asin(arg[2]); + if (A[jj + b_ix] != 0.0) { + if (b_ix != 0) { + iy = j + b_ix; + ipiv[j] = static_cast(iy + 1); + smax = A[j]; + A[j] = A[iy]; + A[iy] = smax; + smax = A[j + 3]; + A[j + 3] = A[iy + 3]; + A[iy + 3] = smax; + smax = A[j + 6]; + A[j + 6] = A[iy + 6]; + A[iy + 6] = smax; + } - // Outport: '/out_of_range' - // : out_of_range = 0; - rtY.out_of_range = 0.0; + iy = (jj - j) + 3; + for (b_ix = jj + 2; b_ix <= iy; b_ix++) { + A[b_ix - 1] /= A[jj]; + } + } + + iy = 1 - j; + b_ix = jj + 5; + for (int32_T c_k = 0; c_k <= iy; c_k++) { + smax = A[(c_k * 3 + jj) + 3]; + if (smax != 0.0) { + int32_T c; + c = (b_ix - j) + 1; + for (int32_T ijA = b_ix; ijA <= c; ijA++) { + A[ijA - 1] += A[((jj + ijA) - b_ix) + 1] * -smax; + } + } + + b_ix += 3; } } - // Outputs for Atomic SubSystem: '/motors2ypr' - // MATLAB Function: '/motors2quat' incorporates: - // Inport: '/theta_meas' + isodd = (ipiv[0] > 1); + y = A[0] * A[4] * A[8]; + if (ipiv[1] > 2) { + isodd = !isodd; + } + + if (isodd) { + y = -y; + } + + return y; +} + +// Model step function +void wrist_decoupler::step() +{ + real_T up[12]; + real_T P_0[9]; + real_T b[9]; + real_T ya_0[9]; + real_T xa[3]; + real_T xb[3]; + real_T ya[3]; + real_T yb[3]; + real_T absx; + real_T b_n; + real_T maxval; + real_T rtb_Gain_o_idx_0; + real_T rtb_Gain_o_idx_1; + real_T rtb_Gain_o_idx_2; + real_T rtb_Product1; + real_T rtb_attitude_idx_0; + real_T st; + real_T theta; + real_T tmp; + real_T tmp_0; + real_T up_tmp; + real_T up_tmp_0; + real_T up_tmp_1; + real_T up_tmp_2; + real_T up_tmp_3; + real_T up_tmp_4; + real_T yc_idx_2; + int32_T cycles; + int32_T exitg1; + int32_T idx; + int32_T r1; + int32_T r2; + int32_T r3; + int32_T rtemp; + int8_T n; + boolean_T exitg2; + boolean_T guard1 = false; + + // Outputs for Atomic SubSystem: '/wrist_decoupler' + // MATLAB Function: '/motors2ypr' incorporates: + // Inport: '/arm_bend' + // Inport: '/plat_off' + // Inport: '/theta_diff' + // Inport: '/theta_off' + + rtb_Gain_o_idx_0 = rtU.theta_diff[0]; + rtb_Gain_o_idx_1 = rtU.theta_diff[1]; + rtb_Gain_o_idx_2 = rtU.theta_diff[2]; - // : if isempty(T) // : if isempty(singularity_reg) - // : theta_diff = theta_meas - [145; -90; 40]; - // : for k=1:3 - // : if max(abs(theta_diff)) < 5.0 - arg[0] = rtU.theta_meas[0] - 145.0; + if (!rtDW.singularity_reg_not_empty) { + // : Pz = -tand(arm_bend)*[1; 1; 1]; + if (rtIsInf(rtU.arm_bend) || rtIsNaN(rtU.arm_bend)) { + maxval = (rtNaN); + } else { + maxval = rt_remd_snf(rtU.arm_bend, 360.0); + absx = std::abs(maxval); + if (absx > 180.0) { + if (maxval > 0.0) { + maxval -= 360.0; + } else { + maxval += 360.0; + } - // : while theta_diff(k) > 180.0 - while (arg[0] > 180.0) { - // : theta_diff(k) = theta_diff(k) - 360.0; - arg[0] -= 360.0; + absx = std::abs(maxval); + } + + if (absx <= 45.0) { + maxval *= 0.017453292519943295; + n = 0; + } else if (absx <= 135.0) { + if (maxval > 0.0) { + maxval = (maxval - 90.0) * 0.017453292519943295; + n = 1; + } else { + maxval = (maxval + 90.0) * 0.017453292519943295; + n = -1; + } + } else if (maxval > 0.0) { + maxval = (maxval - 180.0) * 0.017453292519943295; + n = 2; + } else { + maxval = (maxval + 180.0) * 0.017453292519943295; + n = -2; + } + + maxval = std::tan(maxval); + if ((n == 1) || (n == -1)) { + absx = 1.0 / maxval; + maxval = -(1.0 / maxval); + if (rtIsInf(maxval) && (n == 1)) { + maxval = absx; + } + } + } + + // : P = [cosd(theta_off) sind(theta_off) Pz]; + rtDW.Pz_b[0] = -maxval; + yb[0] = rtU.theta_off[0]; + rtDW.Pz_b[1] = -maxval; + yb[1] = rtU.theta_off[1]; + rtDW.Pz_b[2] = -maxval; + yb[2] = rtU.theta_off[2]; + cosd(yb); + ya[0] = rtU.theta_off[0]; + ya[1] = rtU.theta_off[1]; + ya[2] = rtU.theta_off[2]; + sind(ya); + + // : Q = [cosd(plat_off) sind(plat_off) [0; 0; 0]]; + P_0[0] = yb[0]; + P_0[3] = ya[0]; + yb[0] = rtU.plat_off[0]; + P_0[1] = yb[1]; + P_0[4] = ya[1]; + yb[1] = rtU.plat_off[1]; + P_0[2] = yb[2]; + P_0[5] = ya[2]; + yb[2] = rtU.plat_off[2]; + cosd(yb); + ya[0] = rtU.plat_off[0]; + ya[1] = rtU.plat_off[1]; + ya[2] = rtU.plat_off[2]; + sind(ya); + rtDW.Q_p[0] = yb[0]; + rtDW.Q_p[3] = ya[0]; + rtDW.Q_p[6] = 0.0; + rtDW.Q_p[1] = yb[1]; + rtDW.Q_p[4] = ya[1]; + rtDW.Q_p[7] = 0.0; + rtDW.Q_p[2] = yb[2]; + rtDW.Q_p[5] = ya[2]; + rtDW.Q_p[8] = 0.0; + + // : PQ = [P(1,:)*Q(1,:)' P(2,:)*Q(2,:)' P(3,:)*Q(3,:)']; + rtDW.PQ_f[0] = (P_0[0] * rtDW.Q_p[0] + P_0[3] * rtDW.Q_p[3]) + rtDW.Pz_b[0] * + 0.0; + rtDW.PQ_f[1] = (P_0[1] * rtDW.Q_p[1] + P_0[4] * rtDW.Q_p[4]) + rtDW.Pz_b[1] * + 0.0; + rtDW.PQ_f[2] = (P_0[2] * rtDW.Q_p[2] + P_0[5] * rtDW.Q_p[5]) + rtDW.Pz_b[2] * + 0.0; + + // : T = quaternion(1,0,0,0); + rtDW.T.a = 1.0; + rtDW.T.b = 0.0; + rtDW.T.c = 0.0; + rtDW.T.d = 0.0; + + // : singularity_reg = 0; + rtDW.singularity_reg_not_empty = true; + } + + // : theta_diff = Wrap180(theta_diff); + // : y = x; + // : for i=1:3 + // : if max(abs(theta_diff)) < 5.0 + // : while y(i) > 180.0 + while (rtb_Gain_o_idx_0 > 180.0) { + // : y(i) = y(i) - 360.0; + rtb_Gain_o_idx_0 -= 360.0; } - // : while theta_diff(k) < -180.0 - while (arg[0] < -180.0) { - // : theta_diff(k) = theta_diff(k) + 360.0; - arg[0] += 360.0; + // : while y(i) < -180.0 + while (rtb_Gain_o_idx_0 < -180.0) { + // : y(i) = y(i) + 360.0; + rtb_Gain_o_idx_0 += 360.0; } - st[0] = std::abs(arg[0]); - arg[1] = rtU.theta_meas[1] - -90.0; + xa[0] = std::abs(rtb_Gain_o_idx_0); - // : while theta_diff(k) > 180.0 - while (arg[1] > 180.0) { - // : theta_diff(k) = theta_diff(k) - 360.0; - arg[1] -= 360.0; + // : while y(i) > 180.0 + while (rtb_Gain_o_idx_1 > 180.0) { + // : y(i) = y(i) - 360.0; + rtb_Gain_o_idx_1 -= 360.0; } - // : while theta_diff(k) < -180.0 - while (arg[1] < -180.0) { - // : theta_diff(k) = theta_diff(k) + 360.0; - arg[1] += 360.0; + // : while y(i) < -180.0 + while (rtb_Gain_o_idx_1 < -180.0) { + // : y(i) = y(i) + 360.0; + rtb_Gain_o_idx_1 += 360.0; } - st[1] = std::abs(arg[1]); - arg[2] = rtU.theta_meas[2] - 40.0; + xa[1] = std::abs(rtb_Gain_o_idx_1); - // : while theta_diff(k) > 180.0 - while (arg[2] > 180.0) { - // : theta_diff(k) = theta_diff(k) - 360.0; - arg[2] -= 360.0; + // : while y(i) > 180.0 + while (rtb_Gain_o_idx_2 > 180.0) { + // : y(i) = y(i) - 360.0; + rtb_Gain_o_idx_2 -= 360.0; } - // : while theta_diff(k) < -180.0 - while (arg[2] < -180.0) { - // : theta_diff(k) = theta_diff(k) + 360.0; - arg[2] += 360.0; + // : while y(i) < -180.0 + while (rtb_Gain_o_idx_2 < -180.0) { + // : y(i) = y(i) + 360.0; + rtb_Gain_o_idx_2 += 360.0; } - st[2] = std::abs(arg[2]); - if (maximum(st) < 5.0) { + xa[2] = std::abs(rtb_Gain_o_idx_2); + if (maximum(xa) < 5.0) { // : singularity_reg = 0; rtDW.singularity_reg = 0.0; } @@ -941,466 +1005,346 @@ void wrist_decouplerModelClass::step() // : if singularity_reg if (rtDW.singularity_reg != 0.0) { // : attitude = compact(T); - absx = rtDW.T.a; - theta = rtDW.T.b; - b_oc_idx_1 = rtDW.T.c; - c_oc = rtDW.T.d; + rtb_attitude_idx_0 = rtDW.T.a; + yc_idx_2 = rtDW.T.b; + b_n = rtDW.T.c; + st = rtDW.T.d; // Outport: '/singularity' // : singularity = singularity_reg; rtY.singularity = rtDW.singularity_reg; // : cycles = 0; - idx = 0; + cycles = 0; } else { // : for cycles=1:200 - idx = 1; - b_cycles = 0; + cycles = 1; + idx = 0; guard1 = false; do { - exitg2 = 0; - if (b_cycles < 200) { - idx = b_cycles + 1; + exitg1 = 0; + if (idx < 200) { + cycles = idx + 1; // : q = rotatepoint(T,Q); theta = std::sqrt(((rtDW.T.a * rtDW.T.a + rtDW.T.b * rtDW.T.b) + rtDW.T.c * rtDW.T.c) + rtDW.T.d * rtDW.T.d); - rtb_Product3 = rtDW.T.a / theta; - absx = rtDW.T.b / theta; - rtb_Product1 = rtDW.T.c / theta; - rtb_Product2 = rtDW.T.d / theta; + rtb_attitude_idx_0 = rtDW.T.a / theta; + yc_idx_2 = rtDW.T.b / theta; + b_n = rtDW.T.c / theta; + st = rtDW.T.d / theta; up[0] = 0.0; up[1] = 0.0; up[2] = 0.0; - std::memcpy(&up[3], &rtConstP.pooled4[0], 9U * sizeof(real_T)); - oa_idx_0 = ((rtb_Product3 * up[0] - absx * up[3]) - rtb_Product1 * up[6]) - - rtb_Product2 * up[9]; - c_oc = ((rtb_Product3 * up[3] + absx * up[0]) + rtb_Product1 * up[9]) - - rtb_Product2 * up[6]; - st_0 = ((rtb_Product3 * up[6] - absx * up[9]) + rtb_Product1 * up[0]) + - rtb_Product2 * up[3]; - od = ((rtb_Product3 * up[9] + absx * up[6]) - rtb_Product1 * up[3]) + - rtb_Product2 * up[0]; - oa[0] = ((oa_idx_0 * rtb_Product3 - c_oc * -absx) - st_0 * -rtb_Product1) - - od * -rtb_Product2; - st[0] = ((oa_idx_0 * -absx + c_oc * rtb_Product3) + st_0 * -rtb_Product2) - - od * -rtb_Product1; - b_oc_idx_0 = ((oa_idx_0 * -rtb_Product1 - c_oc * -rtb_Product2) + st_0 * - rtb_Product3) + od * -absx; - arg[0] = ((oa_idx_0 * -rtb_Product2 + c_oc * -rtb_Product1) - st_0 * - -absx) + od * rtb_Product3; - oa_idx_0 = ((rtb_Product3 * up[1] - absx * up[4]) - rtb_Product1 * up[7]) - - rtb_Product2 * up[10]; - c_oc = ((rtb_Product3 * up[4] + absx * up[1]) + rtb_Product1 * up[10]) - - rtb_Product2 * up[7]; - st_0 = ((rtb_Product3 * up[7] - absx * up[10]) + rtb_Product1 * up[1]) + - rtb_Product2 * up[4]; - od = ((rtb_Product3 * up[10] + absx * up[7]) - rtb_Product1 * up[4]) + - rtb_Product2 * up[1]; - oa[1] = ((oa_idx_0 * rtb_Product3 - c_oc * -absx) - st_0 * -rtb_Product1) - - od * -rtb_Product2; - st[1] = ((oa_idx_0 * -absx + c_oc * rtb_Product3) + st_0 * -rtb_Product2) - - od * -rtb_Product1; - b_oc_idx_1 = ((oa_idx_0 * -rtb_Product1 - c_oc * -rtb_Product2) + st_0 * - rtb_Product3) + od * -absx; - arg[1] = ((oa_idx_0 * -rtb_Product2 + c_oc * -rtb_Product1) - st_0 * - -absx) + od * rtb_Product3; - oa_idx_0 = ((rtb_Product3 * up[2] - absx * up[5]) - rtb_Product1 * up[8]) - - rtb_Product2 * up[11]; - c_oc = ((rtb_Product3 * up[5] + absx * up[2]) + rtb_Product1 * up[11]) - - rtb_Product2 * up[8]; - st_0 = ((rtb_Product3 * up[8] - absx * up[11]) + rtb_Product1 * up[2]) + - rtb_Product2 * up[5]; - od = ((rtb_Product3 * up[11] + absx * up[8]) - rtb_Product1 * up[5]) + - rtb_Product2 * up[2]; - oa[2] = ((oa_idx_0 * rtb_Product3 - c_oc * -absx) - st_0 * -rtb_Product1) - - od * -rtb_Product2; - st[2] = ((oa_idx_0 * -absx + c_oc * rtb_Product3) + st_0 * -rtb_Product2) - - od * -rtb_Product1; - b_oc_idx_2 = ((oa_idx_0 * -rtb_Product1 - c_oc * -rtb_Product2) + st_0 * - rtb_Product3) + od * -absx; - oa_idx_0 = ((oa_idx_0 * -rtb_Product2 + c_oc * -rtb_Product1) - st_0 * - -absx) + od * rtb_Product3; - up[3] = st[0]; - up[9] = arg[0]; - oa[3] = st[0]; - oa[6] = b_oc_idx_0; - oa[9] = arg[0]; - up[4] = st[1]; - up[10] = arg[1]; - oa[4] = st[1]; - oa[7] = b_oc_idx_1; - oa[10] = arg[1]; - st_0 = st[2]; - oa[5] = st[2]; - oa[8] = b_oc_idx_2; - oa[11] = oa_idx_0; + std::memcpy(&up[3], &rtDW.Q_p[0], 9U * sizeof(real_T)); + yb[0] = ((rtb_attitude_idx_0 * up[0] - yc_idx_2 * up[3]) - b_n * up[6]) + - st * up[9]; + ya[0] = ((rtb_attitude_idx_0 * up[3] + yc_idx_2 * up[0]) + b_n * up[9]) + - st * up[6]; + xa[0] = ((rtb_attitude_idx_0 * up[6] - yc_idx_2 * up[9]) + b_n * up[0]) + + st * up[3]; + xb[0] = ((rtb_attitude_idx_0 * up[9] + yc_idx_2 * up[6]) - b_n * up[3]) + + st * up[0]; + yb[1] = ((rtb_attitude_idx_0 * up[1] - yc_idx_2 * up[4]) - b_n * up[7]) + - st * up[10]; + ya[1] = ((rtb_attitude_idx_0 * up[4] + yc_idx_2 * up[1]) + b_n * up[10]) + - st * up[7]; + xa[1] = ((rtb_attitude_idx_0 * up[7] - yc_idx_2 * up[10]) + b_n * up[1]) + + st * up[4]; + xb[1] = ((rtb_attitude_idx_0 * up[10] + yc_idx_2 * up[7]) - b_n * up[4]) + + st * up[1]; + yb[2] = ((rtb_attitude_idx_0 * up[2] - yc_idx_2 * up[5]) - b_n * up[8]) + - st * up[11]; + ya[2] = ((rtb_attitude_idx_0 * up[5] + yc_idx_2 * up[2]) + b_n * up[11]) + - st * up[8]; + xa[2] = ((rtb_attitude_idx_0 * up[8] - yc_idx_2 * up[11]) + b_n * up[2]) + + st * up[5]; + xb[2] = ((rtb_attitude_idx_0 * up[11] + yc_idx_2 * up[8]) - b_n * up[5]) + + st * up[2]; + up_tmp = ((yb[0] * -yc_idx_2 + ya[0] * rtb_attitude_idx_0) + xa[0] * -st) + - xb[0] * -b_n; + up_tmp_0 = ((yb[0] * -b_n - ya[0] * -st) + xa[0] * rtb_attitude_idx_0) + + xb[0] * -yc_idx_2; + up_tmp_1 = ((yb[0] * -st + ya[0] * -b_n) - xa[0] * -yc_idx_2) + xb[0] * + rtb_attitude_idx_0; + up[0] = ((yb[0] * rtb_attitude_idx_0 - ya[0] * -yc_idx_2) - xa[0] * -b_n) + - xb[0] * -st; + up[3] = up_tmp; + up[6] = up_tmp_0; + up[9] = up_tmp_1; + up_tmp_2 = ((yb[1] * -yc_idx_2 + ya[1] * rtb_attitude_idx_0) + xa[1] * + -st) - xb[1] * -b_n; + up_tmp_3 = ((yb[1] * -b_n - ya[1] * -st) + xa[1] * rtb_attitude_idx_0) + + xb[1] * -yc_idx_2; + up_tmp_4 = ((yb[1] * -st + ya[1] * -b_n) - xa[1] * -yc_idx_2) + xb[1] * + rtb_attitude_idx_0; + up[1] = ((yb[1] * rtb_attitude_idx_0 - ya[1] * -yc_idx_2) - xa[1] * -b_n) + - xb[1] * -st; + up[4] = up_tmp_2; + up[7] = up_tmp_3; + up[10] = up_tmp_4; + theta = ((yb[2] * -yc_idx_2 + ya[2] * rtb_attitude_idx_0) + xa[2] * -st) + - xb[2] * -b_n; + rtb_Product1 = ((yb[2] * -b_n - ya[2] * -st) + xa[2] * + rtb_attitude_idx_0) + xb[2] * -yc_idx_2; + maxval = ((yb[2] * -st + ya[2] * -b_n) - xa[2] * -yc_idx_2) + xb[2] * + rtb_attitude_idx_0; + up[2] = ((yb[2] * rtb_attitude_idx_0 - ya[2] * -yc_idx_2) - xa[2] * -b_n) + - xb[2] * -st; + up[5] = theta; + up[8] = rtb_Product1; + up[11] = maxval; + for (r1 = 0; r1 < 3; r1++) { + r2 = (r1 + 1) * 3; + P_0[3 * r1] = up[r2]; + P_0[3 * r1 + 1] = up[r2 + 1]; + P_0[3 * r1 + 2] = up[r2 + 2]; + } // : m = [sqrt(q(1,1)^2+q(1,2)^2) sqrt(q(2,1)^2+q(2,2)^2) sqrt(q(3,1)^2+q(3,2)^2)]; - c_oc = b_oc_idx_0 * b_oc_idx_0; - arg[0] = std::sqrt(st[0] * st[0] + c_oc); - b_oc_idx_1 *= b_oc_idx_1; - arg[1] = std::sqrt(st[1] * st[1] + b_oc_idx_1); - arg[2] = std::sqrt(st[2] * st[2] + b_oc_idx_2 * b_oc_idx_2); + absx = std::sqrt(P_0[0] * P_0[0] + P_0[3] * P_0[3]); + tmp = std::sqrt(P_0[1] * P_0[1] + P_0[4] * P_0[4]); + tmp_0 = std::sqrt(P_0[2] * P_0[2] + P_0[5] * P_0[5]); // : if min(abs(m)) == 0 - for (b_k = 0; b_k < 3; b_k++) { - q_tmp = (b_k + 1) * 3; - q[3 * b_k] = oa[q_tmp]; - q[3 * b_k + 1] = oa[q_tmp + 1]; - q[3 * b_k + 2] = oa[q_tmp + 2]; - st[b_k] = std::abs(arg[b_k]); - } - - if (minimum(st) == 0.0) { + xa[0] = absx; + xa[1] = tmp; + xa[2] = tmp_0; + if (minimum(xa) == 0.0) { // : cycles = -1; - idx = -1; + cycles = -1; guard1 = true; - exitg2 = 1; + exitg1 = 1; } else { // : arg = [(PQ(1)-Pz(1)*q(1,3))/m(1); (PQ(2)-Pz(2)*q(2,3))/m(2); (PQ(3)-Pz(3)*q(3,3))/m(3)]; - ct[0] = (0.5 - -0.57735026918962573 * up[9]) / std::sqrt(up[3] * up[3] - + c_oc); - ct[1] = (0.087155742747658166 - -0.57735026918962573 * up[10]) / std:: - sqrt(up[4] * up[4] + b_oc_idx_1); - ct[2] = (-0.49999999999999994 - -0.57735026918962573 * oa_idx_0) / std:: - sqrt(st_0 * st_0 + b_oc_idx_2 * b_oc_idx_2); + yb[0] = (rtDW.PQ_f[0] - rtDW.Pz_b[0] * up_tmp_1) / absx; + yb[1] = (rtDW.PQ_f[1] - rtDW.Pz_b[1] * up_tmp_4) / tmp; + yb[2] = (rtDW.PQ_f[2] - rtDW.Pz_b[2] * maxval) / tmp_0; // : if max(abs(arg)) > 1.0 - st[0] = std::abs(ct[0]); - st[1] = std::abs(ct[1]); - st[2] = std::abs(ct[2]); - if (maximum(st) > 1.0) { + xa[0] = std::abs(yb[0]); + xa[1] = std::abs(yb[1]); + xa[2] = std::abs(yb[2]); + if (maximum(xa) > 1.0) { // : cycles = -2; - idx = -2; + cycles = -2; guard1 = true; - exitg2 = 1; + exitg1 = 1; } else { // : theta = atan2d(q(:,2),q(:,1))+[90; 90; 90;]-asind(arg); - // : theta_err = theta_meas-theta; - // : for k=1:3 + // : theta_err = Wrap180(theta_diff+theta_off-theta); + // : y = x; + // : for i=1:3 // : if max(abs(theta_err)) < 0.01 - ct[0] = 57.295779513082323 * std::asin(ct[0]); - arg[0] = (57.295779513082323 * rt_atan2d_snf(q[3], q[0]) + 90.0) - - ct[0]; - - // : while theta_err(k) > 180.0 - for (rtb_Product1 = rtU.theta_meas[0] - arg[0]; rtb_Product1 > 180.0; - rtb_Product1 -= 360.0) { - // : theta_err(k) = theta_err(k) - 360.0; + absx = (57.295779513082323 * rt_atan2d_snf(P_0[3], P_0[0]) + 90.0) - + 57.295779513082323 * std::asin(yb[0]); + ya[0] = absx; + xb[0] = (rtb_Gain_o_idx_0 + rtU.theta_off[0]) - absx; + + // : while y(i) > 180.0 + while (xb[0] > 180.0) { + // : y(i) = y(i) - 360.0; + xb[0] -= 360.0; } - // : while theta_err(k) < -180.0 - while (rtb_Product1 < -180.0) { - // : theta_err(k) = theta_err(k) + 360.0; - rtb_Product1 += 360.0; + // : while y(i) < -180.0 + while (xb[0] < -180.0) { + // : y(i) = y(i) + 360.0; + xb[0] += 360.0; } - st[0] = std::abs(rtb_Product1); - arg[1] = 57.295779513082323 * rt_atan2d_snf(q[4], q[1]); - ct[1] = 57.295779513082323 * std::asin(ct[1]); - arg[1] = (arg[1] + 90.0) - ct[1]; + xa[0] = std::abs(xb[0]); + absx = (57.295779513082323 * rt_atan2d_snf(P_0[4], P_0[1]) + 90.0) - + 57.295779513082323 * std::asin(yb[1]); + ya[1] = absx; + xb[1] = (rtb_Gain_o_idx_1 + rtU.theta_off[1]) - absx; - // : while theta_err(k) > 180.0 - for (rtb_Product2 = rtU.theta_meas[1] - arg[1]; rtb_Product2 > 180.0; - rtb_Product2 -= 360.0) { - // : theta_err(k) = theta_err(k) - 360.0; + // : while y(i) > 180.0 + while (xb[1] > 180.0) { + // : y(i) = y(i) - 360.0; + xb[1] -= 360.0; } - // : while theta_err(k) < -180.0 - while (rtb_Product2 < -180.0) { - // : theta_err(k) = theta_err(k) + 360.0; - rtb_Product2 += 360.0; + // : while y(i) < -180.0 + while (xb[1] < -180.0) { + // : y(i) = y(i) + 360.0; + xb[1] += 360.0; } - st[1] = std::abs(rtb_Product2); - arg[2] = 57.295779513082323 * rt_atan2d_snf(q[5], q[2]); - ct[2] = 57.295779513082323 * std::asin(ct[2]); - arg[2] = (arg[2] + 90.0) - ct[2]; + xa[1] = std::abs(xb[1]); + absx = (57.295779513082323 * rt_atan2d_snf(P_0[5], P_0[2]) + 90.0) - + 57.295779513082323 * std::asin(yb[2]); + ya[2] = absx; + xb[2] = (rtb_Gain_o_idx_2 + rtU.theta_off[2]) - absx; - // : while theta_err(k) > 180.0 - for (b_oc_idx_2 = rtU.theta_meas[2] - arg[2]; b_oc_idx_2 > 180.0; - b_oc_idx_2 -= 360.0) { - // : theta_err(k) = theta_err(k) - 360.0; + // : while y(i) > 180.0 + while (xb[2] > 180.0) { + // : y(i) = y(i) - 360.0; + xb[2] -= 360.0; } - // : while theta_err(k) < -180.0 - while (b_oc_idx_2 < -180.0) { - // : theta_err(k) = theta_err(k) + 360.0; - b_oc_idx_2 += 360.0; + // : while y(i) < -180.0 + while (xb[2] < -180.0) { + // : y(i) = y(i) + 360.0; + xb[2] += 360.0; } - st[2] = std::abs(b_oc_idx_2); - if (maximum(st) < 0.01) { + xa[2] = std::abs(xb[2]); + if (maximum(xa) < 0.01) { // : T = normalize(T); - rtDW.T.a /= theta; - rtDW.T.b /= theta; - rtDW.T.c /= theta; - rtDW.T.d /= theta; + rtDW.T.a = rtb_attitude_idx_0; + rtDW.T.b = yc_idx_2; + rtDW.T.c = b_n; + rtDW.T.d = st; // : attitude = compact(T); - absx = rtDW.T.a; - theta = rtDW.T.b; - b_oc_idx_1 = rtDW.T.c; - c_oc = rtDW.T.d; - // : singularity = singularity_reg; rtY.singularity = rtDW.singularity_reg; - exitg2 = 1; + exitg1 = 1; } else { // : p = [cosd(theta) sind(theta) Pz]; - if (rtIsInf(arg[0]) || rtIsNaN(arg[0])) { - oa_idx_0 = (rtNaN); - } else { - rtb_Product3 = rt_remd_snf(arg[0], 360.0); - absx = std::abs(rtb_Product3); - if (absx > 180.0) { - if (rtb_Product3 > 0.0) { - rtb_Product3 -= 360.0; - } else { - rtb_Product3 += 360.0; - } - - absx = std::abs(rtb_Product3); - } - - if (absx <= 45.0) { - rtb_Product3 *= 0.017453292519943295; - b_n = 0; - } else if (absx <= 135.0) { - if (rtb_Product3 > 0.0) { - rtb_Product3 = (rtb_Product3 - 90.0) * 0.017453292519943295; - b_n = 1; - } else { - rtb_Product3 = (rtb_Product3 + 90.0) * 0.017453292519943295; - b_n = -1; - } - } else if (rtb_Product3 > 0.0) { - rtb_Product3 = (rtb_Product3 - 180.0) * 0.017453292519943295; - b_n = 2; - } else { - rtb_Product3 = (rtb_Product3 + 180.0) * 0.017453292519943295; - b_n = -2; - } - - switch (b_n) { - case 0: - oa_idx_0 = std::cos(rtb_Product3); - break; - - case 1: - oa_idx_0 = -std::sin(rtb_Product3); - break; - - case -1: - oa_idx_0 = std::sin(rtb_Product3); - break; - - default: - oa_idx_0 = -std::cos(rtb_Product3); - break; - } - } - - if (rtIsInf(arg[1]) || rtIsNaN(arg[1])) { - theta = (rtNaN); - } else { - rtb_Product3 = rt_remd_snf(arg[1], 360.0); - absx = std::abs(rtb_Product3); - if (absx > 180.0) { - if (rtb_Product3 > 0.0) { - rtb_Product3 -= 360.0; - } else { - rtb_Product3 += 360.0; - } - - absx = std::abs(rtb_Product3); - } - - if (absx <= 45.0) { - rtb_Product3 *= 0.017453292519943295; - b_n = 0; - } else if (absx <= 135.0) { - if (rtb_Product3 > 0.0) { - rtb_Product3 = (rtb_Product3 - 90.0) * 0.017453292519943295; - b_n = 1; - } else { - rtb_Product3 = (rtb_Product3 + 90.0) * 0.017453292519943295; - b_n = -1; - } - } else if (rtb_Product3 > 0.0) { - rtb_Product3 = (rtb_Product3 - 180.0) * 0.017453292519943295; - b_n = 2; - } else { - rtb_Product3 = (rtb_Product3 + 180.0) * 0.017453292519943295; - b_n = -2; - } - - switch (b_n) { - case 0: - theta = std::cos(rtb_Product3); - break; - - case 1: - theta = -std::sin(rtb_Product3); - break; - - case -1: - theta = std::sin(rtb_Product3); - break; - - default: - theta = -std::cos(rtb_Product3); - break; - } - } - - if (rtIsInf(arg[2]) || rtIsNaN(arg[2])) { - rtb_Product3 = (rtNaN); - } else { - rtb_Product3 = rt_remd_snf(arg[2], 360.0); - absx = std::abs(rtb_Product3); - if (absx > 180.0) { - if (rtb_Product3 > 0.0) { - rtb_Product3 -= 360.0; - } else { - rtb_Product3 += 360.0; - } - - absx = std::abs(rtb_Product3); - } - - if (absx <= 45.0) { - rtb_Product3 *= 0.017453292519943295; - b_n = 0; - } else if (absx <= 135.0) { - if (rtb_Product3 > 0.0) { - rtb_Product3 = (rtb_Product3 - 90.0) * 0.017453292519943295; - b_n = 1; - } else { - rtb_Product3 = (rtb_Product3 + 90.0) * 0.017453292519943295; - b_n = -1; - } - } else if (rtb_Product3 > 0.0) { - rtb_Product3 = (rtb_Product3 - 180.0) * 0.017453292519943295; - b_n = 2; - } else { - rtb_Product3 = (rtb_Product3 + 180.0) * 0.017453292519943295; - b_n = -2; - } - - switch (b_n) { - case 0: - rtb_Product3 = std::cos(rtb_Product3); - break; - - case 1: - rtb_Product3 = -std::sin(rtb_Product3); - break; - - case -1: - rtb_Product3 = std::sin(rtb_Product3); - break; - - default: - rtb_Product3 = -std::cos(rtb_Product3); - break; - } - } - - sind(arg); - p[3] = arg[0]; - p[4] = arg[1]; - p[5] = arg[2]; + yb[0] = ya[0]; + yb[1] = ya[1]; + yb[2] = absx; + cosd(yb); + sind(ya); + P_0[3] = ya[0]; + P_0[1] = yb[1]; + P_0[4] = ya[1]; + P_0[2] = yb[2]; + P_0[5] = ya[2]; // : q1xp1 = cross(q(1,:),p(1,:)); - arg[2] = q[0] * p[3] - q[3] * oa_idx_0; + ya[0] = rtDW.Pz_b[0] * up_tmp_0 - P_0[3] * up_tmp_1; + ya[1] = yb[0] * up_tmp_1 - rtDW.Pz_b[0] * up_tmp; + ya[2] = up_tmp * P_0[3] - yb[0] * up_tmp_0; // : q2xp2 = cross(q(2,:),p(2,:)); - ct[2] = q[1] * p[4] - q[4] * theta; + yb[0] = rtDW.Pz_b[1] * up_tmp_3 - P_0[4] * up_tmp_4; + yb[1] = P_0[1] * up_tmp_4 - rtDW.Pz_b[1] * up_tmp_2; + yb[2] = up_tmp_2 * P_0[4] - P_0[1] * up_tmp_3; // : q3xp3 = cross(q(3,:),p(3,:)); - absx = q[2] * p[5] - q[5] * rtb_Product3; + yc_idx_2 = theta * P_0[5] - P_0[2] * rtb_Product1; // : if min(abs([q1xp1(3) q2xp2(3) q3xp3(3)])) == 0.0 - st[0] = std::abs(arg[2]); - st[1] = std::abs(ct[2]); - st[2] = std::abs(absx); - if (minimum(st) == 0.0) { + xa[0] = std::abs(ya[2]); + xa[1] = std::abs(yb[2]); + xa[2] = std::abs(yc_idx_2); + if (minimum(xa) == 0.0) { // : cycles = -3; - idx = -3; + cycles = -3; guard1 = true; - exitg2 = 1; + exitg1 = 1; } else { // : if det([q1xp1/q1xp1(3); q2xp2/q2xp2(3); q3xp3/q3xp3(3)]) == 0.0 - c_oc = (q[3] * -0.57735026918962573 - q[6] * p[3]) / arg[2]; - arg_0[0] = c_oc; - b_oc_idx_1 = (q[4] * -0.57735026918962573 - q[7] * p[4]) / ct[2]; - arg_0[1] = b_oc_idx_1; - st_0 = (q[5] * -0.57735026918962573 - q[8] * p[5]) / absx; - arg_0[2] = st_0; - oa_idx_0 = (q[6] * oa_idx_0 - q[0] * -0.57735026918962573) / - arg[2]; - arg_0[3] = oa_idx_0; - theta = (q[7] * theta - q[1] * -0.57735026918962573) / ct[2]; - arg_0[4] = theta; - rtb_Product3 = (q[8] * rtb_Product3 - q[2] * - -0.57735026918962573) / absx; - arg_0[5] = rtb_Product3; - b_oc_idx_0 = arg[2] / arg[2]; - arg_0[6] = b_oc_idx_0; - od = ct[2] / ct[2]; - arg_0[7] = od; - absx /= absx; - arg_0[8] = absx; - if (det(arg_0) == 0.0) { + st = ya[0] / ya[2]; + ya_0[0] = st; + absx = yb[0] / yb[2]; + ya_0[1] = absx; + b_n = (rtDW.Pz_b[2] * rtb_Product1 - P_0[5] * maxval) / yc_idx_2; + ya_0[2] = b_n; + rtb_Product1 = ya[1] / ya[2]; + ya_0[3] = rtb_Product1; + rtb_attitude_idx_0 = yb[1] / yb[2]; + ya_0[4] = rtb_attitude_idx_0; + theta = (P_0[2] * maxval - rtDW.Pz_b[2] * theta) / yc_idx_2; + ya_0[5] = theta; + maxval = ya[2] / ya[2]; + ya_0[6] = maxval; + up_tmp = yb[2] / yb[2]; + ya_0[7] = up_tmp; + yc_idx_2 /= yc_idx_2; + ya_0[8] = yc_idx_2; + if (det(ya_0) == 0.0) { // : cycles = -4; - idx = -4; + cycles = -4; guard1 = true; - exitg2 = 1; + exitg1 = 1; } else { // : T = quaternion(([q1xp1/q1xp1(3); q2xp2/q2xp2(3); q3xp3/q3xp3(3)]\(0.02*theta_err))','rotvec')*T; - arg_0[0] = c_oc; - arg_0[1] = b_oc_idx_1; - arg_0[2] = st_0; - st[0] = 0.02 * rtb_Product1; - arg_0[3] = oa_idx_0; - arg_0[4] = theta; - arg_0[5] = rtb_Product3; - st[1] = 0.02 * rtb_Product2; - arg_0[6] = b_oc_idx_0; - arg_0[7] = od; - arg_0[8] = absx; - st[2] = 0.02 * b_oc_idx_2; - mldivide(arg_0, st, ct); - rtb_Product3 = 1.0; + xb[0] *= 0.02; + P_0[0] = st; + P_0[1] = absx; + P_0[2] = b_n; + xb[1] *= 0.02; + P_0[3] = rtb_Product1; + P_0[4] = rtb_attitude_idx_0; + P_0[5] = theta; + xb[2] *= 0.02; + P_0[6] = maxval; + P_0[7] = up_tmp; + P_0[8] = yc_idx_2; + r1 = 0; + r2 = 1; + r3 = 2; + maxval = std::abs(st); + absx = std::abs(absx); + if (absx > maxval) { + maxval = absx; + r1 = 1; + r2 = 0; + } + + if (std::abs(b_n) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + P_0[r2] /= P_0[r1]; + P_0[r3] /= P_0[r1]; + P_0[r2 + 3] -= P_0[r1 + 3] * P_0[r2]; + P_0[r3 + 3] -= P_0[r1 + 3] * P_0[r3]; + P_0[r2 + 6] -= P_0[r1 + 6] * P_0[r2]; + P_0[r3 + 6] -= P_0[r1 + 6] * P_0[r3]; + if (std::abs(P_0[r3 + 3]) > std::abs(P_0[r2 + 3])) { + rtemp = r2; + r2 = r3; + r3 = rtemp; + } + + P_0[r3 + 3] /= P_0[r2 + 3]; + P_0[r3 + 6] -= P_0[r3 + 3] * P_0[r2 + 6]; + yb[1] = xb[r2] - xb[r1] * P_0[r2]; + yb[2] = (xb[r3] - xb[r1] * P_0[r3]) - P_0[r3 + 3] * yb[1]; + yb[2] /= P_0[r3 + 6]; + yb[0] = xb[r1] - P_0[r1 + 6] * yb[2]; + yb[1] -= P_0[r2 + 6] * yb[2]; + yb[1] /= P_0[r2 + 3]; + yb[0] -= P_0[r1 + 3] * yb[1]; + yb[0] /= P_0[r1]; + maxval = 1.0; absx = 0.0; rtb_Product1 = 0.0; - rtb_Product2 = 0.0; - theta = std::sqrt((ct[0] * ct[0] + ct[1] * ct[1]) + ct[2] * - ct[2]); - st_0 = std::sin(theta / 2.0); + b_n = 0.0; + ya[0] = yb[0] * yb[0]; + ya[1] = yb[1] * yb[1]; + ya[2] = yb[2] * yb[2]; + theta = std::sqrt((ya[0] + ya[1]) + ya[2]); + st = std::sin(theta / 2.0); if (theta != 0.0) { - ct[0] = ct[0] / theta * st_0; - ct[1] = ct[1] / theta * st_0; - rtb_Product3 = std::cos(theta / 2.0); - absx = ct[0]; - rtb_Product1 = ct[1]; - rtb_Product2 = ct[2] / theta * st_0; + yb[0] = yb[0] / theta * st; + yb[1] = yb[1] / theta * st; + yb[2] = yb[2] / theta * st; + maxval = std::cos(theta / 2.0); + absx = yb[0]; + rtb_Product1 = yb[1]; + b_n = yb[2]; } - theta = ((rtb_Product3 * rtDW.T.a - absx * rtDW.T.b) - - rtb_Product1 * rtDW.T.c) - rtb_Product2 * rtDW.T.d; - st_0 = ((rtb_Product3 * rtDW.T.b + absx * rtDW.T.a) + - rtb_Product1 * rtDW.T.d) - rtb_Product2 * rtDW.T.c; - c_oc = ((rtb_Product3 * rtDW.T.c - absx * rtDW.T.d) + - rtb_Product1 * rtDW.T.a) + rtb_Product2 * rtDW.T.b; - rtDW.T.d = ((rtb_Product3 * rtDW.T.d + absx * rtDW.T.c) - - rtb_Product1 * rtDW.T.b) + rtb_Product2 * rtDW.T.a; - rtDW.T.a = theta; - rtDW.T.b = st_0; - rtDW.T.c = c_oc; - b_cycles++; + theta = rtDW.T.a; + st = rtDW.T.b; + yc_idx_2 = rtDW.T.c; + rtDW.T.a = ((maxval * rtDW.T.a - absx * rtDW.T.b) - + rtb_Product1 * rtDW.T.c) - b_n * rtDW.T.d; + rtDW.T.b = ((maxval * rtDW.T.b + absx * theta) + rtb_Product1 * + rtDW.T.d) - b_n * rtDW.T.c; + rtDW.T.c = ((maxval * rtDW.T.c - absx * rtDW.T.d) + + rtb_Product1 * theta) + b_n * st; + rtDW.T.d = ((maxval * rtDW.T.d + absx * yc_idx_2) - + rtb_Product1 * st) + b_n * theta; + idx++; guard1 = false; } } @@ -1409,9 +1353,9 @@ void wrist_decouplerModelClass::step() } } else { guard1 = true; - exitg2 = 1; + exitg1 = 1; } - } while (exitg2 == 0); + } while (exitg1 == 0); if (guard1) { // : T = quaternion(1,0,0,0); @@ -1421,16 +1365,16 @@ void wrist_decouplerModelClass::step() rtDW.T.d = 0.0; // : attitude = compact(T); - absx = rtDW.T.a; - theta = rtDW.T.b; - b_oc_idx_1 = rtDW.T.c; - c_oc = rtDW.T.d; + rtb_attitude_idx_0 = rtDW.T.a; + yc_idx_2 = rtDW.T.b; + b_n = rtDW.T.b; + st = rtDW.T.b; // : singularity_reg = 1; rtDW.singularity_reg = 1.0; // : singularity = singularity_reg; - rtY.singularity = rtDW.singularity_reg; + rtY.singularity = 1.0; } } @@ -1441,31 +1385,40 @@ void wrist_decouplerModelClass::step() // Product: '/Product3' // Sum: '/Sum' - rtb_Product3 = std::sqrt(((absx * absx + theta * theta) + b_oc_idx_1 * - b_oc_idx_1) + c_oc * c_oc); + maxval = std::sqrt(((rtb_attitude_idx_0 * rtb_attitude_idx_0 + yc_idx_2 * + yc_idx_2) + b_n * b_n) + st * st); // Product: '/Product' - absx /= rtb_Product3; + absx = rtb_attitude_idx_0 / maxval; // Product: '/Product1' - rtb_Product1 = theta / rtb_Product3; + rtb_Product1 = yc_idx_2 / maxval; // Product: '/Product2' - rtb_Product2 = b_oc_idx_1 / rtb_Product3; + b_n /= maxval; // Product: '/Product3' - rtb_Product3 = c_oc / rtb_Product3; + maxval = st / maxval; + + // Fcn: '/fcn3' + theta = (rtb_Product1 * maxval - absx * b_n) * -2.0; - // Fcn: '/fcn2' incorporates: - // Fcn: '/fcn5' + // Fcn: '/fcn2' incorporates: + // Fcn: '/fcn5' - c_oc = absx * absx; - b_oc_idx_1 = rtb_Product1 * rtb_Product1; - st_0 = rtb_Product2 * rtb_Product2; - oa_idx_0 = rtb_Product3 * rtb_Product3; + rtb_Gain_o_idx_2 = absx * absx; + rtb_Gain_o_idx_1 = rtb_Product1 * rtb_Product1; + st = b_n * b_n; + yc_idx_2 = maxval * maxval; - // Fcn: '/fcn3' - theta = (rtb_Product1 * rtb_Product3 - absx * rtb_Product2) * -2.0; + // Gain: '/Gain' incorporates: + // Fcn: '/fcn1' + // Fcn: '/fcn2' + // Trigonometry: '/Trigonometric Function1' + + rtb_Gain_o_idx_0 = rt_atan2d_snf((rtb_Product1 * b_n + absx * maxval) * 2.0, + ((rtb_Gain_o_idx_2 + rtb_Gain_o_idx_1) - st) - yc_idx_2) * + 57.295779513082323; // If: '/If' incorporates: // Constant: '/Constant' @@ -1478,70 +1431,451 @@ void wrist_decouplerModelClass::step() theta = 1.0; // End of Outputs for SubSystem: '/If Action Subsystem' + } else if (theta < -1.0) { + // Outputs for IfAction SubSystem: '/If Action Subsystem1' incorporates: + // ActionPort: '/Action Port' + + theta = 1.0; + + // End of Outputs for SubSystem: '/If Action Subsystem1' + } + + // Gain: '/Gain' incorporates: + // Fcn: '/fcn4' + // Fcn: '/fcn5' + // Trigonometry: '/Trigonometric Function3' + + rtb_Gain_o_idx_2 = rt_atan2d_snf((b_n * maxval + absx * rtb_Product1) * 2.0, + ((rtb_Gain_o_idx_2 - rtb_Gain_o_idx_1) - st) + yc_idx_2) * + 57.295779513082323; + + // MATLAB Function: '/ypr2motors' incorporates: + // Inport: '/arm_bend' + // Inport: '/plat_off' + // Inport: '/theta_off' + + // : if isempty(Pz) + if (!rtDW.Pz_not_empty) { + // : Pz = -tand(arm_bend)*[1; 1; 1]; + if (rtIsInf(rtU.arm_bend) || rtIsNaN(rtU.arm_bend)) { + maxval = (rtNaN); + } else { + maxval = rt_remd_snf(rtU.arm_bend, 360.0); + absx = std::abs(maxval); + if (absx > 180.0) { + if (maxval > 0.0) { + maxval -= 360.0; + } else { + maxval += 360.0; + } + + absx = std::abs(maxval); + } + + if (absx <= 45.0) { + maxval *= 0.017453292519943295; + n = 0; + } else if (absx <= 135.0) { + if (maxval > 0.0) { + maxval = (maxval - 90.0) * 0.017453292519943295; + n = 1; + } else { + maxval = (maxval + 90.0) * 0.017453292519943295; + n = -1; + } + } else if (maxval > 0.0) { + maxval = (maxval - 180.0) * 0.017453292519943295; + n = 2; + } else { + maxval = (maxval + 180.0) * 0.017453292519943295; + n = -2; + } + + maxval = std::tan(maxval); + if ((n == 1) || (n == -1)) { + absx = 1.0 / maxval; + maxval = -(1.0 / maxval); + if (rtIsInf(maxval) && (n == 1)) { + maxval = absx; + } + } + } + + rtDW.Pz_not_empty = true; + + // : P = [cosd(theta_off) sind(theta_off) Pz]; + rtDW.Pz[0] = -maxval; + yb[0] = rtU.theta_off[0]; + rtDW.Pz[1] = -maxval; + yb[1] = rtU.theta_off[1]; + rtDW.Pz[2] = -maxval; + yb[2] = rtU.theta_off[2]; + cosd(yb); + ya[0] = rtU.theta_off[0]; + ya[1] = rtU.theta_off[1]; + ya[2] = rtU.theta_off[2]; + sind(ya); + P_0[0] = yb[0]; + P_0[3] = ya[0]; + P_0[1] = yb[1]; + P_0[4] = ya[1]; + P_0[2] = yb[2]; + P_0[5] = ya[2]; + + // : Q = [cosd(plat_off) sind(plat_off) [0; 0; 0]]; + yb[0] = rtU.plat_off[0]; + yb[1] = rtU.plat_off[1]; + yb[2] = rtU.plat_off[2]; + cosd(yb); + ya[0] = rtU.plat_off[0]; + ya[1] = rtU.plat_off[1]; + ya[2] = rtU.plat_off[2]; + sind(ya); + rtDW.Q[0] = yb[0]; + rtDW.Q[3] = ya[0]; + rtDW.Q[6] = 0.0; + rtDW.Q[1] = yb[1]; + rtDW.Q[4] = ya[1]; + rtDW.Q[7] = 0.0; + rtDW.Q[2] = yb[2]; + rtDW.Q[5] = ya[2]; + rtDW.Q[8] = 0.0; + + // : PQ = [P(1,:)*Q(1,:)' P(2,:)*Q(2,:)' P(3,:)*Q(3,:)']; + rtDW.PQ[0] = (P_0[0] * rtDW.Q[0] + P_0[3] * rtDW.Q[3]) + rtDW.Pz[0] * 0.0; + rtDW.PQ[1] = (P_0[1] * rtDW.Q[1] + P_0[4] * rtDW.Q[4]) + rtDW.Pz[1] * 0.0; + rtDW.PQ[2] = (P_0[2] * rtDW.Q[2] + P_0[5] * rtDW.Q[5]) + rtDW.Pz[2] * 0.0; + + // : last_theta_ok = [0;0;0]; + } + + // Switch: '/Switch' incorporates: + // Gain: '/Gain' + // Inport: '/RL' + // Inport: '/ypr_star' + + // : q = Q*eul2rotm(deg2rad(ypr'))'; + if (rtU.RL) { + absx = -rtU.ypr_star[0]; } else { - if (theta < -1.0) { - // Outputs for IfAction SubSystem: '/If Action Subsystem1' incorporates: - // ActionPort: '/Action Port' + absx = rtU.ypr_star[0]; + } - theta = 1.0; + // MATLAB Function: '/ypr2motors' incorporates: + // Gain: '/Gain' + // Inport: '/ypr_star' + // Switch: '/Switch' + + absx *= 0.017453292519943295; + rtb_Gain_o_idx_1 = std::cos(absx); + maxval = std::sin(absx); + absx = 0.017453292519943295 * rtU.ypr_star[1]; + b_n = std::cos(absx); + st = std::sin(absx); + + // Switch: '/Switch' incorporates: + // Gain: '/Gain' + // Inport: '/RL' + // Inport: '/ypr_star' - // End of Outputs for SubSystem: '/If Action Subsystem1' + if (rtU.RL) { + absx = -rtU.ypr_star[2]; + } else { + absx = rtU.ypr_star[2]; + } + + // MATLAB Function: '/ypr2motors' incorporates: + // Inport: '/theta_off' + // Switch: '/Switch' + + absx *= 0.017453292519943295; + rtb_Product1 = std::cos(absx); + absx = std::sin(absx); + b[0] = rtb_Gain_o_idx_1 * b_n; + yc_idx_2 = st * absx; + b[3] = yc_idx_2 * rtb_Gain_o_idx_1 - maxval * rtb_Product1; + rtb_attitude_idx_0 = st * rtb_Product1; + b[6] = rtb_attitude_idx_0 * rtb_Gain_o_idx_1 + maxval * absx; + b[1] = maxval * b_n; + b[4] = yc_idx_2 * maxval + rtb_Gain_o_idx_1 * rtb_Product1; + b[7] = rtb_attitude_idx_0 * maxval - rtb_Gain_o_idx_1 * absx; + b[2] = -st; + b[5] = b_n * absx; + b[8] = b_n * rtb_Product1; + for (r1 = 0; r1 < 3; r1++) { + rtb_Gain_o_idx_1 = rtDW.Q[r1 + 3]; + maxval = rtDW.Q[r1]; + absx = rtDW.Q[r1 + 6]; + for (idx = 0; idx < 3; idx++) { + P_0[r1 + 3 * idx] = (b[idx + 3] * rtb_Gain_o_idx_1 + maxval * b[idx]) + + b[idx + 6] * absx; } } - // End of If: '/If' + // : m = [sqrt(q(1,1)^2+q(1,2)^2) sqrt(q(2,1)^2+q(2,2)^2) sqrt(q(3,1)^2+q(3,2)^2)]; + absx = std::sqrt(P_0[0] * P_0[0] + P_0[3] * P_0[3]); + tmp = std::sqrt(P_0[1] * P_0[1] + P_0[4] * P_0[4]); + tmp_0 = std::sqrt(P_0[2] * P_0[2] + P_0[5] * P_0[5]); - // Outport: '/ypr_meas' incorporates: - // Fcn: '/fcn1' - // Fcn: '/fcn2' - // Fcn: '/fcn4' - // Fcn: '/fcn5' - // Gain: '/Gain3' - // Trigonometry: '/Trigonometric Function1' - // Trigonometry: '/Trigonometric Function3' + // : if min(abs(m)) == 0 + ya[0] = absx; + ya[1] = tmp; + ya[2] = tmp_0; + if (minimum(ya) == 0.0) { + // : theta = last_theta_ok; + xb[0] = rtDW.last_theta_ok[0]; + xb[1] = rtDW.last_theta_ok[1]; + xb[2] = rtDW.last_theta_ok[2]; + + // Outport: '/out_of_range' + // : out_of_range = 1; + rtY.out_of_range = 1.0; + } else { + // : arg = [(PQ(1)-Pz(1)*q(1,3))/m(1); (PQ(2)-Pz(2)*q(2,3))/m(2); (PQ(3)-Pz(3)*q(3,3))/m(3)]; + xa[0] = (rtDW.PQ[0] - rtDW.Pz[0] * P_0[6]) / absx; + xa[1] = (rtDW.PQ[1] - rtDW.Pz[1] * P_0[7]) / tmp; + xa[2] = (rtDW.PQ[2] - rtDW.Pz[2] * P_0[8]) / tmp_0; + + // : if max(abs(arg)) > 1 + ya[0] = std::abs(xa[0]); + ya[1] = std::abs(xa[1]); + ya[2] = std::abs(xa[2]); + if (!rtIsNaN(ya[0])) { + idx = 1; + } else { + idx = 0; + r1 = 2; + exitg2 = false; + while ((!exitg2) && (r1 < 4)) { + if (!rtIsNaN(ya[r1 - 1])) { + idx = r1; + exitg2 = true; + } else { + r1++; + } + } + } + + if (idx == 0) { + maxval = ya[0]; + } else { + maxval = ya[idx - 1]; + for (r1 = idx + 1; r1 < 4; r1++) { + absx = ya[r1 - 1]; + if (maxval < absx) { + maxval = absx; + } + } + } + + if (maxval > 1.0) { + // : theta = last_theta_ok; + xb[0] = rtDW.last_theta_ok[0]; + xb[1] = rtDW.last_theta_ok[1]; + xb[2] = rtDW.last_theta_ok[2]; + + // Outport: '/out_of_range' + // : out_of_range = 1; + rtY.out_of_range = 1.0; + } else { + // : theta = atan2d(q(:,2),q(:,1))+[90; 90; 90;]-asind(arg); + rtb_Gain_o_idx_1 = (57.295779513082323 * rt_atan2d_snf(P_0[3], P_0[0]) + + 90.0) - 57.295779513082323 * std::asin(xa[0]); + maxval = (57.295779513082323 * rt_atan2d_snf(P_0[4], P_0[1]) + 90.0) - + 57.295779513082323 * std::asin(xa[1]); + absx = (57.295779513082323 * rt_atan2d_snf(P_0[5], P_0[2]) + 90.0) - + 57.295779513082323 * std::asin(xa[2]); + + // : gap = abs(Wrap180([(theta(2)-theta(1)) (theta(3)-theta(2)) (theta(1)-theta(3))])); + yb[0] = maxval - rtb_Gain_o_idx_1; + yb[1] = absx - maxval; + yb[2] = rtb_Gain_o_idx_1 - absx; + + // : y = x; + // : for i=1:3 + // : while y(i) > 180.0 + while (yb[0] > 180.0) { + // : y(i) = y(i) - 360.0; + yb[0] -= 360.0; + } + + // : while y(i) < -180.0 + while (yb[0] < -180.0) { + // : y(i) = y(i) + 360.0; + yb[0] += 360.0; + } + + ya[0] = std::abs(yb[0]); + + // : while y(i) > 180.0 + while (yb[1] > 180.0) { + // : y(i) = y(i) - 360.0; + yb[1] -= 360.0; + } + + // : while y(i) < -180.0 + while (yb[1] < -180.0) { + // : y(i) = y(i) + 360.0; + yb[1] += 360.0; + } + + ya[1] = std::abs(yb[1]); + + // : while y(i) > 180.0 + while (yb[2] > 180.0) { + // : y(i) = y(i) - 360.0; + yb[2] -= 360.0; + } + + // : while y(i) < -180.0 + while (yb[2] < -180.0) { + // : y(i) = y(i) + 360.0; + yb[2] += 360.0; + } + + ya[2] = std::abs(yb[2]); + + // : if min(gap) < 33.0 + if (minimum(ya) < 33.0) { + // : theta = last_theta_ok; + xb[0] = rtDW.last_theta_ok[0]; + xb[1] = rtDW.last_theta_ok[1]; + xb[2] = rtDW.last_theta_ok[2]; + + // Outport: '/out_of_range' + // : out_of_range = 1; + rtY.out_of_range = 1.0; + } else { + // : theta = Wrap180(theta-theta_off); + // : y = x; + // : for i=1:3 + // : last_theta_ok = theta; + xb[0] = rtb_Gain_o_idx_1 - rtU.theta_off[0]; + + // : while y(i) > 180.0 + while (xb[0] > 180.0) { + // : y(i) = y(i) - 360.0; + xb[0] -= 360.0; + } + + // : while y(i) < -180.0 + while (xb[0] < -180.0) { + // : y(i) = y(i) + 360.0; + xb[0] += 360.0; + } + + rtDW.last_theta_ok[0] = xb[0]; + xb[1] = maxval - rtU.theta_off[1]; + + // : while y(i) > 180.0 + while (xb[1] > 180.0) { + // : y(i) = y(i) - 360.0; + xb[1] -= 360.0; + } + + // : while y(i) < -180.0 + while (xb[1] < -180.0) { + // : y(i) = y(i) + 360.0; + xb[1] += 360.0; + } + + rtDW.last_theta_ok[1] = xb[1]; + xb[2] = absx - rtU.theta_off[2]; + + // : while y(i) > 180.0 + while (xb[2] > 180.0) { + // : y(i) = y(i) - 360.0; + xb[2] -= 360.0; + } + + // : while y(i) < -180.0 + while (xb[2] < -180.0) { + // : y(i) = y(i) + 360.0; + xb[2] += 360.0; + } + + rtDW.last_theta_ok[2] = xb[2]; + + // Outport: '/out_of_range' incorporates: + // Inport: '/theta_off' + + // : out_of_range = 0; + rtY.out_of_range = 0.0; + } + } + } + + // End of Outputs for SubSystem: '/wrist_decoupler' + + // Outport: '/theta_star' + rtY.theta_star[0] = xb[0]; + + // Outputs for Atomic SubSystem: '/wrist_decoupler' + // Switch: '/Switch1' incorporates: + // Inport: '/RL' + + if (rtU.RL) { + // Outport: '/ypr_meas' incorporates: + // Gain: '/Gain1' + + rtY.ypr_meas[0] = -rtb_Gain_o_idx_0; + } else { + // Outport: '/ypr_meas' + rtY.ypr_meas[0] = rtb_Gain_o_idx_0; + } + + // End of Outputs for SubSystem: '/wrist_decoupler' + + // Outport: '/theta_star' + rtY.theta_star[1] = xb[1]; + + // Outputs for Atomic SubSystem: '/wrist_decoupler' + // If: '/If' incorporates: + // Gain: '/Gain' + // Outport: '/ypr_meas' + // Switch: '/Switch1' // Trigonometry: '/trigFcn' - rtY.ypr_meas[0] = 57.295779513082323 * rt_atan2d_snf((rtb_Product1 * - rtb_Product2 + absx * rtb_Product3) * 2.0, ((c_oc + b_oc_idx_1) - st_0) - - oa_idx_0); rtY.ypr_meas[1] = 57.295779513082323 * std::asin(theta); - rtY.ypr_meas[2] = 57.295779513082323 * rt_atan2d_snf((rtb_Product2 * - rtb_Product3 + absx * rtb_Product1) * 2.0, ((c_oc - b_oc_idx_1) - st_0) + - oa_idx_0); + + // End of Outputs for SubSystem: '/wrist_decoupler' + + // Outport: '/theta_star' + rtY.theta_star[2] = xb[2]; + + // Outputs for Atomic SubSystem: '/wrist_decoupler' + // Switch: '/Switch1' incorporates: + // Inport: '/RL' + + if (rtU.RL) { + // Outport: '/ypr_meas' incorporates: + // Gain: '/Gain1' + + rtY.ypr_meas[2] = -rtb_Gain_o_idx_2; + } else { + // Outport: '/ypr_meas' + rtY.ypr_meas[2] = rtb_Gain_o_idx_2; + } // Outport: '/cycles' incorporates: - // MATLAB Function: '/motors2quat' + // MATLAB Function: '/motors2ypr' - rtY.cycles = idx; + rtY.cycles = cycles; - // End of Outputs for SubSystem: '/motors2ypr' // End of Outputs for SubSystem: '/wrist_decoupler' } // Model initialize function -void wrist_decouplerModelClass::initialize() +void wrist_decoupler::initialize() { // Registration code // initialize non-finites rt_InitInfAndNaN(sizeof(real_T)); - - // SystemInitialize for Atomic SubSystem: '/wrist_decoupler' - // SystemInitialize for Atomic SubSystem: '/motors2ypr' - // SystemInitialize for MATLAB Function: '/motors2quat' - // : T = quaternion(1,0,0,0); - rtDW.T.a = 1.0; - rtDW.T.b = 0.0; - rtDW.T.c = 0.0; - rtDW.T.d = 0.0; - - // End of SystemInitialize for SubSystem: '/motors2ypr' - // End of SystemInitialize for SubSystem: '/wrist_decoupler' - // : singularity_reg = 0; } // Constructor -wrist_decouplerModelClass::wrist_decouplerModelClass() : +wrist_decoupler::wrist_decoupler() : rtU(), rtY(), rtDW(), @@ -1551,13 +1885,13 @@ wrist_decouplerModelClass::wrist_decouplerModelClass() : } // Destructor -wrist_decouplerModelClass::~wrist_decouplerModelClass() +wrist_decoupler::~wrist_decoupler() { // Currently there is no destructor body generated. } // Real-Time Model get method -wrist_decouplerModelClass::RT_MODEL * wrist_decouplerModelClass::getRTM() +wrist_decoupler::RT_MODEL * wrist_decoupler::getRTM() { return (&rtM); } diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler.h index 4fccabd8a7..533a1bdace 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler.h @@ -1,15 +1,15 @@ // // Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, educational organizations only. Not for -// government, commercial, or other organizational use. +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. // // File: wrist_decoupler.h // // Code generated for Simulink model 'wrist_decoupler'. // -// Model version : 2.3 -// Simulink Coder version : 9.4 (R2020b) 29-Jul-2020 -// C/C++ source code generated on : Tue Nov 17 12:30:21 2020 +// Model version : 7.13 +// Simulink Coder version : 9.9 (R2023a) 19-Nov-2022 +// C/C++ source code generated on : Tue Jul 25 11:18:40 2023 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,14 +20,8 @@ // #ifndef RTW_HEADER_wrist_decoupler_h_ #define RTW_HEADER_wrist_decoupler_h_ -#include -#include "rtwtypes.h" -#include -#include -#include #include "rtwtypes.h" - -// Model Code Variants +#include // Macros for accessing real-time model data structure #ifndef rtmGetErrorStatus @@ -38,11 +32,11 @@ #define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) #endif -// Custom Type definition for MATLAB Function: '/motors2quat' -#ifndef struct_tag_v7m7NEstuUdktFlQqRQnjB -#define struct_tag_v7m7NEstuUdktFlQqRQnjB +// Custom Type definition for MATLAB Function: '/motors2ypr' +#ifndef struct_quaternion +#define struct_quaternion -struct tag_v7m7NEstuUdktFlQqRQnjB +struct quaternion { real_T a; real_T b; @@ -50,20 +44,17 @@ struct tag_v7m7NEstuUdktFlQqRQnjB real_T d; }; -#endif //struct_tag_v7m7NEstuUdktFlQqRQnjB - -#ifndef typedef_quaternion -#define typedef_quaternion +#endif // struct_quaternion -typedef tag_v7m7NEstuUdktFlQqRQnjB quaternion; - -#endif //typedef_quaternion - -extern "C" { +extern "C" +{ static real_T rtGetNaN(void); static real32_T rtGetNaNF(void); } // extern "C" - extern "C" + +#define NOT_USING_NONFINITE_LITERALS 1 + +extern "C" { extern real_T rtInf; extern real_T rtMinusInf; @@ -76,75 +67,83 @@ extern "C" { static boolean_T rtIsInfF(real32_T value); static boolean_T rtIsNaN(real_T value); static boolean_T rtIsNaNF(real32_T value); - typedef struct { + struct BigEndianIEEEDouble { struct { uint32_T wordH; uint32_T wordL; } words; - } BigEndianIEEEDouble; + }; - typedef struct { + struct LittleEndianIEEEDouble { struct { uint32_T wordL; uint32_T wordH; } words; - } LittleEndianIEEEDouble; + }; - typedef struct { + struct IEEESingle { union { real32_T wordLreal; uint32_T wordLuint; } wordL; - } IEEESingle; + }; } // extern "C" -extern "C" { +extern "C" +{ static real_T rtGetInf(void); static real32_T rtGetInfF(void); static real_T rtGetMinusInf(void); static real32_T rtGetMinusInfF(void); } // extern "C" - // Class declaration for model wrist_decoupler - class wrist_decouplerModelClass + +// Class declaration for model wrist_decoupler +class wrist_decoupler { // public data and function members public: // Block signals and states (default storage) for system '' - typedef struct { - quaternion T; // '/motors2quat' - real_T singularity_reg; // '/motors2quat' - } DW; - - // Constant parameters (default storage) - typedef struct { - // Pooled Parameter (Expression: Q) - // Referenced by: - // '/ypr2motors' - // '/motors2quat' - - real_T pooled4[9]; - } ConstP; + struct DW { + quaternion T; // '/motors2ypr' + real_T Pz[3]; // '/ypr2motors' + real_T PQ[3]; // '/ypr2motors' + real_T Q[9]; // '/ypr2motors' + real_T last_theta_ok[3]; // '/ypr2motors' + real_T Q_p[9]; // '/motors2ypr' + real_T PQ_f[3]; // '/motors2ypr' + real_T Pz_b[3]; // '/motors2ypr' + real_T singularity_reg; // '/motors2ypr' + boolean_T Pz_not_empty; // '/ypr2motors' + boolean_T singularity_reg_not_empty;// '/motors2ypr' + }; // External inputs (root inport signals with default storage) - typedef struct { - real_T theta_meas[3]; // '/theta_meas' - real_T ypr[3]; // '/ypr_star' - } ExtU; + struct ExtU { + real_T theta_diff[3]; // '/theta_diff' + real_T theta_off[3]; // '/theta_off' + real_T plat_off[3]; // '/plat_off' + real_T arm_bend; // '/arm_bend' + real_T ypr_star[3]; // '/ypr_star' + boolean_T RL; // '/RL' + }; // External outputs (root outports fed by signals with default storage) - typedef struct { + struct ExtY { real_T theta_star[3]; // '/theta_star' real_T out_of_range; // '/out_of_range' real_T ypr_meas[3]; // '/ypr_meas' real_T singularity; // '/singularity' real_T cycles; // '/cycles' - } ExtY; + }; // Real-time Model Data Structure struct RT_MODEL { const char_T * volatile errorStatus; }; + // Real-Time Model get method + wrist_decoupler::RT_MODEL * getRTM(); + // External inputs ExtU rtU; @@ -158,34 +157,26 @@ extern "C" { void step(); // Constructor - wrist_decouplerModelClass(); + wrist_decoupler(); // Destructor - ~wrist_decouplerModelClass(); - - // Real-Time Model get method - wrist_decouplerModelClass::RT_MODEL * getRTM(); + ~wrist_decoupler(); // private data and function members private: - // Block signals and states + // Block states DW rtDW; - // Real-Time Model - RT_MODEL rtM; - // private member function(s) for subsystem '' + void cosd(real_T x[3]); + void sind(real_T x[3]); real_T maximum(const real_T x[3]); real_T minimum(const real_T x[3]); - void sind(real_T x[3]); real_T det(const real_T x[9]); - void mldivide(const real_T A[9], const real_T B_0[3], real_T Y[3]); -} -; - -// Constant parameters (default storage) -extern const wrist_decouplerModelClass::ConstP rtConstP; + // Real-Time Model + RT_MODEL rtM; +}; //- // The generated code includes comments that allow you to trace directly @@ -199,25 +190,25 @@ extern const wrist_decouplerModelClass::ConstP rtConstP; // MATLAB hilite_system command to trace the generated code back // to the parent model. For example, // -// hilite_system('ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler') - opens subsystem ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler -// hilite_system('ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/Kp') - opens and selects block Kp +// hilite_system('spherical_wrist/Position_Controller/wrist_decoupler') - opens subsystem spherical_wrist/Position_Controller/wrist_decoupler +// hilite_system('spherical_wrist/Position_Controller/wrist_decoupler/Kp') - opens and selects block Kp // // Here is the system hierarchy for this model // -// '' : 'ok_wrist_simple_motors_3/Position_Controller' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/ypr2motors' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/Quaternions to Rotation Angles1' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/motors2quat' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/Quaternions to Rotation Angles1/Angle Calculation' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/Quaternions to Rotation Angles1/Quaternion Normalize' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/Quaternions to Rotation Angles1/Angle Calculation/Protect asincos input' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/Quaternions to Rotation Angles1/Angle Calculation/Protect asincos input/If Action Subsystem' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/Quaternions to Rotation Angles1/Angle Calculation/Protect asincos input/If Action Subsystem1' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/Quaternions to Rotation Angles1/Angle Calculation/Protect asincos input/If Action Subsystem2' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/Quaternions to Rotation Angles1/Quaternion Normalize/Quaternion Modulus' -// '' : 'ok_wrist_simple_motors_3/Position_Controller/wrist_decoupler/motors2ypr/Quaternions to Rotation Angles1/Quaternion Normalize/Quaternion Modulus/Quaternion Norm' +// '' : 'spherical_wrist/Position_Controller' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Quaternions to Rotation Angles' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Radians to Degrees' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/motors2ypr' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/ypr2motors' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Quaternions to Rotation Angles/Angle Calculation' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Quaternions to Rotation Angles/Quaternion Normalize' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Quaternions to Rotation Angles/Angle Calculation/Protect asincos input' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Quaternions to Rotation Angles/Angle Calculation/Protect asincos input/If Action Subsystem' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Quaternions to Rotation Angles/Angle Calculation/Protect asincos input/If Action Subsystem1' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Quaternions to Rotation Angles/Angle Calculation/Protect asincos input/If Action Subsystem2' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Quaternions to Rotation Angles/Quaternion Normalize/Quaternion Modulus' +// '' : 'spherical_wrist/Position_Controller/wrist_decoupler/Quaternions to Rotation Angles/Quaternion Normalize/Quaternion Modulus/Quaternion Norm' #endif // RTW_HEADER_wrist_decoupler_h_ diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler_data.cpp b/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler_data.cpp deleted file mode 100644 index cb5fb7100a..0000000000 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/wrist_decoupler_data.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// -// Non-Degree Granting Education License -- for use at non-degree -// granting, nonprofit, educational organizations only. Not for -// government, commercial, or other organizational use. -// -// File: wrist_decoupler_data.cpp -// -// Code generated for Simulink model 'wrist_decoupler'. -// -// Model version : 2.3 -// Simulink Coder version : 9.4 (R2020b) 29-Jul-2020 -// C/C++ source code generated on : Tue Nov 17 12:30:21 2020 -// -// Target selection: ert.tlc -// Embedded hardware selection: ARM Compatible->ARM Cortex-M -// Code generation objectives: -// 1. Execution efficiency -// 2. RAM efficiency -// Validation result: Not run -// -#include "wrist_decoupler.h" - -// Constant parameters (default storage) -const wrist_decouplerModelClass::ConstP rtConstP = { - // Pooled Parameter (Expression: Q) - // Referenced by: - // '/ypr2motors' - // '/motors2quat' - - { 0.087155742747658166, -0.99619469809174555, 0.17364817766693033, - 0.99619469809174555, -0.087155742747658166, -0.984807753012208, 0.0, 0.0, - 0.0 } -}; - -// -// File trailer for generated code. -// -// [EOF] -// diff --git a/emBODY/eBcode/arch-arm/embot/app/eth/embot_app_eth_theEncoderReader.cpp b/emBODY/eBcode/arch-arm/embot/app/eth/embot_app_eth_theEncoderReader.cpp index 0a24141346..96126ca870 100644 --- a/emBODY/eBcode/arch-arm/embot/app/eth/embot_app_eth_theEncoderReader.cpp +++ b/emBODY/eBcode/arch-arm/embot/app/eth/embot_app_eth_theEncoderReader.cpp @@ -116,6 +116,8 @@ struct embot::app::eth::theEncoderReader::Impl static bool s_eo_isconnected(eOmc_encoder_descriptor_t *des); static uint32_t rescale2icubdegrees(uint32_t val_raw, uint8_t jomo, eOmc_position_t pos); static bool isValidValue_AEA(const uint32_t &valueraw, embot::app::eth::encoder::v1::Error &error); + static bool isValidValue_AEA3(const uint32_t &valueraw, embot::app::eth::encoder::v1::Error &error); + }; @@ -422,6 +424,36 @@ bool embot::app::eth::theEncoderReader::Impl::Read(uint8_t jomo, embot::app::eth } break; + + case eomc_enc_aea3: + { + embot::hw::encoder::POS spiRawValue = 0; + // hal_spiencoder_errors_flags flags = {0}; + + + // if(hal_res_OK == hal_spiencoder_get_value((hal_spiencoder_t)prop.descriptor->port, &spiRawValue, &flags)) + if(resOK == embot::hw::encoder::getValue(e, spiRawValue/*, &diagn*/)) + { // ok, the hal reads correctly + if(true == isValidValue_AEA3(spiRawValue, prop.valueinfo->errortype)) + { // the spi raw reading from hal is valid. i just need to rescale it. + // the resolution is 16384 ticks per revolution. + + prop.valueinfo->value[0] = rescale2icubdegrees(spiRawValue, jomo, (eOmc_position_t)prop.descriptor->pos); + } + else + { // we have a valid raw value from hal but ... it is not valid after a check + prop.valueinfo->errortype = prop.valueinfo->errortype; + errorparam = spiRawValue; //(spiRawValue >> 6) & 0x0FFF; + } + } + else + { // we dont even have a valid reading from hal + prop.valueinfo->errortype = embot::app::eth::encoder::v1::Error::AEA_READING; + errorparam = 0xffff; + } + + } break; + default: { // we have not recognised any valid encoder type prop.valueinfo->errortype = embot::app::eth::encoder::v1::Error::GENERIC; @@ -567,6 +599,14 @@ bool embot::app::eth::theEncoderReader::Impl::isValidValue_AEA(const uint32_t &v return true; } +bool embot::app::eth::theEncoderReader::Impl::isValidValue_AEA3(const uint32_t &valueraw, embot::app::eth::encoder::v1::Error &error) +{ + error = embot::app::eth::encoder::v1::Error::NONE; + // TODO: there is no way to check the validity when using the AEA3 in SSI mode + return true; +} + + // -------------------------------------------------------------------------------------------------------------------- diff --git a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_chip_MA730.cpp b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_chip_MA730.cpp index a60ec29bdb..306a54392f 100644 --- a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_chip_MA730.cpp +++ b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_chip_MA730.cpp @@ -173,9 +173,17 @@ bool embot::hw::chip::MA730::Impl::read(Data &data, embot::core::relTime timeout if(resOK == embot::hw::spi::read(_config.spi, da, timeout)) { + if((_databuffer[0] == 0xFF) && (_databuffer[1] == 0xFF)) + { + //the spi is not reading + data.status.ok = false; + return r;//false + } + // AEA3 offers 14 bit of resolution (0-16383) // in this way we are reading 16 bits (1 bit dummy + 14 bit valid + 1 bit zero padded). The dummy bit has been masked. - data.position = ((_databuffer[0] & 0x7F) << 7) | (_databuffer[1] >> 1); + data.position = (((uint16_t(_databuffer[0])<<8) | uint16_t(_databuffer[1])) >> 1) & 0x3FFF; + data.status.ok = true; r = true; } @@ -191,8 +199,17 @@ void embot::hw::chip::MA730::Impl::onCompletion(void *p) embot::hw::chip::MA730::Impl *pimpl = reinterpret_cast(p); if(nullptr != pimpl->_tmpdata) { - pimpl->_tmpdata->position = ((pimpl->_databuffer[0] & 0x7F) << 7) | (pimpl->_databuffer[1] >> 1); - pimpl->_tmpdata->status.ok = true; + if((pimpl->_databuffer[0] == 0xFF) && (pimpl->_databuffer[1] == 0xFF)) + { + //the spi is not reading + pimpl->_tmpdata->status.ok = false; + } + else + { + pimpl->_tmpdata->position = (((uint16_t(pimpl->_databuffer[0])<<8) | uint16_t(pimpl->_databuffer[1])) >> 1) & 0x3FFF; + pimpl->_tmpdata->status.ok = true; + //embot::core::print("_databuffer[0]="+ std::to_string(pimpl->_databuffer[0]) + " _databuffer[1]="+std::to_string(pimpl->_databuffer[1])); + } } pimpl->_tmponcompletion.execute(); pimpl->_tmponcompletion.clear(); diff --git a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_chip_MA730.h b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_chip_MA730.h index ac1520264f..c46918d6dd 100644 --- a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_chip_MA730.h +++ b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_chip_MA730.h @@ -84,6 +84,15 @@ namespace embot { namespace hw { namespace chip { } }; + static constexpr embot::hw::spi::Config standardspiconfig + { + embot::hw::spi::Prescaler::sixtyfour, + embot::hw::spi::DataSize::eight, + embot::hw::spi::Mode::two, + { {embot::hw::gpio::Pull::pullup, embot::hw::gpio::Pull::nopull, // | miso | mosi | + embot::hw::gpio::Pull::pulldown, embot::hw::gpio::Pull::pullup} } // | sclk | sel | + }; + MA730(); ~MA730(); diff --git a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_encoder.cpp b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_encoder.cpp index 351a701137..fc7e72fcfe 100644 --- a/emBODY/eBcode/arch-arm/embot/hw/embot_hw_encoder.cpp +++ b/emBODY/eBcode/arch-arm/embot/hw/embot_hw_encoder.cpp @@ -117,11 +117,11 @@ namespace embot { namespace hw { namespace encoder { const embot::hw::encoder::BSP &encoderbsp = embot::hw::encoder::getBSP(); embot::hw::SPI spiBus = encoderbsp.getPROP(e)->spiBus; - // TODO: safe guard (remove it when AEA3 is completely implemented) - if(cfg.type != embot::hw::encoder::Type::chipAS5045) - { - return resNOK; - } +// // TODO: safe guard (remove it when AEA3 is completely implemented) +// if(cfg.type != embot::hw::encoder::Type::chipAS5045) +// { +// return resNOK; +// } uint8_t index = embot::core::tointegral(e); @@ -139,9 +139,8 @@ namespace embot { namespace hw { namespace encoder { } else if(embot::hw::encoder::Type::chipMA730 == cfg.type) { - // TODO: placeholder for AEA3 - //_data_array[index].chip_MA730 = new embot::hw::chip::MA730; - return resNOK; + _data_array[index].chip_MA730 = new embot::hw::chip::MA730; + _data_array[index].chip_MA730->init({spiBus, _data_array[index].chip_MA730->standardspiconfig} ); } embot::core::binary::bit::set(initialisedmask, index); @@ -161,10 +160,10 @@ namespace embot { namespace hw { namespace encoder { const embot::hw::encoder::BSP &encoderbsp = embot::hw::encoder::getBSP(); embot::hw::encoder::Type type = _data_array[index].config.type; - if(type != embot::hw::encoder::Type::chipAS5045) - { - return resNOK; - } +// if(type != embot::hw::encoder::Type::chipAS5045) +// { +// return resNOK; +// } if(embot::hw::encoder::Type::chipAS5045 == type) { @@ -216,7 +215,18 @@ namespace embot { namespace hw { namespace encoder { } else if(embot::hw::encoder::Type::chipMA730 == _data_array[index].config.type) { - // TODO: AEA3 + _data_array[index].start(on_completion_userdef); + + embot::core::Callback cbk { onChipCompletionReading, &_data_array[index] }; + + if (true == _data_array[index].chip_MA730->read(_data_array[index].ma730_data, cbk)) + { + return resOK; + } + else + { + return resNOK; + } return resNOK; } else @@ -245,6 +255,20 @@ namespace embot { namespace hw { namespace encoder { res = resOK; } } + else if(embot::hw::encoder::Type::chipMA730 == _data_array[index].config.type) + { + // retrieve the current encoder position from data saved internally. + if(_data_array[index].data_is_ready) + { + if(_data_array[index].ma730_data.status.ok) + { + pos = _data_array[index].ma730_data.position; + res = resOK; + } + else + res = resNOK; + } + } return res; } @@ -272,7 +296,14 @@ namespace embot { namespace hw { namespace encoder { } else if(embot::hw::encoder::Type::chipMA730 == _data_array[index].config.type) { - // placeholder for AEA3 + embot::hw::chip::MA730::Data dd {}; + _data_array[index].chip_MA730->read(dd, timeout); + + if(dd.status.ok) + { + pos = dd.position; + //destination.status = dd.status.ok; + } } else {