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

The amc board uses ICC and CAN to move the wrist of ergoCub #948

Merged
merged 2 commits into from
Mar 5, 2024
Merged

The amc board uses ICC and CAN to move the wrist of ergoCub #948

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c5c27dc
removed support to eomn_serv_MC_mc4pluspmc, added to eomn_serv_MC_advfoc
marcoaccame Feb 26, 2024
c53922e
forced use of icub_firmware_shared VERSION 1.38.1
marcoaccame Mar 4, 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
2 changes: 1 addition & 1 deletion conf/iCubFindDependencies.cmake
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ checkandset_dependency(IPOPT)
checkandset_dependency(OpenCV)
checkandset_dependency(Qt5)

set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.38.0)
set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.38.1)

if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared)
if(icub_firmware_shared_VERSION VERSION_LESS ${MINIMUM_REQUIRED_icub_firmware_shared_VERSION})
Expand Down
254 changes: 201 additions & 53 deletions src/libraries/icubmod/embObjLib/serviceParser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -508,6 +508,116 @@ bool ServiceParser::convert(eObrd_location_t const &loc, char *str, int len)
return true;
}

bool ServiceParser::convert(std::string const &fromstring, eOlocation_t &location, bool &formaterror)
{
// it is actually a micro-parser: PRE-num

const char *t = fromstring.c_str();
int len = strlen(t);

if(len > 15)
{
yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because it is too long with size = " << len;
formaterror = true;
return false;
}

char prefix[16] = {0};
sscanf(t, "%3c", prefix);
if(0 == strcmp(prefix, "LOC"))
{
int adr = 0;
sscanf(t, "%3c:%d", prefix, &adr);
location.bus = eobus_local;
location.ffu = 0;
location.adr = adr;
}
else if((0 == strcmp(prefix, "CAN")) || (0 == strcmp(prefix, "ICC")))
{
int bus = 0;
int adr = 0;
int numberofreaditems = sscanf(t, "%3c%d:%d", prefix, &bus, &adr);

if(3 != numberofreaditems)
{
yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because we dont have correct number of sections separated by :";
formaterror = true;
return false;
}

// verify bus being eitehr 1 or 2, and adr being 1 ----- 14
if((1 != bus) && (2 != bus))
{
yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because we can have either ---1 or ---2";
formaterror = true;
return false;
}

if((0 == adr) || (adr > 14))
{
yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because address is not in range [1, 14]";
formaterror = true;
return false;
}

location.ffu = 0;
if(0 == strcmp(prefix, "CAN"))
{
location.bus = (1 == bus) ? eobus_can1 : eobus_can2;
}
else if(0 == strcmp(prefix, "ICC"))
{
location.bus = (1 == bus) ? eobus_icc1 : eobus_icc2;
}

location.adr = adr;

}
else
{
yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because it does not begin with LOC, CAN, ICC";
formaterror = true;
return false;
}

return true;
}

bool ServiceParser::convert(eOlocation_t const &loc, char *str, int len)
{
if((NULL == str) || (0 == len))
{
return false;
}

if(eobus_can1 == loc.bus)
{
snprintf(str, len, "CAN1:%d", loc.adr);
}
else if(eobus_can2 == loc.bus)
{
snprintf(str, len, "CAN2:%d", loc.adr);
}
else if(eobus_local == loc.bus)
{
snprintf(str, len, "LOC:%d", loc.adr);
}
else if(eobus_icc1 == loc.bus)
{
snprintf(str, len, "ICC1:%d", loc.adr);
}
else if(eobus_icc2 == loc.bus)
{
snprintf(str, len, "ICC2:%d", loc.adr);
}
else
{
return false;
}

return true;
}

bool ServiceParser::convert(eObrd_canlocation_t const &canloc, char *str, int len)
{
if((NULL == str) || (0 == len))
Expand Down Expand Up @@ -2243,7 +2353,6 @@ bool ServiceParser::parse_actuator_port(std::string const &fromstring, eObrd_eth

case eomc_act_foc:
case eomc_act_mc4:
case eomc_act_pmc:
{
// read it as a CAN address
eObrd_location_t loc;
Expand All @@ -2270,15 +2379,6 @@ bool ServiceParser::parse_actuator_port(std::string const &fromstring, eObrd_eth
ret = false;
}
}
else if(eomc_act_pmc == type)
{
if((eobrd_place_can != loc.any.place))
{
yWarning() << "ServiceParser::parse_actuator_port():" << t << "is not a legal string for a eomc_act_pmc location";
formaterror = true;
ret = false;
}
}

if(false == ret)
{
Expand Down Expand Up @@ -2333,10 +2433,32 @@ bool ServiceParser::parse_actuator_port(std::string const &fromstring, eObrd_eth
}
}

} break;

case eomc_act_advfoc:
{
// read it as a location
eOlocation_t loc {eobus_none, 0, 0};
bool result = convert(fromstring, loc, formaterror);
ret = true;
if(false == result)
{
yWarning() << "ServiceParser::parse_actuator_port():" << t << "is not a legal string for a eOlocation_t required by eomc_act_advfoc";
formaterror = true;
ret = false;
}

if(false == ret)
{
return ret;
}

// copy into todes.gen as it is uded by advfoc
todes.gen.location = loc;

} break;


}

return ret;
Expand Down Expand Up @@ -2922,9 +3044,9 @@ bool ServiceParser::check_motion(Searchable &config)

} break;

case eomn_serv_MC_mc4pluspmc:
case eomn_serv_MC_advfoc:
{
// must have: ETHBOARD, CANBOARDS, POS, JOINTMAPPING
// must have: ETHBOARD, CANBOARDS, JOINTMAPPING

itisoksofar = true;

Expand All @@ -2940,12 +3062,6 @@ bool ServiceParser::check_motion(Searchable &config)
itisoksofar = false;
}

if(false == has_PROPERTIES_POS)
{
yError() << "ServiceParser::check_motion() cannot find PROPERTIES.POS for type" << eomn_servicetype2string(mc_service.type);
itisoksofar = false;
}

if(false == has_PROPERTIES_JOINTMAPPING)
{
yError() << "ServiceParser::check_motion() cannot find PROPERTIES.JOINTMAPPING for type" << eomn_servicetype2string(mc_service.type);
Expand Down Expand Up @@ -3475,6 +3591,10 @@ bool ServiceParser::check_motion(Searchable &config)
yError() << "ServiceParser::check_motion() cannot find PROPERTIES.JOINTMAPPING.ACTUATOR.type";
return false;
}

Bottle b_PROPERTIES_JOINTMAPPING_ACTUATOR_onboard = Bottle(b_PROPERTIES_JOINTMAPPING_ACTUATOR.findGroup("onboard"));
// mandatory only for advfoc

Bottle b_PROPERTIES_JOINTMAPPING_ACTUATOR_port = Bottle(b_PROPERTIES_JOINTMAPPING_ACTUATOR.findGroup("port"));
if(b_PROPERTIES_JOINTMAPPING_ACTUATOR_port.isNull())
{
Expand Down Expand Up @@ -3623,6 +3743,27 @@ bool ServiceParser::check_motion(Searchable &config)
mc_service.properties.mc4joints.push_back(item);
}

if(eomn_serv_MC_advfoc == mc_service.type)
{
if(b_PROPERTIES_JOINTMAPPING_ACTUATOR_onboard.isNull())
{
yError() << "ServiceParser::check_motion() cannot find PROPERTIES.JOINTMAPPING.ACTUATOR.onboard for eomn_serv_MC_advfoc";
return false;
}

if(act.type != eomc_act_advfoc)
{
yError() << "ServiceParser::check_motion() detectde that PROPERTIES.JOINTMAPPING.ACTUATOR.type for eomn_serv_MC_advfoc id not advfoc";
return false;
}

act.advdescr.type = eomc_act_advfoc;
act.advdescr.location = act.desc.gen.location;
eObrd_cantype_t bb {};
convert(b_PROPERTIES_JOINTMAPPING_ACTUATOR_onboard.get(i+1).asString(), bb, formaterror);
act.advdescr.board.type = bb;
}


// encoder1s ...

Expand Down Expand Up @@ -4239,57 +4380,64 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)

} break;

case eomn_serv_MC_advfoc:
{
eOmn_serv_config_data_mc_advfoc_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.advfoc);

case eomn_serv_MC_mc4pluspmc:
{
eOmn_serv_config_data_mc_mc4pluspmc_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.mc4pluspmc);

// 1. ->pos
eOmn_serv_config_data_as_pos_t *pos = &data_mc->pos;

for(size_t i=0; i<mc_service.properties.poslocations.size(); i++)
{
pos->config.boardconfig[0].canloc.port = mc_service.properties.poslocations[i].port;
pos->config.boardconfig[0].canloc.addr = mc_service.properties.poslocations[i].addr;
pos->config.boardconfig[0].canloc.insideindex = mc_service.properties.poslocations[i].insideindex;
}


pos->config.boardconfig[0].boardinfo.firmware.major = mc_service.properties.canboards.at(0).firmware.major;
pos->config.boardconfig[0].boardinfo.firmware.minor = mc_service.properties.canboards.at(0).firmware.minor;
pos->config.boardconfig[0].boardinfo.firmware.build = mc_service.properties.canboards.at(0).firmware.build;
pos->config.boardconfig[0].boardinfo.protocol.major = mc_service.properties.canboards.at(0).protocol.major;
pos->config.boardconfig[0].boardinfo.protocol.minor = mc_service.properties.canboards.at(0).protocol.minor;
ret = true;

// 2. ->arrayofjomodescriptors
EOarray *arrayofjomos = eo_array_New(7, sizeof(eOmc_jomo_descriptor_t), &data_mc->arrayof7jomodescriptors);
// 1. ->eOmc_arrayof_4advjomodescriptors_t
EOarray *arrayofjomos = eo_array_New(4, sizeof(eOmc_adv_jomo_descriptor_t), &data_mc->arrayof4advjomodescriptors);
size_t numofjomos = mc_service.properties.numofjoints;

for(size_t i=0; i<numofjomos; i++)
{
eOmc_jomo_descriptor_t jomodes = {};
eOmc_adv_jomo_descriptor_t jomodes = {};

// 1. actuator is on pwm for the first 4 and on pmc on trh last 3: we need the port
if(mc_service.properties.actuators[i].type == eomc_act_pwm)
// 1. actuator is on adv: we need the board, its location and its required versions of application and protocol
if(mc_service.properties.actuators[i].type != eomc_act_advfoc)
{
jomodes.actuator.pwm.port = mc_service.properties.actuators[i].desc.pwm.port;
yError() << "ServiceParser::parseService() unknown actuator value for eomn_serv_MC_advfoc mode. we need eomc_act_advfoc";
ret = false;
continue;
}
else if(mc_service.properties.actuators[i].type == eomc_act_pmc)

// find the relevant brd inside the vector mc_service.properties.canboards that matches mc_service.properties.actuators[i].board
// so far i just use the first one

// 1. actuator...
jomodes.actuator.type = eomc_act_advfoc;
jomodes.actuator.location = mc_service.properties.actuators[i].advdescr.location; //mc_service.properties.actuators[i].desc.gen.location;
jomodes.actuator.board.type = mc_service.properties.actuators[i].advdescr.board.type;
eObrd_firmwareversion_t fw {};
eObrd_protocolversion_t pr {};
// i must search for the fw and pr given the board inside ....
bool ff {false};
for(size_t i=0; i<mc_service.properties.canboards.size(); i++)
{
jomodes.actuator.pmc.canloc.port = mc_service.properties.actuators[i].desc.pmc.canloc.port;
jomodes.actuator.pmc.canloc.addr = mc_service.properties.actuators[i].desc.pmc.canloc.addr;
jomodes.actuator.pmc.canloc.insideindex = mc_service.properties.actuators[i].desc.pmc.canloc.insideindex;
if(mc_service.properties.canboards[i].type == jomodes.actuator.board.type)
{
fw = mc_service.properties.canboards[i].firmware;
pr = mc_service.properties.canboards[i].protocol;
ff = true;
break;
}
}
else

if(false == ff)
{
yError() << "ServiceParser::parseService() unknown vatuator value for eomn_serv_MC_mc4pluspmc mode";
yError() << "ServiceParser::parseService() could not find the actuator board inside the mc_service.properties.canboards[] vector";
return false;
}

jomodes.actuator.board.firmware = fw;
jomodes.actuator.board.protocol = pr;

// 2. encoder1 is ...
jomodes.encoder1.type = mc_service.properties.encoder1s[i].desc.type;
jomodes.encoder1.port = mc_service.properties.encoder1s[i].desc.port;
jomodes.encoder1.pos = mc_service.properties.encoder1s[i].desc.pos;

// 3. encoder2 is ...
jomodes.encoder2.type = mc_service.properties.encoder2s[i].desc.type;
jomodes.encoder2.port = mc_service.properties.encoder2s[i].desc.port;
Expand All @@ -4299,7 +4447,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig)
}

// ok, everything is done
ret = true;
ret = ret; // keep it as ret = ret ...

} break;

Expand Down Expand Up @@ -4739,7 +4887,7 @@ servMC_encoder_t * ServiceParser::getEncoderAtMotor(int index)
{ // joint 0: jaw
.actuator.pwm =
{
.port = 0 // eomn_serv_mc_port_mc4plus_pwmP3 is hal_motor1=0 ?? verify!
ret = ret; .port = 0 // eomn_serv_mc_port_mc4plus_pwmP3 is hal_motor1=0 ?? verify!
},
.sensor =
{
Expand Down
3 changes: 3 additions & 0 deletions src/libraries/icubmod/embObjLib/serviceParser.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -215,6 +215,7 @@ typedef struct
{
eOmc_actuator_t type;
eOmc_actuator_descriptor_t desc;
eOmc_adv_actuator_descriptor_t advdescr;
} servMC_actuator_t;


Expand Down Expand Up @@ -335,9 +336,11 @@ class ServiceParser
bool convert(std::string const &fromstring, std::string &str, bool &formaterror);
bool convert(std::string const &fromstring, const uint8_t strsize, char *str, bool &formaterror);
bool convert(std::string const &fromstring, eObrd_location_t &location, bool &formaterror);
bool convert(std::string const &fromstring, eOlocation_t &location, bool &formaterror);


bool convert(eObrd_location_t const &loc, char *str, int len);
bool convert(eOlocation_t const &loc, char *str, int len);
bool convert(eObrd_canlocation_t const &canloc, char *str, int len);

bool convert(eObrd_protocolversion_t const &prot, char *str, int len);
Expand Down
Loading
Loading