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

Runtime configuration of mtb4 / mtb4c boards to manage ergoCub hand (5 fingers + palm) #530

Merged
merged 7 commits into from
Oct 30, 2024
+56 −52
Merged

Runtime configuration of mtb4 / mtb4c boards to manage ergoCub hand (5 fingers + palm) #530

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
51291f3
MTB 5th I2C and palm skin - modified remapping + new skintype for onl…
simeonedussoni Oct 15, 2024
317e89e
mtb4/c implemented new skin type for runtime reshuffle to include 5th…
simeonedussoni Oct 16, 2024
8ae1ea4
mtb4/c cleanup of comments and indented code frm the 5th i2c implemen…
simeonedussoni Oct 29, 2024
3814173
substituted all the tabs inside embot_app_application_theSkin.cpp w/ …
marcoaccame Oct 30, 2024
3b2f176
removed masking of bcfg->skintype w/ 0x07 because valid values of ski…
marcoaccame Oct 30, 2024
f80a7f5
removed the constraint of using compiler V6.19 because since https://…
marcoaccame Oct 30, 2024
eb616bf
added description of SKIN_SET_BRD_CFG.skinType + clarified how the tr…
marcoaccame Oct 30, 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
14 changes: 7 additions & 7 deletions emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvoptx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -722,7 +722,7 @@

<Group>
<GroupName>rtos</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -754,7 +754,7 @@

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

<Group>
<GroupName>embot-app-protocol</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -1054,7 +1054,7 @@

<Group>
<GroupName>embot-application</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -1170,7 +1170,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 @@ -1358,7 +1358,7 @@

<Group>
<GroupName>other-hw</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down Expand Up @@ -1434,7 +1434,7 @@

<Group>
<GroupName>embot::hw::bsp</GroupName>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
Expand Down
5 changes: 2 additions & 3 deletions emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvprojx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -873,8 +873,7 @@
<TargetName>mtb4-v1D3</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<pArmCC>6190000::V6.19::.\armclang-r6p19-00rel0</pArmCC>
<pCCUsed>6190000::V6.19::.\armclang-r6p19-00rel0</pCCUsed>
<pCCUsed>6220000::V6.22::ARMCLANG</pCCUsed>
<uAC6>1</uAC6>
<TargetOption>
<TargetCommonOption>
Expand Down Expand Up @@ -1202,7 +1201,7 @@
<v6Rtti>0</v6Rtti>
<VariousControls>
<MiscControls>-Wno-pragma-pack -Wno-deprecated-register</MiscControls>
<Define>USE_STM32HAL STM32HAL_BOARD_MTB4 STM32HAL_DRIVER_V1D3 ENABLE_FAPreader</Define>
<Define>USE_STM32HAL STM32HAL_BOARD_MTB4 STM32HAL_DRIVER_V1D3 ENABLE_FAPreader dUSE_FIFTH_I2C</Define>
<Undefine></Undefine>
<IncludePath>..\src-plus;..\..\..\..\..\..\eBcode\arch-arm\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\eBcode\arch-arm\libs\midware\eventviewer\api;..\..\..\..\..\..\eBcode\arch-arm\embobj\core\exec\multitask;..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\core\core;..\..\..\..\..\..\eBcode\arch-arm\libs\lowlevel\stm32hal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\..\eBcode\arch-arm\embot\app;..\..\..\..\..\..\eBcode\arch-arm\embot\i2h;..\..\..\..\..\..\eBcode\arch-arm\embot\hw;..\..\..\..\..\..\eBcode\arch-arm\embot\os;..\..\..\..\..\..\eBcode\arch-arm\embot;..\src\others;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\embot\app\skeleton;..\..\..\..\embot\prot\can;..\..\bsp</IncludePath>
</VariousControls>
Expand Down
8 changes: 2 additions & 6 deletions emBODY/eBcode/arch-arm/board/mtb4/application/src/main-appcan.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,18 +15,14 @@
constexpr embot::app::theCANboardInfo::applicationInfo applInfo
{
#if defined(CUSTOMIZATION_MTB4_FOR_TLR)
embot::prot::can::versionOfAPPLICATION {20, 21, 0},
embot::prot::can::versionOfAPPLICATION {20, 23, 0},
embot::prot::can::versionOfCANPROTOCOL {20, 0}
#elif defined(USE_FIFTH_I2C)
embot::prot::can::versionOfAPPLICATION {51, 22, 0},
embot::prot::can::versionOfCANPROTOCOL {2, 0}
#else
embot::prot::can::versionOfAPPLICATION {1, 22, 0},
embot::prot::can::versionOfAPPLICATION {1, 23, 0},
embot::prot::can::versionOfCANPROTOCOL {2, 0}
#endif
};

//#define USE_FIFTH_I2C

constexpr std::uint16_t threadIDLEstacksize = 512;
constexpr std::uint16_t threadINITstacksize = 2048;
Expand Down
11 changes: 1 addition & 10 deletions emBODY/eBcode/arch-arm/board/mtb4/application/src/others/AD7147.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,9 +94,6 @@ static if2hw_data_ad7147_t (*s_CapOffset)[12] = CapOffset;
#endif

uint16_t SKV = 0;
uint16_t taxsel0 = 0;
uint16_t taxsel4 = 0;
uint16_t taxsel5 = 0;

static uint32_t triangleconnectionmask = 0xffff;

Expand Down Expand Up @@ -153,7 +150,6 @@ void ServiceAD7147Isr(unsigned char Channel)
{
unsigned int i=0;
// code added SD + Marco Accame
constexpr size_t nI2Clines {1};
constexpr size_t NumberOfRegistertoRead {12};
for (i=0;i<4;i++)// this 4 is number of addresses on each I2C line
{
Expand All @@ -162,21 +158,16 @@ void ServiceAD7147Isr(unsigned char Channel)
s_AD7147Registers[i+4],
s_AD7147Registers[i+8],
s_AD7147Registers[i+12],
// s_AD7147Registers[i+16],
s_AD7147Registers_5th[i],
0);
}
static int x = 0;
x++;
if(x > 11) x =0;
#ifdef USE_FIFTH_I2C
#ifdef DEBUG_FIFTH_I2C
SKV = s_AD7147Registers_5th[0][x];
#endif
// SKV = s_AD7147Registers[16][x];

taxsel0 = s_AD7147Registers_5th[0][0];
taxsel4 = s_AD7147Registers_5th[0][4];
taxsel5 = s_AD7147Registers_5th[0][5];

}

Expand Down
4 changes: 2 additions & 2 deletions emBODY/eBcode/arch-arm/board/mtb4/application/src/others/I2C_Multi_SDA.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -996,7 +996,7 @@ void StartI2CMaster(unsigned char Channel) {
#warning marco.accame: why param input is not used?
void SetSdaReg(i2c_sda_num_t sdaNum, unsigned char input)
{
if(sdaNum < 4)
if(sdaNum < 5)
{
LL_GPIO_SetPinMode(props.sda[sdaNum].stmport, props.sda[sdaNum].stmpin, LL_GPIO_MODE_INPUT);
}
Expand All @@ -1005,7 +1005,7 @@ void SetSdaReg(i2c_sda_num_t sdaNum, unsigned char input)

void SetValReg(i2c_sda_num_t sdaNum, unsigned char val)
{
if(sdaNum < 4)
if(sdaNum < 5)
{ // because val=0 -> GPIO_PIN_RESET else -> GPIO_PIN_SET
HAL_GPIO_WritePin(props.sda[sdaNum].stmport, props.sda[sdaNum].stmpin, (0==val) ? (GPIO_PIN_RESET) : (GPIO_PIN_SET));
}
Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvoptx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1312,7 +1312,7 @@
<GroupNumber>12</GroupNumber>
<FileNumber>58</FileNumber>
<FileType>8</FileType>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\..\..\mtb4\application\src\others\AD7147.c</PathWithFileName>
Expand Down
3 changes: 1 addition & 2 deletions emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvprojx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -896,8 +896,7 @@
<TargetName>mtb4c-v1D3</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<pArmCC>6190000::V6.19::.\armclang-r6p19-00rel0</pArmCC>
<pCCUsed>6190000::V6.19::.\armclang-r6p19-00rel0</pCCUsed>
<pCCUsed>6220000::V6.22::ARMCLANG</pCCUsed>
<uAC6>1</uAC6>
<TargetOption>
<TargetCommonOption>
Expand Down
5 changes: 1 addition & 4 deletions emBODY/eBcode/arch-arm/board/mtb4c/application/src/main-appcan.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,8 @@ constexpr embot::app::theCANboardInfo::applicationInfo applInfo
#if defined(CUSTOMIZATION_MTB4_FOR_TLR)
embot::prot::can::versionOfAPPLICATION {22, 0, 0},
embot::prot::can::versionOfCANPROTOCOL {20, 0}
#elif defined(USE_FIFTH_I2C)
embot::prot::can::versionOfAPPLICATION {52, 2, 0},
embot::prot::can::versionOfCANPROTOCOL {2, 0}
#else
embot::prot::can::versionOfAPPLICATION {2, 2, 0},
embot::prot::can::versionOfAPPLICATION {2, 3, 0},
embot::prot::can::versionOfCANPROTOCOL {2, 0}
#endif
};
Expand Down
1 change: 0 additions & 1 deletion emBODY/eBcode/arch-arm/board/mtb4c/bsp/embot_hw_bsp_mtb4c.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -657,7 +657,6 @@ namespace embot { namespace hw { namespace multisda {
{ SCK0_GPIO_Port, SCK0_Pin, nullptr },
// sda
{{
// { SDA0_GPIO_Port, SDA0_Pin, nullptr }, { SDA1_GPIO_Port, SDA1_Pin, nullptr }, { SDA2_GPIO_Port, SDA2_Pin, nullptr }, { SDA3_GPIO_Port, SDA3_Pin, nullptr }
{ SDA0_GPIO_Port, SDA0_Pin, nullptr }, { SDA1_GPIO_Port, SDA1_Pin, nullptr }, { SDA2_GPIO_Port, SDA2_Pin, nullptr }, { SDA3_GPIO_Port, SDA3_Pin, nullptr } , {GPIOC, GPIO_PIN_0, nullptr } /*as an alternative use the definitiin from cubeMX in main.h */
}}
};
Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/embobj/plus/can/config-can-protocol/EoCANprotASpolling.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -290,7 +290,7 @@ extern eOresult_t eocanprotASpolling_former_POL_SK_CMD__SET_BRD_CFG(eOcanprot_de
icubCanProto_skinboard_config_t* bcfg = (icubCanProto_skinboard_config_t *)descriptor->cmd.value;
/* 2) set can command (see SkinPrototype::calibrateSensor)*/
frame->data[0] = ICUBCANPROTO_POL_SK_CMD__SET_BRD_CFG;
frame->data[1] = (bcfg->skintype &0x07);
frame->data[1] = bcfg->skintype;
frame->data[2] = bcfg->period;
frame->data[3] = bcfg->noload;

Expand Down
44 changes: 31 additions & 13 deletions emBODY/eBcode/arch-arm/embot/app/embot_app_application_theSkin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,11 +62,10 @@
std::uint16_t dbg = 0;
constexpr std::uint8_t dotNumberOf = 12;
constexpr std::uint8_t trgNumberOf = 16; //4 for each sda. This is used in the CAN message forming so it's locked to 16
#ifdef USE_FIFTH_I2C
uint16_t trianglesOrder[16] = {0,4,8,12,16,1,2,3,5,6,7,9,10,11,13,14};
#else
constexpr std::uint16_t trianglesOrder[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
#endif
uint16_t trianglesOrder[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
uint16_t origTrianglesOrder[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}; //consecutive ordergin of triangles used in iCub skin patches
uint16_t altTrianglesOrder[16] = {0,4,8,12,16,10,11,13,15,1,2,3,5,6,7,9}; //to reshuffle the triangles on-the-run with the ergoCub hand mapping


struct TriangleCfg
{
Expand Down Expand Up @@ -111,16 +110,11 @@ struct Triangles
}
};

//uint16_t testraw[20][12] = {0};
//uint16_t testoff[20][12] = {0};

const std::uint8_t Triangles::GAIN[dotNumberOf] = {70 ,96, 83, 38, 38, 70, 0, 45, 77, 164, 0, 77}; // these gains are moltiplied by 128 with respect to matlab
const std::uint8_t Triangles::GAIN_PALM[dotNumberOf] = {0 ,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // these gains are moltiplied by 128 with respect to matlab
const std::uint8_t Triangles::GAIN_V2[dotNumberOf] = {58 , 91, 39, 23, 35, 90, 0, 38, 68, 115, 0, 86}; // from simeone.dussoni on 30jul19


// static const std::uint8_t GAIN[12] = {70 ,96, 83, 38, 38, 70, 0, 45, 77, 164, 0, 77}; // these gains are moltiplied by 128 with respect to matlab
// static const std::uint8_t GAIN_PALM[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

struct embot::app::application::theSkin::Impl
{
Expand Down Expand Up @@ -342,7 +336,9 @@ int embot::app::application::theSkin::Impl::computedrift(std::uint8_t trg, std::
case embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::withTemperatureCompensation:
case embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::palmFingerTip:
case embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::withTemperatureCompensationV2:
case embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::ergoHand:
{

if(embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::withTemperatureCompensation == boardconfig.skintype)
{
gain = triangles.GAIN[dot];
Expand All @@ -353,14 +349,28 @@ int embot::app::application::theSkin::Impl::computedrift(std::uint8_t trg, std::
gain = triangles.GAIN_PALM[dot];
posTemp = 11;
}
else if(embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::ergoHand == boardconfig.skintype)
{
if(trg>4)
{ //are the indexes of the triangles
gain = triangles.GAIN_V2[dot];
posTemp = 6;
}
else
{ // are the indexes of the 3DMID
gain = triangles.GAIN_PALM[dot];
posTemp = 11;
}
}
else
{
gain = triangles.GAIN_V2[dot];
posTemp = 6;
}

Tpad_base = triangles.capoffsets[trg][posTemp]; // in original mtb3 code there is ADCRESULT_S6 which is defined as 6
Tpad = triangles.rawvalues[trg][posTemp];
// temperature compensation does not apply to fingertips i.e. triangles 0,4,8,12,16 thus this module-16 op is a bug but not having any effect:
Tpad_base = triangles.capoffsets[trianglesOrder[trg]%16][posTemp]; // in original mtb3 code there is ADCRESULT_S6 which is defined as 6
Tpad = triangles.rawvalues[trianglesOrder[trg]%16][posTemp];

if(Tpad > Tpad_base)
{
Expand Down Expand Up @@ -388,7 +398,15 @@ int embot::app::application::theSkin::Impl::computedrift(std::uint8_t trg, std::
bool embot::app::application::theSkin::Impl::fill(embot::prot::can::skin::periodic::Message_TRG::Info &info, const std::uint8_t trg)
{
bool ret = true;

if(embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::ergoHand == boardconfig.skintype)
{
for (uint8_t i = 0; i<16; i++) trianglesOrder[i] = altTrianglesOrder[i];
}
else
{
for (uint8_t i = 0; i<16; i++) trianglesOrder[i] = origTrianglesOrder[i];
}

if2hw_data_ad7147_t *the12rawvalues = ad7147_get12rawvaluesoftriangle(trianglesOrder[trg]);
if2hw_data_ad7147_t *the12capoffsets = ad7147_get12capoffsetsoftriangle(trianglesOrder[trg]);

Expand Down
7 changes: 6 additions & 1 deletion emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -266,7 +266,12 @@ namespace embot { namespace prot { namespace can { namespace analog { namespace
{
info.skintype = SkinType::withTemperatureCompensationV2;
} break;


case static_cast<std::uint8_t>(SkinType::ergoHand):
{
info.skintype = SkinType::ergoHand;
} break;

default:
{
info.skintype = SkinType::none;
Expand Down
2 changes: 1 addition & 1 deletion emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -348,7 +348,7 @@ namespace embot { namespace prot { namespace can { namespace analog { namespace
{
public:

enum class SkinType { withTemperatureCompensation = 0, palmFingerTip = 1, withoutTempCompensation = 2, testmodeRAW = 7, withTemperatureCompensationV2 = 8, none = 254 };
enum class SkinType { withTemperatureCompensation = 0, palmFingerTip = 1, withoutTempCompensation = 2, ergoHand = 3, testmodeRAW = 7, withTemperatureCompensationV2 = 8, none = 254};

struct Info
{
Expand Down
Binary file modified emBODY/eBdocs/arch-arm/TSD-ICUBUNIT-canprotocol-sensorboards.docx
View file
Open in desktop
Binary file not shown.