void partMover::calib_click(GtkButton *button, gtkClassData* currentClassData)
{
  //ask for confirmation
  if (!dialog_question("Do you really want to recalibrate the joint?")) 
  {
     return;
  }

  partMover *currentPart = currentClassData->partPointer;
  int * joint = currentClassData->indexPointer;
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  IControlCalibration2 *ical = currentPart->cal;
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  ResourceFinder *fnd = currentPart->finder;
  //fprintf(stderr, "opening file \n");
  char buffer1[800];
  char buffer2[800];

  strcpy(buffer1, currentPart->partLabel);
  strcpy(buffer2, strcat(buffer1, "_calib"));

  if (!fnd->findGroup(buffer2).isNull())
    {
      bool ok = true;
      Bottle xtmp;

      xtmp.clear();
      xtmp = fnd->findGroup(buffer2).findGroup("CalibrationType");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      unsigned char type = (unsigned char) xtmp.get(*joint+1).asDouble();
      fprintf(stderr, "%d ", type);

      xtmp.clear();
      xtmp = fnd->findGroup(buffer2).findGroup("Calibration1");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double param1 = xtmp.get(*joint+1).asDouble();
      fprintf(stderr, "%f ", param1);

      xtmp.clear();
      xtmp = fnd->findGroup(buffer2).findGroup("Calibration2");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double param2 = xtmp.get(*joint+1).asDouble();
      fprintf(stderr, "%f ", param2);

      xtmp.clear();
      xtmp = fnd->findGroup(buffer2).findGroup("Calibration3");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double param3 = xtmp.get(*joint+1).asDouble();
      fprintf(stderr, "%f \n", param3);


      if(!ok)
    dialog_message(GTK_MESSAGE_ERROR,(char *)"Check number of calib entries in the group",  buffer2, true);
      else
    ical->calibrate2(*joint, type, param1, param2, param3);
    }
  else
    dialog_message(GTK_MESSAGE_ERROR,(char *)"The supplied file does not conatain a group named:",  buffer2, true);

  return;
}
Ejemplo n.º 2
0
/*
 * Enable all PID and refresh position sliders
 */
void partMover::calib_all(GtkButton *button, partMover* currentPart)
{
  //ask for confirmation
  if (!dialog_question("Do you really want to recalibrate the whole part?")) 
  {
     return;
  }

  IPositionControl *ipos = currentPart->pos;
  IControlCalibration2 *ical = currentPart->cal;
	 
  int joint;
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  ResourceFinder *fnd = currentPart->finder;
  //fprintf(stderr, "opening file \n");
  char buffer1[800];
  char buffer2[800];

  strcpy(buffer1, currentPart->partLabel);
  strcpy(buffer2, strcat(buffer1, "_calib"));

  if (!fnd->findGroup(buffer2).isNull())
    {
      bool ok = true;
      Bottle p1 = fnd->findGroup(buffer2).findGroup("Calibration1");
      ok = ok && (p1.size() == NUMBER_OF_JOINTS+1);
      Bottle p2 = fnd->findGroup(buffer2).findGroup("Calibration2");
      ok = ok && (p2.size() == NUMBER_OF_JOINTS+1);
      Bottle p3 = fnd->findGroup(buffer2).findGroup("Calibration3");
      ok = ok && (p3.size() == NUMBER_OF_JOINTS+1);
      Bottle ty = fnd->findGroup(buffer2).findGroup("CalibrationType");
      ok = ok && (ty.size() == NUMBER_OF_JOINTS+1);
      if (ok)
	{
	  for (joint=0; joint < NUMBER_OF_JOINTS; joint++)
	    {

	      double param1 = p1.get(joint+1).asDouble();
	      //fprintf(stderr, "%f ", param1);


	      double param2 = p2.get(joint+1).asDouble();
	      //fprintf(stderr, "%f ", param2);


	      double param3 = p3.get(joint+1).asDouble();
	      //fprintf(stderr, "%f ", param3);

	      unsigned char type = (unsigned char) ty.get(joint+1).asDouble();
	      //fprintf(stderr, "%d ", type);

	      ical->calibrate2(joint, type, param1, param2, param3);
	    }
	}
      else
	dialog_message(GTK_MESSAGE_ERROR,(char *) "Check number of entries in the file",  buffer1, true);
    }
  else
    {
      dialog_message(GTK_MESSAGE_ERROR,(char *) "No calib file found. Define a suitable file called:",  buffer1, true);   
    }
  return;

}