From e2df9acaa5c1bfc3b9bca2e4dd5cb43a3e35c931 Mon Sep 17 00:00:00 2001 From: ale-git Date: Sun, 8 Sep 2024 22:19:04 +0200 Subject: [PATCH 1/5] Torque sensorless estimation added to devel branch of EMS firmware. And alternative experimental torque control function is added in Motor.c --- .../v2/proj/ems4rd.diagnostic2ready.uvoptx | 32 +++- .../v2/proj/ems4rd.diagnostic2ready.uvprojx | 44 +++++- .../eBcode/arch-arm/embobj/plus/mc/JointSet.c | 44 +++++- .../eBcode/arch-arm/embobj/plus/mc/JointSet.h | 2 + emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c | 66 +++++++- emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h | 1 + .../arch-arm/embobj/plus/mc/Motor_hid.h | 8 +- .../mbd/torque_estimator/TorqueEstimator.cpp | 99 ++++++++++++ .../mbd/torque_estimator/TorqueEstimator.h | 149 ++++++++++++++++++ .../arch-arm/mbd/torque_estimator/rtwtypes.h | 106 +++++++++++++ 10 files changed, 537 insertions(+), 14 deletions(-) create mode 100644 emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.cpp create mode 100644 emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.h create mode 100644 emBODY/eBcode/arch-arm/mbd/torque_estimator/rtwtypes.h 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..fd13d19841 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 @@ -4449,6 +4449,26 @@ + + mbd::torque-estimator + 1 + 0 + 0 + 0 + + 28 + 175 + 8 + 1 + 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..8df41ef2b1 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 @@ -4647,7 +4677,7 @@ ems-ulpro 0x4 ARM-ADS - 6220000::V6.22::ARMCLANG + 6190000::V6.19::ARMCLANG 1 @@ -4977,7 +5007,7 @@ -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 - ..\..\..\..\..\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/JointSet.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c index 42b1e11874..5931848472 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c @@ -138,6 +138,12 @@ void JointSet_config // o->Jjm = Jjm; o->Sjm = Sjm; o->Jmj = Jmj; o->Smj = Smj; o->Sje = Sje; + +#if defined(SENSORLESS_TORQUE) + o->sensorless_torque = TRUE; +#else + o->sensorless_torque = FALSE; +#endif } void JointSet_do_odometry(JointSet* o) // @@ -186,6 +192,34 @@ void JointSet_do_odometry(JointSet* o) // } } + if (o->sensorless_torque) + { + for (int ms=0; msmotors_of_set[ms]; + + if (o->Jmj) + { + CTRL_UNITS joint_trq_fbk = ZERO; + + for (int js=0; jsjoints_of_set[js]; + + // Tau = Jinvt mu + // transposed inverse Jacobian + joint_trq_fbk += o->Jmj[m][j]*o->motor[m].torque; + } + + Joint_update_torque_fbk(o->joint+j, joint_trq_fbk*1000000.0f); + } + else + { + Joint_update_torque_fbk(o->joint+m, o->motor[m].torque*1000000.0f); + } + } + } + float kf_input[MAX_JOINTS_PER_BOARD]; if (!o->Sje) // no encoder coupling @@ -1270,7 +1304,10 @@ static void JointSet_do_current_control(JointSet* o) motor_vel_kf_icubdeg_sec += o->Jmj[m][j] * o->joint[j].vel_fbk * o->motor[m].GEARBOX; } - motor_current_ref = Motor_do_trq_control(o->motor+m, motor_trq_ref, motor_trq_fbk, motor_vel_kf_icubdeg_sec); + if (o->sensorless_torque) + motor_current_ref = Motor_do_trq_control_EXPERIMENTAL(o->motor+m, motor_trq_ref, motor_trq_fbk, motor_vel_kf_icubdeg_sec); + else + motor_current_ref = Motor_do_trq_control(o->motor+m, motor_trq_ref, motor_trq_fbk, motor_vel_kf_icubdeg_sec); //char info[70]; //snprintf(info, 70, "comp %f m:%d", motor_current_ref, m); @@ -1279,7 +1316,10 @@ 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); + if (o->sensorless_torque) + motor_current_ref = Motor_do_trq_control_EXPERIMENTAL(o->motor+m, o->joint[m].trq_ref, o->joint[m].trq_fbk, motor_vel_kf_icubdeg_sec); + else + 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); } } else diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h index 810d312ca7..cd50a60ada 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h @@ -135,6 +135,8 @@ typedef struct // JointSet TripodCalib tripod_calib; HardStopCalib hard_stop_calib; + + BOOL sensorless_torque; } JointSet; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c index a5de15309d..d6ad08cbe5 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c @@ -356,7 +356,24 @@ 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->sensorless_torque = TRUE; +#else + o->sensorless_torque = FALSE; +#endif + + if (o->sensorless_torque) + { + o->torque_estimator.initialize(); + o->torque_estimator.rtU.Km = 4.3; + o->torque_estimator.rtU.Kw = 0.045; + o->torque_estimator.rtU.S0 = 2.5; + o->torque_estimator.rtU.S1 = 0.0; + o->torque_estimator.rtU.Vth = 5.0; + o->torque_estimator.rtU.Fc = 3.3; + o->torque_estimator.rtU.Fs = 7.0; + } } void Motor_config_encoder(Motor* o, int32_t resolution) @@ -962,13 +979,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 +1211,24 @@ 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 (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; + } } 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..a5abc22533 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor_hid.h @@ -20,7 +20,7 @@ #ifndef MC_MOTOR_HID_H___ #define MC_MOTOR_HID_H___ - +#include "TorqueEstimator.h" #include "WatchDog.h" typedef struct @@ -177,7 +177,11 @@ struct Motor_hid WatchDog can_2FOC_alive_wdog; uint8_t can_motor_config[7]; //BOOL outOfLimitsSignaled; - + + float torque; + BOOL sensorless_torque; + + TorqueEstimator torque_estimator {}; }; 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..ca3124473c --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.cpp @@ -0,0 +1,99 @@ +// +// 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.98 +// Simulink Coder version : 24.1 (R2024a) 19-Nov-2023 +// C/C++ source code generated on : Sun Sep 8 21:32:31 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, real_T rtu_Fs, real_T *rty_Torque, real_T *rty_Motor, real_T + *rty_Friction, DW_TorqueEstimator *localDW) +{ + real_T tmp; + real_T tmp_0; + real_T tmp_1; + real_T zdot; + zdot = rtu_Velocity - std::abs(rtu_Velocity) * rtu_S0 * localDW->z / ((rtu_Fs + - rtu_Fc) * std::exp(-std::abs(rtu_Velocity / rtu_Vth)) + rtu_Fc); + localDW->z += 0.001 * zdot; + tmp = rtu_Km * rtu_Current; + *rty_Motor = tmp; + tmp_0 = rtu_Kw * rtu_Velocity; + tmp_1 = rtu_S0 * localDW->z; + zdot *= rtu_S1; + *rty_Friction = (tmp_0 + tmp_1) + zdot; + *rty_Torque = ((tmp - tmp_0) - tmp_1) - zdot; +} + +// Model step function +void TorqueEstimator::step() +{ + // MATLAB Function: '/TorqueEstimator' incorporates: + // Inport: '/Current' + // Inport: '/Fc' + // Inport: '/Fs' + // Inport: '/Km' + // Inport: '/Kw' + // Inport: '/S0' + // Inport: '/S1' + // Inport: '/Velocity' + // Inport: '/Vth' + // Outport: '/Friction' + // Outport: '/Motor' + // Outport: '/Torque' + + TorqueEstimator_l(rtU.Current, rtU.Velocity, rtU.Km, rtU.Kw, rtU.S0, rtU.S1, + rtU.Vth, rtU.Fc, rtU.Fs, &rtY.Torque, &rtY.Motor, + &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..959fbbba2c --- /dev/null +++ b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.h @@ -0,0 +1,149 @@ +// +// 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.98 +// Simulink Coder version : 24.1 (R2024a) 19-Nov-2023 +// C/C++ source code generated on : Sun Sep 8 21:32:31 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; // '/Fc' + real_T Fs; // '/Fs' + }; + + // External outputs (root outports fed by signals with default storage) + struct ExtY { + real_T Torque; // '/Torque' + real_T Motor; // '/Motor' + 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, real_T rtu_Fs, real_T *rty_Torque, real_T *rty_Motor, 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..e8603ca1ef --- /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.98 +// Simulink Coder version : 24.1 (R2024a) 19-Nov-2023 +// C/C++ source code generated on : Sun Sep 8 21:32:31 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] +// From c56996af86f38dc565025d10d49ac11520be76c2 Mon Sep 17 00:00:00 2001 From: ale-git Date: Thu, 19 Sep 2024 16:53:26 +0200 Subject: [PATCH 2/5] LuGre parameters added to EMS board configuration protocol. --- .../cfg/eoprot-callbacks/EoProtocolMC_fun_ems4rd.c | 14 ++++++++++++++ emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c | 14 ++++++++++++++ emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h | 1 + emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c | 11 +++++++++++ emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h | 1 + 5 files changed, 41 insertions(+) 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..0a71a50248 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 @@ -321,6 +321,20 @@ extern void eoprot_fun_UPDT_mc_joint_config_motor_params(const EOnv* nv, const e } } +extern void eoprot_fun_UPDT_mc_joint_config_LuGre_params(const EOnv* nv, const eOropdescriptor_t* rd) +{ // not for mc4can + eOmc_LuGre_params_t *mparams = (eOmc_LuGre_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)) + { + MController_config_LuGre_params(jxx, mparams); + } +} + // 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/embobj/plus/mc/Controller.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c index 15d14c5bb2..78c215867a 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c @@ -1002,6 +1002,20 @@ void MController_config_motor_friction(int m, eOmc_motor_params_t* params) // Motor_config_friction(smc->motor+m, params->bemf_value, params->ktau_value, params->friction); } +void MController_config_LuGre_params(int m, eOmc_LuGre_params_t* params) // +{ + Motor_config_LuGre(smc->motor+m, + params->Km, + params->Kw, + params->S0, + params->S1, + params->Vth, + params->Fc, + params->Fs + ); +} + + void MController_config_joint_impedance(int j, eOmc_impedance_t* impedance) // { Joint_set_impedance(smc->joint+j, impedance); diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h index ade8b5f462..da867dfa93 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h @@ -85,6 +85,7 @@ extern void MController_update_motor_pos_fbk(int m, int32_t position_raw); extern void MController_update_motor_current_fbk(int m, int16_t current); extern void MController_update_motor_temperature_fbk(int m, int16_t temperature); extern void MController_config_motor_friction(int m, eOmc_motor_params_t* params); // +extern void MController_config_LuGre_params(int m, eOmc_LuGre_params_t* params); extern void MController_config_joint_impedance(int j, eOmc_impedance_t* impedance); // extern void MController_config_minjerk_pid(int j, eOmc_PID_t *pid_conf); // diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c index d6ad08cbe5..2556e526ea 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c @@ -446,6 +446,17 @@ void Motor_config_friction(Motor* o, float Bemf, float Ktau, eOmc_FrictionParams PID_config_friction(&o->trqPID, Bemf, Ktau, friction); } +void Motor_config_LuGre(Motor* o, float Km, float Kw, float S0, float S1, float Vth, float Fc, float Fs) +{ + o->torque_estimator.rtU.Km = Km; + o->torque_estimator.rtU.Kw = Kw; + o->torque_estimator.rtU.S0 = S0; + o->torque_estimator.rtU.S1 = S1; + o->torque_estimator.rtU.Vth = Vth; + o->torque_estimator.rtU.Fc = Fc; + o->torque_estimator.rtU.Fs = Fs; +} + void Motor_calibrate_withOffset(Motor* o, int32_t offset) // { o->pos_calib_offset = offset; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h index eb37c36552..e940d8b18f 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h @@ -58,6 +58,7 @@ extern void Motor_config_speed_PID(Motor* o, eOmc_PID_t* pid); extern void Motor_config_filter(Motor* o, uint8_t filter); // extern void Motor_config_friction(Motor* o, float Bemf, float Ktau, eOmc_FrictionParams_t friction); // +extern void Motor_config_LuGre(Motor* o, float Km, float Kw, float S0, float S1, float Vth, float Fc, float Fs); extern void Motor_calibrate_withOffset(Motor* o, int32_t offset); // extern BOOL Motor_calibrate_moving2Hardstop(Motor* o, int32_t pwm, int32_t zero); // extern void Motor_uncalibrate(Motor* o); From 722961333c3cc9867e83b911d53c696555a96e59 Mon Sep 17 00:00:00 2001 From: ale-git Date: Tue, 22 Oct 2024 13:41:16 +0200 Subject: [PATCH 3/5] Added positive and negative rotation parameters for Coulomb and Stribeck frictions. --- .../v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h | 5 +- .../v2/proj/ems4rd.diagnostic2ready.uvoptx | 2 +- .../arch-arm/embobj/plus/mc/Controller.c | 6 ++- emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c | 14 +++-- emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h | 2 +- .../mbd/torque_estimator/TorqueEstimator.cpp | 52 ++++++++++++------- .../mbd/torque_estimator/TorqueEstimator.h | 17 +++--- .../arch-arm/mbd/torque_estimator/rtwtypes.h | 4 +- 8 files changed, 61 insertions(+), 41 deletions(-) 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/proj/ems4rd.diagnostic2ready.uvoptx b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvoptx index fd13d19841..221827a37f 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 @@ -4459,7 +4459,7 @@ 28 175 8 - 1 + 0 0 0 ..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c index 78c215867a..30660295d1 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c @@ -1010,8 +1010,10 @@ void MController_config_LuGre_params(int m, eOmc_LuGre_params_t* params) // params->S0, params->S1, params->Vth, - params->Fc, - params->Fs + params->Fc_pos, + params->Fc_neg, + params->Fs_pos, + params->Fs_neg ); } diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c index 2556e526ea..934af2f446 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c @@ -371,8 +371,10 @@ void Motor_config(Motor* o, uint8_t ID, eOmc_motor_config_t* config) // o->torque_estimator.rtU.S0 = 2.5; o->torque_estimator.rtU.S1 = 0.0; o->torque_estimator.rtU.Vth = 5.0; - o->torque_estimator.rtU.Fc = 3.3; - o->torque_estimator.rtU.Fs = 7.0; + o->torque_estimator.rtU.Fc_pos = 3.3; + o->torque_estimator.rtU.Fc_neg = 3.3; + o->torque_estimator.rtU.Fs_pos = 7.0; + o->torque_estimator.rtU.Fs_neg = 7.0; } } @@ -446,15 +448,17 @@ void Motor_config_friction(Motor* o, float Bemf, float Ktau, eOmc_FrictionParams PID_config_friction(&o->trqPID, Bemf, Ktau, friction); } -void Motor_config_LuGre(Motor* o, float Km, float Kw, float S0, float S1, float Vth, float Fc, float Fs) +void Motor_config_LuGre(Motor* o, float Km, float Kw, float S0, float S1, float Vth, float Fc_pos, float Fc_neg, float Fs_pos, float Fs_neg) { o->torque_estimator.rtU.Km = Km; o->torque_estimator.rtU.Kw = Kw; o->torque_estimator.rtU.S0 = S0; o->torque_estimator.rtU.S1 = S1; o->torque_estimator.rtU.Vth = Vth; - o->torque_estimator.rtU.Fc = Fc; - o->torque_estimator.rtU.Fs = Fs; + o->torque_estimator.rtU.Fc_pos = Fc_pos; + o->torque_estimator.rtU.Fc_neg = Fc_neg; + o->torque_estimator.rtU.Fs_pos = Fs_pos; + o->torque_estimator.rtU.Fs_neg = Fs_neg; } void Motor_calibrate_withOffset(Motor* o, int32_t offset) // diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h index e940d8b18f..f6422f33d8 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h @@ -58,7 +58,7 @@ extern void Motor_config_speed_PID(Motor* o, eOmc_PID_t* pid); extern void Motor_config_filter(Motor* o, uint8_t filter); // extern void Motor_config_friction(Motor* o, float Bemf, float Ktau, eOmc_FrictionParams_t friction); // -extern void Motor_config_LuGre(Motor* o, float Km, float Kw, float S0, float S1, float Vth, float Fc, float Fs); +extern void Motor_config_LuGre(Motor* o, float Km, float Kw, float S0, float S1, float Vth, float Fc_pos, float Fc_neg, float Fs_pos, float Fs_neg); extern void Motor_calibrate_withOffset(Motor* o, int32_t offset); // extern BOOL Motor_calibrate_moving2Hardstop(Motor* o, int32_t pwm, int32_t zero); // extern void Motor_uncalibrate(Motor* o); diff --git a/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.cpp b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.cpp index ca3124473c..c71fe95e4d 100644 --- a/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.cpp +++ b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'TorqueEstimator'. // -// Model version : 2.98 +// Model version : 2.100 // Simulink Coder version : 24.1 (R2024a) 19-Nov-2023 -// C/C++ source code generated on : Sun Sep 8 21:32:31 2024 +// 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 @@ -25,23 +25,32 @@ // 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, real_T rtu_Fs, real_T *rty_Torque, real_T *rty_Motor, real_T - *rty_Friction, DW_TorqueEstimator *localDW) + 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; - real_T tmp_1; - real_T zdot; - zdot = rtu_Velocity - std::abs(rtu_Velocity) * rtu_S0 * localDW->z / ((rtu_Fs - - rtu_Fc) * std::exp(-std::abs(rtu_Velocity / rtu_Vth)) + rtu_Fc); - localDW->z += 0.001 * zdot; + 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 = tmp; - tmp_0 = rtu_Kw * rtu_Velocity; - tmp_1 = rtu_S0 * localDW->z; - zdot *= rtu_S1; - *rty_Friction = (tmp_0 + tmp_1) + zdot; - *rty_Torque = ((tmp - tmp_0) - tmp_1) - zdot; + *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 @@ -49,8 +58,10 @@ void TorqueEstimator::step() { // MATLAB Function: '/TorqueEstimator' incorporates: // Inport: '/Current' - // Inport: '/Fc' - // Inport: '/Fs' + // Inport: '/Fc_neg' + // Inport: '/Fc_pos' + // Inport: '/Fs_neg' + // Inport: '/Fs_pos' // Inport: '/Km' // Inport: '/Kw' // Inport: '/S0' @@ -58,12 +69,13 @@ void TorqueEstimator::step() // Inport: '/Velocity' // Inport: '/Vth' // Outport: '/Friction' - // Outport: '/Motor' + // Outport: '/Motor_torque' // Outport: '/Torque' TorqueEstimator_l(rtU.Current, rtU.Velocity, rtU.Km, rtU.Kw, rtU.S0, rtU.S1, - rtU.Vth, rtU.Fc, rtU.Fs, &rtY.Torque, &rtY.Motor, - &rtY.Friction, &rtDW.sf_TorqueEstimator); + 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 diff --git a/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.h b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.h index 959fbbba2c..08f0e1c4db 100644 --- a/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.h +++ b/emBODY/eBcode/arch-arm/mbd/torque_estimator/TorqueEstimator.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'TorqueEstimator'. // -// Model version : 2.98 +// Model version : 2.100 // Simulink Coder version : 24.1 (R2024a) 19-Nov-2023 -// C/C++ source code generated on : Sun Sep 8 21:32:31 2024 +// 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 @@ -56,14 +56,16 @@ class TorqueEstimator final real_T S0; // '/S0' real_T S1; // '/S1' real_T Vth; // '/Vth' - real_T Fc; // '/Fc' - real_T Fs; // '/Fs' + 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; // '/Motor' + real_T Motor_torque; // '/Motor_torque' real_T Friction; // '/Friction' }; @@ -113,8 +115,9 @@ class TorqueEstimator final // 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, real_T rtu_Fs, real_T *rty_Torque, real_T *rty_Motor, real_T - *rty_Friction, DW_TorqueEstimator *localDW); + 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; diff --git a/emBODY/eBcode/arch-arm/mbd/torque_estimator/rtwtypes.h b/emBODY/eBcode/arch-arm/mbd/torque_estimator/rtwtypes.h index e8603ca1ef..6eaf7bfeab 100644 --- a/emBODY/eBcode/arch-arm/mbd/torque_estimator/rtwtypes.h +++ b/emBODY/eBcode/arch-arm/mbd/torque_estimator/rtwtypes.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'TorqueEstimator'. // -// Model version : 2.98 +// Model version : 2.100 // Simulink Coder version : 24.1 (R2024a) 19-Nov-2023 -// C/C++ source code generated on : Sun Sep 8 21:32:31 2024 +// 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 From 28b5161ca41c4537e282003598ffe447f7a22741 Mon Sep 17 00:00:00 2001 From: ale-git Date: Wed, 13 Nov 2024 21:09:15 +0100 Subject: [PATCH 4/5] Changes required by PR review: - LuGre model configuration moved to motor configuration - callbacks for independent configuration removed Coupled jointset management. --- .../amc/application/v2/proj/amc-icc.uvoptx | 24 ++++++- .../amc/application/v2/proj/amc-icc.uvprojx | 14 ++++- .../EoProtocolMC_fun_ems4rd.c | 13 ---- .../v2/proj/ems4rd.diagnostic2ready.uvoptx | 2 +- .../v2/proj/ems4rd.diagnostic2ready.uvprojx | 6 +- .../embobj/plus/mc/CalibrationHelperData.h | 2 +- .../arch-arm/embobj/plus/mc/Calibrators.c | 4 +- .../arch-arm/embobj/plus/mc/Controller.c | 20 +----- .../arch-arm/embobj/plus/mc/Controller.h | 1 - emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c | 2 +- .../eBcode/arch-arm/embobj/plus/mc/JointSet.c | 62 ++++++++++--------- .../eBcode/arch-arm/embobj/plus/mc/JointSet.h | 5 +- emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.c | 58 +++++++++-------- emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h | 1 - .../arch-arm/embobj/plus/mc/Motor_hid.h | 10 ++- .../arch-arm/embobj/plus/mc/Trajectory.c | 14 ++--- 16 files changed, 120 insertions(+), 118 deletions(-) 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/eoprot-callbacks/EoProtocolMC_fun_ems4rd.c b/emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoprot-callbacks/EoProtocolMC_fun_ems4rd.c index 0a71a50248..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 @@ -321,19 +321,6 @@ extern void eoprot_fun_UPDT_mc_joint_config_motor_params(const EOnv* nv, const e } } -extern void eoprot_fun_UPDT_mc_joint_config_LuGre_params(const EOnv* nv, const eOropdescriptor_t* rd) -{ // not for mc4can - eOmc_LuGre_params_t *mparams = (eOmc_LuGre_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)) - { - MController_config_LuGre_params(jxx, mparams); - } -} // f-marker-begin 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 221827a37f..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 @@ -3535,7 +3535,7 @@ 16 106 8 - 0 + 1 0 0 ..\..\..\..\..\embobj\plus\mc\Motor.c 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 8df41ef2b1..1807045650 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 @@ -4677,7 +4677,7 @@ ems-ulpro 0x4 ARM-ADS - 6190000::V6.19::ARMCLANG + 6220000::V6.22::ARMCLANG 1 @@ -4801,7 +4801,7 @@ 0 1 1 - 4100 + 4097 1 BIN\ULP2CM3.DLL @@ -5005,7 +5005,7 @@ 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;..\..\..\..\..\mbd\torque_estimator 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 30660295d1..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 @@ -1002,22 +1002,6 @@ void MController_config_motor_friction(int m, eOmc_motor_params_t* params) // Motor_config_friction(smc->motor+m, params->bemf_value, params->ktau_value, params->friction); } -void MController_config_LuGre_params(int m, eOmc_LuGre_params_t* params) // -{ - Motor_config_LuGre(smc->motor+m, - params->Km, - params->Kw, - params->S0, - params->S1, - params->Vth, - params->Fc_pos, - params->Fc_neg, - params->Fs_pos, - params->Fs_neg - ); -} - - void MController_config_joint_impedance(int j, eOmc_impedance_t* impedance) // { Joint_set_impedance(smc->joint+j, impedance); diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h index da867dfa93..ade8b5f462 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.h @@ -85,7 +85,6 @@ extern void MController_update_motor_pos_fbk(int m, int32_t position_raw); extern void MController_update_motor_current_fbk(int m, int16_t current); extern void MController_update_motor_temperature_fbk(int m, int16_t temperature); extern void MController_config_motor_friction(int m, eOmc_motor_params_t* params); // -extern void MController_config_LuGre_params(int m, eOmc_LuGre_params_t* params); extern void MController_config_joint_impedance(int j, eOmc_impedance_t* impedance); // extern void MController_config_minjerk_pid(int j, eOmc_PID_t *pid_conf); // 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 5931848472..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 // @@ -138,12 +137,6 @@ void JointSet_config // o->Jjm = Jjm; o->Sjm = Sjm; o->Jmj = Jmj; o->Smj = Smj; o->Sje = Sje; - -#if defined(SENSORLESS_TORQUE) - o->sensorless_torque = TRUE; -#else - o->sensorless_torque = FALSE; -#endif } void JointSet_do_odometry(JointSet* o) // @@ -191,34 +184,50 @@ void JointSet_do_odometry(JointSet* o) // } } } - - if (o->sensorless_torque) + +#if defined(SENSORLESS_TORQUE) + if (o->Jmj) { - for (int ms=0; msmotors_of_set[ms]; - - if (o->Jmj) + CTRL_UNITS joint_trq_fbk = ZERO; + BOOL sensorless_torque = FALSE; + + int j = o->joints_of_set[js]; + + for (int ms=0; msjoints_of_set[js]; + int m = o->motors_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 + } + } + else + { + for (int k=0; kmotors_of_set[k]; + + if (o->motor[m].sensorless_torque) { - Joint_update_torque_fbk(o->joint+m, o->motor[m].torque*1000000.0f); + 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]; @@ -1304,10 +1313,7 @@ static void JointSet_do_current_control(JointSet* o) motor_vel_kf_icubdeg_sec += o->Jmj[m][j] * o->joint[j].vel_fbk * o->motor[m].GEARBOX; } - if (o->sensorless_torque) - motor_current_ref = Motor_do_trq_control_EXPERIMENTAL(o->motor+m, motor_trq_ref, motor_trq_fbk, motor_vel_kf_icubdeg_sec); - else - motor_current_ref = Motor_do_trq_control(o->motor+m, motor_trq_ref, motor_trq_fbk, motor_vel_kf_icubdeg_sec); + motor_current_ref = Motor_do_trq_control(o->motor+m, motor_trq_ref, motor_trq_fbk, motor_vel_kf_icubdeg_sec); //char info[70]; //snprintf(info, 70, "comp %f m:%d", motor_current_ref, m); @@ -1316,10 +1322,8 @@ static void JointSet_do_current_control(JointSet* o) else { motor_vel_kf_icubdeg_sec = o->joint[m].vel_fbk * o->motor[m].GEARBOX; - if (o->sensorless_torque) - motor_current_ref = Motor_do_trq_control_EXPERIMENTAL(o->motor+m, o->joint[m].trq_ref, o->joint[m].trq_fbk, motor_vel_kf_icubdeg_sec); - else - 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); + + 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); } } else diff --git a/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h b/emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.h index cd50a60ada..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,9 +135,6 @@ typedef struct // JointSet TripodCalib tripod_calib; HardStopCalib hard_stop_calib; - - BOOL sensorless_torque; - } 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 934af2f446..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) // @@ -358,24 +364,27 @@ void Motor_config(Motor* o, uint8_t ID, eOmc_motor_config_t* config) // } #if defined(SENSORLESS_TORQUE) - o->sensorless_torque = TRUE; -#else - o->sensorless_torque = FALSE; -#endif - - if (o->sensorless_torque) + o->torque = ZERO; + + if (config->LuGre_params.Km > ZERO) { - o->torque_estimator.initialize(); - o->torque_estimator.rtU.Km = 4.3; - o->torque_estimator.rtU.Kw = 0.045; - o->torque_estimator.rtU.S0 = 2.5; - o->torque_estimator.rtU.S1 = 0.0; - o->torque_estimator.rtU.Vth = 5.0; - o->torque_estimator.rtU.Fc_pos = 3.3; - o->torque_estimator.rtU.Fc_neg = 3.3; - o->torque_estimator.rtU.Fs_pos = 7.0; - o->torque_estimator.rtU.Fs_neg = 7.0; + 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) @@ -448,19 +457,6 @@ void Motor_config_friction(Motor* o, float Bemf, float Ktau, eOmc_FrictionParams PID_config_friction(&o->trqPID, Bemf, Ktau, friction); } -void Motor_config_LuGre(Motor* o, float Km, float Kw, float S0, float S1, float Vth, float Fc_pos, float Fc_neg, float Fs_pos, float Fs_neg) -{ - o->torque_estimator.rtU.Km = Km; - o->torque_estimator.rtU.Kw = Kw; - o->torque_estimator.rtU.S0 = S0; - o->torque_estimator.rtU.S1 = S1; - o->torque_estimator.rtU.Vth = Vth; - o->torque_estimator.rtU.Fc_pos = Fc_pos; - o->torque_estimator.rtU.Fc_neg = Fc_neg; - o->torque_estimator.rtU.Fs_pos = Fs_pos; - o->torque_estimator.rtU.Fs_neg = Fs_neg; -} - void Motor_calibrate_withOffset(Motor* o, int32_t offset) // { o->pos_calib_offset = offset; @@ -919,7 +915,7 @@ BOOL Motor_check_faults(Motor* o) // fault_state.bits.EncoderFault = FALSE; } */ - + if (can_dead && !o->can_dead) { @@ -1227,6 +1223,7 @@ void Motor_update_odometry_fbk_can(Motor* o, CanOdometry2FocMsg* can_msg) // 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; @@ -1244,6 +1241,7 @@ void Motor_update_odometry_fbk_can(Motor* o, CanOdometry2FocMsg* can_msg) // 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 f6422f33d8..eb37c36552 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h +++ b/emBODY/eBcode/arch-arm/embobj/plus/mc/Motor.h @@ -58,7 +58,6 @@ extern void Motor_config_speed_PID(Motor* o, eOmc_PID_t* pid); extern void Motor_config_filter(Motor* o, uint8_t filter); // extern void Motor_config_friction(Motor* o, float Bemf, float Ktau, eOmc_FrictionParams_t friction); // -extern void Motor_config_LuGre(Motor* o, float Km, float Kw, float S0, float S1, float Vth, float Fc_pos, float Fc_neg, float Fs_pos, float Fs_neg); extern void Motor_calibrate_withOffset(Motor* o, int32_t offset); // extern BOOL Motor_calibrate_moving2Hardstop(Motor* o, int32_t pwm, int32_t zero); // extern void Motor_uncalibrate(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 a5abc22533..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,7 +20,10 @@ #ifndef MC_MOTOR_HID_H___ #define MC_MOTOR_HID_H___ -#include "TorqueEstimator.h" +#if defined(SENSORLESS_TORQUE) + #include "TorqueEstimator.h" +#endif + #include "WatchDog.h" typedef struct @@ -177,11 +180,12 @@ struct Motor_hid WatchDog can_2FOC_alive_wdog; 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; From 6223aeb1c8a74e5a23047a9d14691fbb74a6cb30 Mon Sep 17 00:00:00 2001 From: ale-git Date: Sun, 17 Nov 2024 09:18:19 +0100 Subject: [PATCH 5/5] EMS version incremented, SENSORLESS_TORQUE compilation macro enabled. --- .../board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvprojx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 1807045650..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 @@ -5005,7 +5005,7 @@ 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 ___SENSORLESS_TORQUE + 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;..\..\..\..\..\mbd\torque_estimator