Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Sensorless torque estimation added to motor control #527

Merged
merged 5 commits into from
Jan 9, 2025
Merged

Sensorless torque estimation added to motor control #527

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
e2df9ac
Torque sensorless estimation added to devel branch of EMS firmware.
ale-git Sep 8, 2024
c56996a
LuGre parameters added to EMS board configuration protocol.
ale-git Sep 19, 2024
7229613
Added positive and negative rotation parameters for Coulomb and Strib…
ale-git Oct 22, 2024
28b5161
Changes required by PR review:
ale-git Nov 13, 2024
6223aeb
EMS version incremented, SENSORLESS_TORQUE compilation macro enabled.
ale-git Nov 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 22 additions & 2 deletions emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvoptx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -379,7 +379,7 @@

<Group>
<GroupName>embot::app::eth::config</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -563,7 +563,7 @@

<Group>
<GroupName>embot::hw</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -2733,4 +2733,24 @@
</File>
</Group>

<Group>
<GroupName>mbd::torque-estimator</GroupName>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>41</GroupNumber>
<FileNumber>182</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp</PathWithFileName>
<FilenameWithoutPath>TorqueEstimator.cpp</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>

</ProjectOpt>
14 changes: 12 additions & 2 deletions emBODY/eBcode/arch-arm/board/amc/application/v2/proj/amc-icc.uvprojx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
<TargetName>amc-icc-osal-ulpro</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<pCCUsed>6190000::V6.19::.\armclang-r6p19-00rel0</pCCUsed>
<pCCUsed>6220000::V6.22::ARMCLANG</pCCUsed>
<uAC6>1</uAC6>
<TargetOption>
<TargetCommonOption>
Expand Down Expand Up @@ -340,7 +340,7 @@
<MiscControls>-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</MiscControls>
<Define>USE_ICC_COMM USE_EMBOT_HW EMBOBJ_USE_EMBOT USE_STM32HAL STM32HAL_BOARD_AMC STM32HAL_DRIVER_V1A0 WRIST_MK2</Define>
<Undefine></Undefine>
<IncludePath>..\..\..\..\..\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</IncludePath>
<IncludePath>..\..\..\..\..\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</IncludePath>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just a clarification: the project of the amc in here contains TorqueEstimator.cpp but it does not use it because SENSORLESS_TORQUE is not defined. It can be OK.

</VariousControls>
</Cads>
<Aads>
Expand Down Expand Up @@ -1745,6 +1745,16 @@
</File>
</Files>
</Group>
<Group>
<GroupName>mbd::torque-estimator</GroupName>
<Files>
<File>
<FileName>TorqueEstimator.cpp</FileName>
<FileType>8</FileType>
<FilePath>..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp</FilePath>
</File>
</Files>
</Group>
</Groups>
</Target>
</Targets>
Expand Down
5 changes: 2 additions & 3 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoemsappl/EOMtheEMSapplCfg_cfg.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,8 +81,7 @@ extern "C" {
#define EOMTHEEMSAPPLCFG_VERSION_MAJOR (VERSION_MAJOR_OFFSET+3)
// <o> minor <0-255>
// <o> minor <0-255>
#define EOMTHEEMSAPPLCFG_VERSION_MINOR 96

#define EOMTHEEMSAPPLCFG_VERSION_MINOR 98
// </h>version

// <h> build date
Expand All @@ -91,7 +90,7 @@ extern "C" {
// <o> month <1-12>
#define EOMTHEEMSAPPLCFG_BUILDDATE_MONTH 12
// <o> day <1-31>
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 02
#define EOMTHEEMSAPPLCFG_BUILDDATE_DAY 24
// <o> hour <0-23>
#define EOMTHEEMSAPPLCFG_BUILDDATE_HOUR 18
// <o> minute <0-59>
Expand Down
1 change: 1 addition & 0 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/cfg/eoprot-callbacks/EoProtocolMC_fun_ems4rd.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
34 changes: 27 additions & 7 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvoptx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2147,7 +2147,7 @@

<Group>
<GroupName>main</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand All @@ -2167,7 +2167,7 @@

<Group>
<GroupName>abslayer-lib</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -2223,7 +2223,7 @@

<Group>
<GroupName>abslayer-cfg</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -2315,7 +2315,7 @@

<Group>
<GroupName>eo-core</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -2599,7 +2599,7 @@

<Group>
<GroupName>eo-core-mee</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -2723,7 +2723,7 @@

<Group>
<GroupName>eo-arm-env</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -3535,7 +3535,7 @@
<GroupNumber>16</GroupNumber>
<FileNumber>106</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\..\..\..\..\embobj\plus\mc\Motor.c</PathWithFileName>
Expand Down Expand Up @@ -4449,6 +4449,26 @@
</File>
</Group>

<Group>
<GroupName>mbd::torque-estimator</GroupName>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>28</GroupNumber>
<FileNumber>175</FileNumber>
<FileType>8</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp</PathWithFileName>
<FilenameWithoutPath>TorqueEstimator.cpp</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>

<Group>
<GroupName>::CMSIS</GroupName>
<tvExp>0</tvExp>
Expand Down
46 changes: 43 additions & 3 deletions emBODY/eBcode/arch-arm/board/ems004/appl/v2/proj/ems4rd.diagnostic2ready.uvprojx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1383,6 +1383,16 @@
</File>
</Files>
</Group>
<Group>
<GroupName>mbd::torque-estimator</GroupName>
<Files>
<File>
<FileName>TorqueEstimator.cpp</FileName>
<FileType>8</FileType>
<FilePath>..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>::CMSIS</GroupName>
<GroupOption>
Expand Down Expand Up @@ -2925,6 +2935,16 @@
</File>
</Files>
</Group>
<Group>
<GroupName>mbd::torque-estimator</GroupName>
<Files>
<File>
<FileName>TorqueEstimator.cpp</FileName>
<FileType>8</FileType>
<FilePath>..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>::CMSIS</GroupName>
<GroupOption>
Expand Down Expand Up @@ -4569,6 +4589,16 @@
</File>
</Files>
</Group>
<Group>
<GroupName>mbd::torque-estimator</GroupName>
<Files>
<File>
<FileName>TorqueEstimator.cpp</FileName>
<FileType>8</FileType>
<FilePath>..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>::CMSIS</GroupName>
<GroupOption>
Expand Down Expand Up @@ -4771,7 +4801,7 @@
<RunIndependent>0</RunIndependent>
<UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
<Capability>1</Capability>
<DriverSelection>4100</DriverSelection>
<DriverSelection>4097</DriverSelection>
</Flash1>
<bUseTDR>1</bUseTDR>
<Flash2>BIN\ULP2CM3.DLL</Flash2>
Expand Down Expand Up @@ -4975,9 +5005,9 @@
<v6Rtti>0</v6Rtti>
<VariousControls>
<MiscControls>-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</MiscControls>
<Define>DIAGNOSTIC2_enabled DIAGNOSTIC2_receive_from_daemon DIAGNOSTIC2_send_to_yarprobotinterface _no_DIAGNOSTIC2_send_to_daemon</Define>
<Define>DIAGNOSTIC2_enabled DIAGNOSTIC2_receive_from_daemon DIAGNOSTIC2_send_to_yarprobotinterface _no_DIAGNOSTIC2_send_to_daemon SENSORLESS_TORQUE</Define>
<Undefine></Undefine>
<IncludePath>..\..\..\..\..\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</IncludePath>
<IncludePath>..\..\..\..\..\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</IncludePath>
</VariousControls>
</Cads>
<Aads>
Expand Down Expand Up @@ -6162,6 +6192,16 @@
</File>
</Files>
</Group>
<Group>
<GroupName>mbd::torque-estimator</GroupName>
<Files>
<File>
<FileName>TorqueEstimator.cpp</FileName>
<FileType>8</FileType>
<FilePath>..\..\..\..\..\mbd\torque_estimator\TorqueEstimator.cpp</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>::CMSIS</GroupName>
<GroupOption>
Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/CalibrationHelperData.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
4 changes: 2 additions & 2 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Calibrators.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
4 changes: 2 additions & 2 deletions emBODY/eBcode/arch-arm/embobj/plus/mc/Controller.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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; s<o->nSets; ++s)
{
JointSet_config
Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/Joint.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
46 changes: 45 additions & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/mc/JointSet.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 //
Expand Down Expand Up @@ -185,6 +184,50 @@ void JointSet_do_odometry(JointSet* o) //
}
}
}

#if defined(SENSORLESS_TORQUE)
if (o->Jmj)
{
for (int js=0; js<N; ++js)
{
CTRL_UNITS joint_trq_fbk = ZERO;
BOOL sensorless_torque = FALSE;

int j = o->joints_of_set[js];

for (int ms=0; ms<N; ++ms)
{
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
{
for (int k=0; k<N; ++k)
{
int m = o->motors_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];

Expand Down Expand Up @@ -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);
}
}
Expand Down
Loading