Example #1
0
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();
}
Example #2
0
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++;
	}

}
Example #3
0
 void _setPositions(YVector &v)
 {
   arm->setPositions(__nj, v.data());
 }
Example #4
0
 void _setPositions(YVector &v)
 {
   arm->setPositions(v.Length(), v.data());
 }