/** Lorenzo Natale, Dec 2007. Test remotization of calibrate
* functions. Useful for general tests on device remotization
* (see fakebot for example).
*/
int main(int argc, const char **argv)
{
Network yarp;
IControlCalibration2 *ical;
Property p;
p.put("device", "remote_controlboard");
p.put("local", "/motortest"); //prefix for local names
p.put("remote", "/controlboard"); //prefix for remote names
PolyDriver device;
device.open(p);
device.view(ical);
//ical->calibrate();
ical->calibrate2(1,1000,1.1,1.1,1.1);
ical->done(2);
ical->park();
device.close();
return 0;
}
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;
}
/*
* 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;
}
