diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvoptx b/emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvoptx index acb1250ede..d1ae6229bb 100644 --- a/emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvoptx +++ b/emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvoptx @@ -379,7 +379,7 @@ embot::app::eth::config - 1 + 0 0 0 0 @@ -563,7 +563,7 @@ embot::hw - 1 + 0 0 0 0 @@ -2733,4 +2733,24 @@ + + mbd::torque-estimator + 0 + 0 + 0 + 0 + + 41 + 182 + 8 + 0 + 0 + 0 + ..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp + TorqueEstimator.cpp + 0 + 0 + + + diff --git a/emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvprojx b/emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvprojx index bd8bd71ddb..bce28f7a35 100644 --- a/emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvprojx +++ b/emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvprojx @@ -10,7 +10,7 @@ amc-icc-osal-ulpro 0x4 ARM-ADS - 6190000::V6.19::.\armclang-r6p19-00rel0 + 6220000::V6.22::ARMCLANG 1 @@ -340,7 +340,7 @@ -DxdebugNOicc -DxYRI_uses_MC_foc_actuator_descriptor_generic -DxuseMCfoc_actuator_descriptor_generic -Wno-pragma-pack -Wno-deprecated-register -DEMBOT_USE_rtos_osal -DIPAL_use_cfg2 -DUSE_EMBOT_theHandler -DUSE_EMBOT_theServices -DUSE_EMBOT_theServicesMC USE_ICC_COMM 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\rop;..\..\..\..\..\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;..\..\..\..\..\embot\prot\can;..\..\..\..\..\mbd\wrist_decoupler + ..\..\..\..\..\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\rop;..\..\..\..\..\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;..\..\..\..\..\embot\prot\can;..\..\..\..\..\mbd\wrist_decoupler;..\..\..\..\..\mbd\torque_estimator @@ -1745,6 +1745,16 @@ + + mbd::torque-estimator + + + TorqueEstimator.cpp + 8 + ..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.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 d3ce32c593..6af9c8e369 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,8 +81,7 @@ extern "C" { #define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3) // minor <0-255> // minor <0-255> -#define EOMTHEEMSAPPLCFG_VERSION_MINOR 96 - +#define EOMTHEEMSAPPLCFG_VERSION_MINOR 98 // version // build date @@ -91,7 +90,7 @@ extern "C" { // month <1-12> #define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12 // day <1-31> -#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 02 +#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24 // hour <0-23> #define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18 // minute <0-59> diff --git a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoprot-callbacks/EoProtocolMC_fun_ems4rd.c b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoprot-callbacks/EoProtocolMC_fun_ems4rd.c index 456fa5c3e4..09c851fdba 100644 --- a/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoprot-callbacks/EoProtocolMC_fun_ems4rd.c +++ b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoprot-callbacks/EoProtocolMC_fun_ems4rd.c @@ -322,6 +322,7 @@ extern void eoprot_fun_UPDT_mc_joint_config_motor_params(const EOnv* nv, const e } + // f-marker-begin extern void eoprot_fun_UPDT_mc_joint_config_impedance(const EOnv* nv, const eOropdescriptor_t* rd) { 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 6912fd4c75..35be17ebe4 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 @@ -2147,7 +2147,7 @@ main - 1 + 0 0 0 0 @@ -2167,7 +2167,7 @@ abslayer-lib - 1 + 0 0 0 0 @@ -2223,7 +2223,7 @@ abslayer-cfg - 1 + 0 0 0 0 @@ -2315,7 +2315,7 @@ eo-core - 1 + 0 0 0 0 @@ -2599,7 +2599,7 @@ eo-core-mee - 1 + 0 0 0 0 @@ -2723,7 +2723,7 @@ eo-arm-env - 1 + 0 0 0 0 @@ -3535,7 +3535,7 @@ 16 106 8 - 0 + 1 0 0 ..\..\..\..\..\embobj\plus\mc\Motor.c @@ -4449,6 +4449,26 @@ + + mbd::torque-estimator + 1 + 0 + 0 + 0 + + 28 + 175 + 8 + 0 + 0 + 0 + ..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp + TorqueEstimator.cpp + 0 + 0 + + + ::CMSIS 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 4d6ff29cc9..e2cdfd7c03 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 @@ -1383,6 +1383,16 @@ + + mbd::torque-estimator + + + TorqueEstimator.cpp + 8 + ..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp + + + ::CMSIS @@ -2925,6 +2935,16 @@ + + mbd::torque-estimator + + + TorqueEstimator.cpp + 8 + ..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp + + + ::CMSIS @@ -4569,6 +4589,16 @@ + + mbd::torque-estimator + + + TorqueEstimator.cpp + 8 + ..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp + + + ::CMSIS @@ -4771,7 +4801,7 @@ 0 1 1 - 4100 + 4097 1 BIN\ULP2CM3.DLL @@ -4975,9 +5005,9 @@ 0 -DxYRI_uses_MC_foc_actuator_descriptor_generic -DxTESTRTC_IS_ACTIVE -DXenableTHESERVICETESTER -Wno-pragma-pack -Wno-deprecated-register -DUSE_EMS4RD -DUSE_OLD_BUGGY_MODE_TO_SEND_UP_FULLSCALE_WITH_INVERTED_BYTES -DEMBOT_APP_SCOPE_core -DDEBUG_encoder_AKSIM - DIAGNOSTIC2_enabled DIAGNOSTIC2_receive_from_daemon DIAGNOSTIC2_send_to_yarprobotinterface _no_DIAGNOSTIC2_send_to_daemon + DIAGNOSTIC2_enabled DIAGNOSTIC2_receive_from_daemon DIAGNOSTIC2_send_to_yarprobotinterface _no_DIAGNOSTIC2_send_to_daemon SENSORLESS_TORQUE - ..\..\..\..\..\libs\highlevel\abslayer\ipal\api;..\..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\core\core;..\..\..\..\..\embobj\core\exec\multitask;..\..\..\..\..\embobj\plus\embenv;..\..\..\..\..\embobj\plus\ipnet;..\..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\..\libs\midware\eventviewer\api;..\cfg\eoemsappl;..\cfg\abslayer;..\..\..\..\..\embobj\plus\ctrloop;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\icub;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\protocol\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\protocol\src;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\transport;..\..\..\..\..\embobj\plus\board\ems001;..\..\..\..\..\embobj\plus\mc;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\utils;.;..\cfg\eoprot-callbacks;..\src\eoappservices;..\cfg\eoappservices\icub-can-proto;..\cfg\eoappservices\icub-can-net;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\robotconfig\v1\backdoor;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\opcprot;..\..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\cfg\eoprot-boards;..\..\..\..\..\libs\highlevel\abslayer\hal2\api;..\..\..\..\ems004\env\cfg;..\cfg\eoemsappl;..\..\..\..\..\libs\highlevel\services\embodyrobot;..\..\..\..\..\libs\midware\hl-plus\api;..\..\..\..\..\embobj\plus\can;..\..\..\..\..\embobj\plus\can\config-can-mapping;..\..\..\..\..\embobj\plus\can\config-can-protocol;..\..\..\..\..\embobj\plus\board;..\..\..\..\mc4plus\appl\v2\src\eoappservices;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embot;..\..\..\..\..\embot\cif;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\prot\eth;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\..\..\..\mbd\kalman_filter;..\..\..\..\..\embot\app\eth;..\..\..\..\..\embot\prot\can;..\..\..\..\..\embot\hw;..\..\..\..\..\embot\app + ..\..\..\..\..\libs\highlevel\abslayer\ipal\api;..\..\..\..\..\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\core\core;..\..\..\..\..\embobj\core\exec\multitask;..\..\..\..\..\embobj\plus\embenv;..\..\..\..\..\embobj\plus\ipnet;..\..\..\..\..\libs\highlevel\services\embenv\api;..\..\..\..\..\libs\midware\eventviewer\api;..\cfg\eoemsappl;..\cfg\abslayer;..\..\..\..\..\embobj\plus\ctrloop;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\icub;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\protocol\api;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\protocol\src;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\transport;..\..\..\..\..\embobj\plus\board\ems001;..\..\..\..\..\embobj\plus\mc;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\utils;.;..\cfg\eoprot-callbacks;..\src\eoappservices;..\cfg\eoappservices\icub-can-proto;..\cfg\eoappservices\icub-can-net;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\robotconfig\v1\backdoor;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\plus\comm-v2\opcprot;..\..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\cfg\eoprot-boards;..\..\..\..\..\libs\highlevel\abslayer\hal2\api;..\..\..\..\ems004\env\cfg;..\cfg\eoemsappl;..\..\..\..\..\libs\highlevel\services\embodyrobot;..\..\..\..\..\libs\midware\hl-plus\api;..\..\..\..\..\embobj\plus\can;..\..\..\..\..\embobj\plus\can\config-can-mapping;..\..\..\..\..\embobj\plus\can\config-can-protocol;..\..\..\..\..\embobj\plus\board;..\..\..\..\mc4plus\appl\v2\src\eoappservices;..\..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embot;..\..\..\..\..\embot\cif;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\prot\eth;..\..\..\..\..\..\..\..\..\icub-firmware-shared\embot\tools;..\..\..\..\..\mbd\kalman_filter;..\..\..\..\..\embot\app\eth;..\..\..\..\..\embot\prot\can;..\..\..\..\..\embot\hw;..\..\..\..\..\embot\app;..\..\..\..\..\mbd\torque_estimator @@ -6162,6 +6192,16 @@ + + mbd::torque-estimator + + + TorqueEstimator.cpp + 8 + ..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp + + + ::CMSIS diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h index 1152269bc9..a68847ca8c 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h @@ -41,7 +41,7 @@ typedef struct // CableCalib typedef struct // HardStopCalib { - int32_t pwm; // [2FOC PWM units] (-32000 : +32000) = (-100% : +100%) + int32_t pwm; // [2FOC PWM units] (-32000 : +32000) = (-100% : +100%) int32_t zero; // [icubdegrees] int32_t space_thr; // [icubdegrees] int32_t time_thr; // [milliseconds] diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c index 5094671808..483c007e9d 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c @@ -108,10 +108,10 @@ static eOresult_t JointSet_do_wait_calibration_6_singleJoint(JointSet *o, int in //get the encoder of joint to calibrate AbsEncoder* e_ptr = o->absEncoder+ o->encoders_of_set[indexSet]; - + jointCalibType6Data *jCalib6Data_ptr = &(j_ptr->running_calibration.data.type6); - + switch(jCalib6Data_ptr->state) { diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c index 15d14c5bb2..bd9feceab8 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c @@ -935,8 +935,8 @@ void MController_config_board(const eOmn_serv_configuration_t* brd_cfg) if(!isIdentityMatrix) Sje_aux = o->Sje; - - + + for (int s=0; snSets; ++s) { JointSet_config diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c index 533f06ad13..9ecda06faa 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c @@ -159,7 +159,7 @@ 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; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c index 42b1e11874..4128bea3ea 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c @@ -108,7 +108,6 @@ void JointSet_init(JointSet* o) // o->special_constraint = eomc_jsetconstraint_none; o->calibration_in_progress = eomc_calibration_typeUndefined; - } void JointSet_config // @@ -185,6 +184,50 @@ void JointSet_do_odometry(JointSet* o) // } } } + +#if defined(SENSORLESS_TORQUE) + if (o->Jmj) + { + for (int js=0; jsjoints_of_set[js]; + + for (int ms=0; msmotors_of_set[ms]; + + if (o->motor[m].sensorless_torque) + { + sensorless_torque = TRUE; + + // Tau = Jinvt mu + // transposed inverse Jacobian + joint_trq_fbk += o->Jmj[m][j]*o->motor[m].torque; + } + } + + if (sensorless_torque) + { + Joint_update_torque_fbk(o->joint+j, joint_trq_fbk*1000000.0f); + } + } + } + else + { + for (int k=0; kmotors_of_set[k]; + + if (o->motor[m].sensorless_torque) + { + Joint_update_torque_fbk(o->joint+o->joints_of_set[k], o->motor[m].torque*1000000.0f); + } + } + } +#endif float kf_input[MAX_JOINTS_PER_BOARD]; @@ -1279,6 +1322,7 @@ static void JointSet_do_current_control(JointSet* o) else { motor_vel_kf_icubdeg_sec = o->joint[m].vel_fbk * o->motor[m].GEARBOX; + motor_current_ref = Motor_do_trq_control(o->motor+m, o->joint[m].trq_ref, o->joint[m].trq_fbk, motor_vel_kf_icubdeg_sec); } } diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h index 810d312ca7..ad3dfaa127 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h @@ -42,7 +42,7 @@ 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 { - // The warmup flag is == 2 at startup and makes the wrist start a first parking move. + // The warmup flag is == 2 at startup and makes the wrist start a first parking move. // warmup == 1 means first parking in progress, and warmup == 0 means first parking done. // Thanks to the new direct kinematics solver all the parking procedures will be likely removed // after some time of use of the robot without problems because no more necessary. @@ -135,7 +135,6 @@ typedef struct // JointSet TripodCalib tripod_calib; HardStopCalib hard_stop_calib; - } JointSet; extern JointSet* JointSet_new(uint8_t n); // diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c index a5de15309d..7f762df202 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c @@ -308,6 +308,12 @@ void Motor_init(Motor* o) // Motor_hardStopCalbData_reset(o); //o->outOfLimitsSignaled = FALSE; + +#if defined(SENSORLESS_TORQUE) + o->torque = ZERO; + o->sensorless_torque = FALSE; + o->torque_estimator.initialize(); +#endif } void Motor_config(Motor* o, uint8_t ID, eOmc_motor_config_t* config) // @@ -356,7 +362,29 @@ void Motor_config(Motor* o, uint8_t ID, eOmc_motor_config_t* config) // o->control_mode = icubCanProto_controlmode_idle; hal_motor_disable(static_cast(o->motorlocation.adr)); } - + +#if defined(SENSORLESS_TORQUE) + o->torque = ZERO; + + if (config->LuGre_params.Km > ZERO) + { + o->sensorless_torque = TRUE; + + o->torque_estimator.rtU.Km = config->LuGre_params.Km; //4.3; + o->torque_estimator.rtU.Kw = config->LuGre_params.Kw; //0.045; + o->torque_estimator.rtU.S0 = config->LuGre_params.S0; //2.5; + o->torque_estimator.rtU.S1 = config->LuGre_params.S1; //0.0; + o->torque_estimator.rtU.Vth = config->LuGre_params.Vth; //5.0; + o->torque_estimator.rtU.Fc_pos = config->LuGre_params.Fc_pos; //3.3; + o->torque_estimator.rtU.Fc_neg = config->LuGre_params.Fc_neg; //3.3; + o->torque_estimator.rtU.Fs_pos = config->LuGre_params.Fs_pos; //7.0; + o->torque_estimator.rtU.Fs_neg = config->LuGre_params.Fs_neg; //7.0; + } + else + { + o->sensorless_torque = FALSE; + } +#endif } void Motor_config_encoder(Motor* o, int32_t resolution) @@ -887,7 +915,7 @@ BOOL Motor_check_faults(Motor* o) // fault_state.bits.EncoderFault = FALSE; } */ - + if (can_dead && !o->can_dead) { @@ -962,13 +990,40 @@ BOOL Motor_is_calibrated(Motor* o) // return !(o->not_calibrated); } -CTRL_UNITS Motor_do_trq_control(Motor* o, CTRL_UNITS trq_ref, CTRL_UNITS trq_fbk, CTRL_UNITS motor_vel_icubdeg_sec) // + +CTRL_UNITS Motor_do_trq_control_EXPERIMENTAL(Motor* o, CTRL_UNITS trq_ref, CTRL_UNITS trq_fbk, CTRL_UNITS motor_vel_icubdeg_sec) // { o->trq_ref = trq_ref; o->trq_fbk = trq_fbk; o->trq_err = trq_ref - trq_fbk; + +// CTRL_UNITS current = (o->motor[m].GEARBOX > 0)? o->motor[m].Iqq_fbk : -o->motor[m].Iqq_fbk; +// motor_current_ref = 0.975f*current + (o->joint[m].trq_ref - o->joint[m].trq_fbk)/4500.0f - 0.04f*o->joint[m].vel_fbk; +// LIMIT(motor_current_ref, 3000.0f); +// if (o->motor[m].GEARBOX < 0) motor_current_ref = -motor_current_ref; +// //motor_current_ref = Motor_do_trq_control(o->motor+m, o->joint[m].trq_ref, o->joint[m].trq_fbk, motor_vel_kf_icubdeg_sec); + + CTRL_UNITS current = (o->GEARBOX > 0)? o->Iqq_fbk : -o->Iqq_fbk; + + CTRL_UNITS motor_current_ref = 0.975f*current + o->trqPID.Ktau*(trq_ref - trq_fbk) - 0.0004f*motor_vel_icubdeg_sec; + + LIMIT(motor_current_ref, o->trqPID.out_max); + + if (o->GEARBOX < 0) motor_current_ref = -motor_current_ref; + + return motor_current_ref; +} + + +CTRL_UNITS Motor_do_trq_control(Motor* o, CTRL_UNITS trq_ref, CTRL_UNITS trq_fbk, CTRL_UNITS motor_vel_icubdeg_sec) // +{ + o->trq_ref = trq_ref; + o->trq_fbk = trq_fbk; + + o->trq_err = trq_ref - trq_fbk; + return PID_do_out(&o->trqPID, o->trq_err) + PID_do_friction_comp(&o->trqPID, motor_vel_icubdeg_sec, o->trq_ref); } @@ -1167,6 +1222,26 @@ void Motor_update_odometry_fbk_can(Motor* o, CanOdometry2FocMsg* can_msg) // o->pos_raw_fbk = can_msg->position; o->pos_fbk = o->pos_raw_fbk/o->GEARBOX - o->pos_calib_offset; o->pos_raw_cal_fbk = o->pos_raw_fbk - o->pos_calib_offset*o->GEARBOX; + +#if defined(SENSORLESS_TORQUE) + if (o->sensorless_torque) + { + static const float IDEG2RAD = 6.28f/65536.0f; + + o->torque_estimator.rtU.Current = 0.001f* o->Iqq_fbk; + o->torque_estimator.rtU.Velocity = IDEG2RAD*o->vel_raw_fbk; + + if (o->GEARBOX < 0) + { + o->torque_estimator.rtU.Current = -o->torque_estimator.rtU.Current; + o->torque_estimator.rtU.Velocity = -o->torque_estimator.rtU.Velocity; + } + + o->torque_estimator.step(); + + o->torque = o->torque_estimator.rtY.Torque; + } +#endif } void Motor_update_pos_fbk(Motor* o, int32_t position_raw) diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h index e8538a3832..eb37c36552 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h @@ -72,6 +72,7 @@ extern BOOL Motor_check_faults(Motor* o); // extern void Motor_reset(Motor *o);// extern CTRL_UNITS Motor_do_trq_control(Motor* o, CTRL_UNITS trq_ref, CTRL_UNITS trq_fbk, CTRL_UNITS motor_vel_kf_icubdeg_sec); // +extern CTRL_UNITS Motor_do_trq_control_EXPERIMENTAL(Motor* o, CTRL_UNITS trq_ref, CTRL_UNITS trq_fbk, CTRL_UNITS motor_vel_icubdeg_sec); extern void Motor_update_state_fbk(Motor* o, void* state_msg); // extern void Motor_update_odometry_fbk_can(Motor* o, CanOdometry2FocMsg* data); // extern void Motor_do_calibration_hard_stop(Motor* o); // diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h index 617127e2c0..9c6f2d250e 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h @@ -20,6 +20,9 @@ #ifndef MC_MOTOR_HID_H___ #define MC_MOTOR_HID_H___ +#if defined(SENSORLESS_TORQUE) + #include "TorqueEstimator.h" +#endif #include "WatchDog.h" @@ -178,6 +181,11 @@ struct Motor_hid uint8_t can_motor_config[7]; //BOOL outOfLimitsSignaled; +#if defined(SENSORLESS_TORQUE) + float torque; + BOOL sensorless_torque; + TorqueEstimator torque_estimator {}; +#endif }; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Trajectory.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Trajectory.c index a98e12a10a..951399e37f 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Trajectory.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Trajectory.c @@ -136,8 +136,8 @@ void Trajectory_set_pos_end(Trajectory *o, /*float x0,*/ float xStar, float velA o->xTimer = o->xT = 0.0f; o->xX = (float)xStar; - - o->xV = o->xA = 0.0f; + + o->xV = o->xA = 0.0f; return; } @@ -181,9 +181,9 @@ void Trajectory_set_vel_end(Trajectory *o, float vStar, float accAvg) o->vTimer = o->vT = 0.0f; o->vV = vStar; - - o->vA = 0.0f; - + + o->vA = 0.0f; + return; } @@ -279,7 +279,7 @@ int8_t Trajectory_do_step(Trajectory* o, float *p, float *v, float *a) *v = 0.0f; } - + if (*a < 0.0f) *a = 0.0f; *p = o->pos_min; @@ -294,7 +294,7 @@ int8_t Trajectory_do_step(Trajectory* o, float *p, float *v, float *a) *v = 0.0f; } - + if (*a > 0.0f) *a = 0.0f; *p = o->pos_max; diff --git a/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.cpp b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.cpp new file mode 100644 index 0000000000..c71fe95e4d --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.cpp @@ -0,0 +1,111 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: TorqueEstimator.cpp +// +// Code generated for Simulink model 'TorqueEstimator'. +// +// Model version : 2.100 +// Simulink Coder version : 24.1 (R2024a) 19-Nov-2023 +// C/C++ source code generated on : Tue Oct 22 09:33:53 2024 +// +// 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 "TorqueEstimator.h" +#include "rtwtypes.h" +#include + +// Output and update for atomic system: '/TorqueEstimator' +void TorqueEstimator::TorqueEstimator_l(real_T rtu_Current, real_T rtu_Velocity, + real_T rtu_Km, real_T rtu_Kw, real_T rtu_S0, real_T rtu_S1, real_T rtu_Vth, + real_T rtu_Fc_pos, real_T rtu_Fc_neg, real_T rtu_Fs_pos, real_T rtu_Fs_neg, + real_T *rty_Torque, real_T *rty_Motor_torque, real_T *rty_Friction, + DW_TorqueEstimator *localDW) +{ + real_T Fc; + real_T Fs; + real_T tmp; + real_T tmp_0; + if (rtu_Velocity >= 0.0) { + Fc = rtu_Fc_pos; + Fs = rtu_Fs_pos; + } else { + Fc = rtu_Fc_neg; + Fs = rtu_Fs_neg; + } + + Fc = rtu_Velocity - std::abs(rtu_Velocity) * rtu_S0 * localDW->z / ((Fs - Fc) * + std::exp(-std::abs(rtu_Velocity / rtu_Vth)) + Fc); + localDW->z += 0.001 * Fc; + tmp = rtu_Km * rtu_Current; + *rty_Motor_torque = tmp; + Fs = rtu_Kw * rtu_Velocity; + tmp_0 = rtu_S0 * localDW->z; + Fc *= rtu_S1; + *rty_Friction = (Fs + tmp_0) + Fc; + *rty_Torque = ((tmp - Fs) - tmp_0) - Fc; +} + +// Model step function +void TorqueEstimator::step() +{ + // MATLAB Function: '/TorqueEstimator' incorporates: + // Inport: '/Current' + // Inport: '/Fc_neg' + // Inport: '/Fc_pos' + // Inport: '/Fs_neg' + // Inport: '/Fs_pos' + // Inport: '/Km' + // Inport: '/Kw' + // Inport: '/S0' + // Inport: '/S1' + // Inport: '/Velocity' + // Inport: '/Vth' + // Outport: '/Friction' + // Outport: '/Motor_torque' + // Outport: '/Torque' + + TorqueEstimator_l(rtU.Current, rtU.Velocity, rtU.Km, rtU.Kw, rtU.S0, rtU.S1, + rtU.Vth, rtU.Fc_pos, rtU.Fc_neg, rtU.Fs_pos, rtU.Fs_neg, + &rtY.Torque, &rtY.Motor_torque, &rtY.Friction, + &rtDW.sf_TorqueEstimator); +} + +// Model initialize function +void TorqueEstimator::initialize() +{ + // (no initialization code required) +} + +// Constructor +TorqueEstimator::TorqueEstimator() : + rtU(), + rtY(), + rtDW(), + rtM() +{ + // Currently there is no constructor body generated. +} + +// Destructor +// Currently there is no destructor body generated. +TorqueEstimator::~TorqueEstimator() = default; + +// Real-Time Model get method +TorqueEstimator::RT_MODEL * TorqueEstimator::getRTM() +{ + return (&rtM); +} + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.h b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.h new file mode 100644 index 0000000000..08f0e1c4db --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.h @@ -0,0 +1,152 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: TorqueEstimator.h +// +// Code generated for Simulink model 'TorqueEstimator'. +// +// Model version : 2.100 +// Simulink Coder version : 24.1 (R2024a) 19-Nov-2023 +// C/C++ source code generated on : Tue Oct 22 09:33:53 2024 +// +// 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 +// +#ifndef TorqueEstimator_h_ +#define TorqueEstimator_h_ +#include +#include "rtwtypes.h" + +// Macros for accessing real-time model data structure +#ifndef rtmGetErrorStatus +#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) +#endif + +#ifndef rtmSetErrorStatus +#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val)) +#endif + +// Class declaration for model TorqueEstimator +class TorqueEstimator final +{ + // public data and function members + public: + // Block signals and states (default storage) for system '/TorqueEstimator' + struct DW_TorqueEstimator { + real_T z; // '/TorqueEstimator' + }; + + // Block signals and states (default storage) for system '' + struct DW { + DW_TorqueEstimator sf_TorqueEstimator;// '/TorqueEstimator' + }; + + // External inputs (root inport signals with default storage) + struct ExtU { + real_T Current; // '/Current' + real_T Velocity; // '/Velocity' + real_T Km; // '/Km' + real_T Kw; // '/Kw' + real_T S0; // '/S0' + real_T S1; // '/S1' + real_T Vth; // '/Vth' + real_T Fc_pos; // '/Fc_pos' + real_T Fc_neg; // '/Fc_neg' + real_T Fs_pos; // '/Fs_pos' + real_T Fs_neg; // '/Fs_neg' + }; + + // External outputs (root outports fed by signals with default storage) + struct ExtY { + real_T Torque; // '/Torque' + real_T Motor_torque; // '/Motor_torque' + real_T Friction; // '/Friction' + }; + + // Real-time Model Data Structure + struct RT_MODEL { + const char_T * volatile errorStatus; + }; + + // Copy Constructor + TorqueEstimator(TorqueEstimator const&) = delete; + + // Assignment Operator + TorqueEstimator& operator= (TorqueEstimator const&) & = delete; + + // Move Constructor + TorqueEstimator(TorqueEstimator &&) = delete; + + // Move Assignment Operator + TorqueEstimator& operator= (TorqueEstimator &&) = delete; + + // Real-Time Model get method + TorqueEstimator::RT_MODEL * getRTM(); + + // External inputs + ExtU rtU; + + // External outputs + ExtY rtY; + + // model initialize function + static void initialize(); + + // model step function + void step(); + + // Constructor + TorqueEstimator(); + + // Destructor + ~TorqueEstimator(); + + // private data and function members + private: + // Block states + DW rtDW; + + // private member function(s) for subsystem '/TorqueEstimator' + static void TorqueEstimator_l(real_T rtu_Current, real_T rtu_Velocity, real_T + rtu_Km, real_T rtu_Kw, real_T rtu_S0, real_T rtu_S1, real_T rtu_Vth, real_T + rtu_Fc_pos, real_T rtu_Fc_neg, real_T rtu_Fs_pos, real_T rtu_Fs_neg, real_T * + rty_Torque, real_T *rty_Motor_torque, real_T *rty_Friction, + DW_TorqueEstimator *localDW); + + // Real-Time Model + RT_MODEL rtM; +}; + +//- +// The generated code includes comments that allow you to trace directly +// back to the appropriate location in the model. The basic format +// is /block_name, where system is the system number (uniquely +// assigned by Simulink) and block_name is the name of the block. +// +// Note that this particular code originates from a subsystem build, +// and has its own system numbers different from the parent model. +// Refer to the system hierarchy for this subsystem below, and use the +// MATLAB hilite_system command to trace the generated code back +// to the parent model. For example, +// +// hilite_system('torque_est_code_source/TorqueEstimator') - opens subsystem torque_est_code_source/TorqueEstimator +// hilite_system('torque_est_code_source/TorqueEstimator/Kp') - opens and selects block Kp +// +// Here is the system hierarchy for this model +// +// '' : 'torque_est_code_source' +// '' : 'torque_est_code_source/TorqueEstimator' + +#endif // TorqueEstimator_h_ + +// +// File trailer for generated code. +// +// [EOF] +// diff --git a/emBODY/eBcode/arch-arm/mbd/torque_estimator/rtwtypes.h b/emBODY/eBcode/arch-arm/mbd/torque_estimator/rtwtypes.h new file mode 100644 index 0000000000..6eaf7bfeab --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/torque_estimator/rtwtypes.h @@ -0,0 +1,106 @@ +// +// Non-Degree Granting Education License -- for use at non-degree +// granting, nonprofit, education, and research organizations only. Not +// for commercial or industrial use. +// +// File: rtwtypes.h +// +// Code generated for Simulink model 'TorqueEstimator'. +// +// Model version : 2.100 +// Simulink Coder version : 24.1 (R2024a) 19-Nov-2023 +// C/C++ source code generated on : Tue Oct 22 09:33:53 2024 +// +// 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 +// + +#ifndef RTWTYPES_H +#define RTWTYPES_H + +// Logical type definitions +#if (!defined(__cplusplus)) +#ifndef false +#define false (0U) +#endif + +#ifndef true +#define true (1U) +#endif +#endif + +//=======================================================================* +// Target hardware information +// Device type: ARM Compatible->ARM Cortex-M +// Number of bits: char: 8 short: 16 int: 32 +// long: 32 long long: 64 +// native word size: 32 +// Byte ordering: LittleEndian +// Signed integer division rounds to: Zero +// Shift right on a signed integer as arithmetic shift: on +// ======================================================================= + +//=======================================================================* +// Fixed width word size data types: * +// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * +// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * +// real32_T, real64_T - 32 and 64 bit floating point numbers * +// ======================================================================= +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef long long int64_T; +typedef unsigned long long uint64_T; +typedef float real32_T; +typedef double real64_T; + +//===========================================================================* +// Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * +// real_T, time_T, ulong_T, ulonglong_T. * +// =========================================================================== +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef unsigned long long ulonglong_T; +typedef char char_T; +typedef unsigned char uchar_T; +typedef char_T byte_T; + +//=======================================================================* +// Min and Max: * +// int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * +// uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * +// ======================================================================= +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255U)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535U)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MAX_int64_T ((int64_T)(9223372036854775807LL)) +#define MIN_int64_T ((int64_T)(-9223372036854775807LL-1LL)) +#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFULL)) + +// Block D-Work pointer type +typedef void * pointer_T; + +#endif // RTWTYPES_H + +// +// File trailer for generated code. +// +// [EOF] +//