|
Hormodular 1.0
Locomotion gait discovery by using digital hormones on modular robots
|
|
Hormodular 1.0
Locomotion gait discovery by using digital hormones on modular robots
|
Process the incoming hormones, and discover the different module IDs from them.
{
//-- Ping Hormones processing & sending
//-----------------------------------------------------------------------------------------------------
std::vector<Connector *> activeConnectors;
std::vector<int> activeConnectorsIndex;
//-- Find local ID from "Ping" hormones
unsigned int tempID = 0;
for (int i = 0; i < (int) connectors.size(); i++)
if ( connectors[i] != NULL )
{
bool foundPingHormone = false;
for (int j = 0; j < (int) connectors[i]->getInputBuffer().size(); j++)
{
if ( connectors[i]->getInputBuffer()[j].getType() == Hormone::PING_HORMONE )
{
Hormone pingHormone = connectors[i]->getInputBuffer()[j];
tempID += (pingHormone.getSourceConnector() + Orientation::getRelativeOrientation(i, orientation, Orientation(pingHormone.getData()))* 4) * pow(17, i);
foundPingHormone = true;
activeConnectorsIndex.push_back(i);
activeConnectors.push_back(connectors[i]);
break;
}
}
if (!foundPingHormone)
tempID += 16 * pow( 17, i);
}
else
{
std::cerr << "[Module] Error ocurred when accesing connector " << i << ", conector doesn't exist."
<< std::endl;
return false;
}
id = tempID;
//-- Set ping hormones on the outputBuffer of the connectors
for (int i = 0; i < (int) connectors.size(); i++)
connectors[i]->addOutputHormone( Hormone( i, Hormone::PING_HORMONE, orientation.str()));
//-- Leg hormones processing & sending
//-------------------------------------------------------------------------------------------------------
std::vector<int> legHormoneReceivedConnectors;
std::vector<int> legHormoneNotReceivedConnectors;
bool headModule = false;
bool legModule = false;
for (int i = 0; i < (int) activeConnectors.size(); i++)
for (int j = 0; j < (int) activeConnectors[i]->getInputBuffer().size(); j++)
if ( activeConnectors[i]->getInputBuffer()[j].getType() == Hormone::LEG_HORMONE )
{
legHormoneReceivedConnectors.push_back(i);
break;
}
else
{
legHormoneNotReceivedConnectors.push_back(i);
break;
}
//-- Set leg hormones on the outputBuffer of the connectors
if( activeConnectors.size() != 0 )
{
if ( activeConnectors.size() == 1)
{
//-- This case is for the 'leg' modules, that have to generate the leg hormone flux
//std::cout << "Hey, I'm a leg module! (I am module with id: " << id << ")" << std::endl;
legModule = true;
activeConnectors[0]->addOutputHormone( Hormone( activeConnectorsIndex[0], Hormone::LEG_HORMONE ));
}
else if ( activeConnectors.size() == legHormoneReceivedConnectors.size() )
{
//-- If a module receives 'leg' hormones in all its active connectors, it is the head module
//std::cout << "Hey, I'm the head module! (I am module with id: " << id << ")" << std::endl;
headModule = true;
if (legHormoneReceivedConnectors.size() == 2)
{
//std::cout << "I'm MultiDof-11-2!" << std::endl;
configurationId = 0;
}
else if ( legHormoneReceivedConnectors.size() == 3 )
{
//std::cout << "I'm MultiDof-7-Tripod!" << std::endl;
configurationId = 1;
}
else if ( legHormoneReceivedConnectors.size() == 4 )
{
//std::cout << "I'm MultiDof-9-Quad!" << std::endl;
configurationId = 2;
}
}
else if ( legHormoneReceivedConnectors.size() > 0)
{
//-- Otherwise, relay the hormones in all the active connectors that didn't receive leg hormones
for (int i = 0; i < (int) legHormoneNotReceivedConnectors.size(); i++)
activeConnectors[legHormoneNotReceivedConnectors[i]]->addOutputHormone(
Hormone( activeConnectorsIndex[legHormoneNotReceivedConnectors[i]], Hormone::LEG_HORMONE ));
}
}
//-- "Head" modules processing & sending
//-------------------------------------------------------------------------------------------------------
if (headModule)
{
//-- Generate head hormones to tell the other modules who the hell are they
for (int i = 0; i < (int) activeConnectors.size(); i++)
{
std::stringstream configurationIdStr;
configurationIdStr << configurationId;
configurationIdStr << " " << activeConnectorsIndex[i];
activeConnectors[i]->addOutputHormone( Hormone( activeConnectorsIndex[i],
Hormone::HEAD_HORMONE,
configurationIdStr.str()));
}
}
else
{
std::vector<int> headHormoneReceivedConnectors;
std::vector<int> headHormoneNotReceivedConnectors;
std::string hormoneData;
std::vector<std::string> splitHormoneData;
//-- If there is any head hormone
for (int i = 0; i < (int) activeConnectors.size(); i++)
for (int j = 0; j < (int) activeConnectors[i]->getInputBuffer().size(); j++)
if ( activeConnectors[i]->getInputBuffer()[j].getType() == Hormone::HEAD_HORMONE )
{
headHormoneReceivedConnectors.push_back(i);
hormoneData = activeConnectors[i]->getInputBuffer()[j].getData();
splitHormoneData = splitString(hormoneData);
configurationId = atoi(splitHormoneData[0].c_str());
if (legModule)
id = 83521 + atoi(splitHormoneData[1].c_str());
break;
}
else
{
headHormoneNotReceivedConnectors.push_back(i);
break;
}
if( headHormoneReceivedConnectors.size() > 0 )
for (int i = 0; i < (int) headHormoneNotReceivedConnectors.size(); i++)
{
if ( configurationId == 0 && ( id == 31466 || id == 30752) )
{
std::stringstream configurationIdStr;
configurationIdStr << configurationId;
configurationIdStr << " " << i + atoi(splitHormoneData[1].c_str());
hormoneData = configurationIdStr.str();
}
Connector * outputConnector = activeConnectors[headHormoneNotReceivedConnectors[i]];
outputConnector->addOutputHormone( Hormone( activeConnectorsIndex[headHormoneNotReceivedConnectors[i]],
Hormone::HEAD_HORMONE,
hormoneData));
}
}
//-- Clean input buffers
for (int i = 0; i < (int) connectors.size(); i++)
connectors[i]->clearInputBuffer();
//std::cout << "[Debug] Id: " << id << "-> " << configurationId << std::endl;
return true;
}
Here is the call graph for this function:
 |
1.7.4
