void HeadSampler::doLoop()
{
int ret1,ret2;
ret1 = head->getPositions(positions.data());
ret2 = head->getRefPositions(references.data());
positionsPort.Content()=positions;
#ifdef __HEAD_SAMPLER_VERBOSE__
static int count=0;
count++;
if (count%50==0)
{
count = 0;
int x = (int) (positions[0]+0.5);
int y = (int) (positions[1]+0.5);
fprintf(stderr, "current pos: %d %d\n", x, y);
}
#endif
if (dumpFile!=NULL)
{
for(int k=0;k<_nj;k++)
fprintf(dumpFile, "%lf\t", positions[k]);
for(int k=0;k<_nj;k++)
fprintf(dumpFile, "%lf\t", references[k]);
fprintf(dumpFile, "\n");
}
positionsPort.Write();
}
void _parseFile(char *path, myList &list, YVector &home)
{
YARPConfigFile cfg(path, "demo.cfg");
printf("%s\n", path);
int n = __nj;
int length;
home.Resize(__nj);
if (cfg.get("[POS]", "N=", &length)==YARP_FAIL)
printf("Error reading file\n");
cfg.get("[POS]", "Home=", home.data(),__nj);
YVector tmp(__nj);
for(int i = 1; i <= length; i++)
{
char k[80];
sprintf(k, "WayPoint%d=", i);
cfg.get("[POS]", k, tmp.data(),__nj);
list.push_back(tmp);
}
/// BEGIN DEBUG
printf("N=%d\n", length);
printf("Home: ");
for(int i = 1; i<=__nj; i++)
printf("%lf\t", home(i));
printf("\n");
myList::iterator it,end;
it = list.begin();
end = list.end();
while(it!=end)
{
for(int i = 1; i<=__nj; i++)
printf("%lf\t", (*it)(i));
printf("\n");
it++;
}
}
void _setPositions(YVector &v)
{
arm->setPositions(__nj, v.data());
}
void _setPositions(YVector &v)
{
arm->setPositions(v.Length(), v.data());
}