virtual bool open(Searchable &s)
{
Time::turboBoost();
// get command line options
options.fromString(s.toString());
if (!options.check("robot") || !options.check("part"))
{
printf("Missing either --robot or --part options. Quitting!\n");
return false;
}
std::string dumpername;
// get dumepr name
if (options.check("name"))
{
dumpername = options.find("name").asString();
dumpername += "/";
}
else
{
dumpername = "/controlBoardDumper/";
}
Value& robot = options.find("robot");
Value& part = options.find("part");
configFileRobotPart = "config_";
configFileRobotPart = configFileRobotPart + robot.asString();
configFileRobotPart = configFileRobotPart + "_";
configFileRobotPart = configFileRobotPart + part.asString();
configFileRobotPart = configFileRobotPart + ".ini";
//get the joints to be dumped
getRate(options, rate);
if (getNumberUsedJoints(options, nJoints) == 0)
return false;
thetaMap = new int[nJoints];
if (getUsedJointsMap(options, nJoints, thetaMap) == 0)
return false;
//get the type of data to be dumped
if (getNumberDataToDump(options, nData) == 0)
return false;
dataToDump = new std::string[nData];
if (getDataToDump(options, dataToDump, nData, &useDebugClient) == 0)
return false;
// Printing configuration to the user
yInfo("Running with the following configuration:\n");
yInfo("Selected rate is: %d\n", rate);
yInfo("Data selected to be dumped are:\n");
for (int i = 0; i < nData; i++)
yInfo("\t%s \n", dataToDump[i].c_str());
//open remote control board
ddBoardOptions.put("device", "remote_controlboard");
ddDebugOptions.put("device", "debugInterfaceClient");
ConstString localPortName = name;
ConstString localDebugPortName = name;
localPortName = localPortName + dumpername.c_str();
localDebugPortName = localPortName + "debug/";
//localPortName = localPortName + robot.asString();
localPortName = localPortName + part.asString();
localDebugPortName = localDebugPortName + part.asString();
ddBoardOptions.put("local", localPortName.c_str());
ddDebugOptions.put("local", localDebugPortName.c_str());
ConstString remotePortName = "/";
ConstString remoteDebugPortName;
remotePortName = remotePortName + robot.asString();
remotePortName = remotePortName + "/";
remotePortName = remotePortName + part.asString();
ddBoardOptions.put("remote", remotePortName.c_str());
remoteDebugPortName = remotePortName + "/debug";
ddDebugOptions.put("remote", remoteDebugPortName.c_str());
// create a device
ddBoard.open(ddBoardOptions);
if (!ddBoard.isValid()) {
printf("Device not available.\n");
Network::fini();
return false;
}
if (useDebugClient )
{
ddDebug.open(ddDebugOptions);
if (!ddDebug.isValid())
{
yError("\n-----------------------------------------\n");
yError("Debug Interface is mandatory to run this module with the '--dataToDumpAll' option or to dump any of the getRotorxxx data.\n");
yError("Please Verify the following 2 conditions are satisfied:\n\n");
yError("1) Check 'debugInterfaceClient' is available using 'yarpdev --list' command\n");
// yError("%s", Drivers::factory().toString().c_str());
std::string deviceList, myDev;
deviceList.clear();
deviceList.append(Drivers::factory().toString().c_str());
myDev = "debugInterfaceClient";
if(deviceList.find(myDev) != std::string::npos)
yError("\t--> Seems OK\n");
else
yError("\t--> Seems NOT OK. The device was not found, please activate the compilation using the corrisponding CMake flag.\n");
yError("\n2) Check if the robot has the 'debugInterfaceWrapper' device up and running. \n You should see from 'yarp name list' output, a port called\n");
yError("\t/robotName/part_name/debug/rpc:i\n If not, fix the robot configuration files to instantiate the 'debugInterfaceWrapper' device.\n");
yError("\nQuickFix: If you set the --dataToDumpAll and do not need the advanced debug feature (getRotorxxx) just remove this option. See help for more information.\n");
yError("------------- END ERROR MESSAGE ---------------\n\n");
Network::fini();
return false;
}
}
bool logToFile = false;
if (options.check("logToFile")) logToFile = true;
portPrefix= dumpername.c_str() + part.asString() + "/";
//boardDumperThread *myDumper = new boardDumperThread(&dd, rate, portPrefix, dataToDump[0]);
//myDumper->setThetaMap(thetaMap, nJoints);
myDumper = new boardDumperThread[nData];
for (int i = 0; i < nData; i++)
{
if (dataToDump[i] == "getEncoders" )
{
if (ddBoard.view(ienc))
{
yInfo("Initializing a getEncs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetEncs.setInterface(ienc);
if (ddBoard.view(istmp))
{
yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetEncs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetEncs);
}
}
else if (dataToDump[i] == "getEncoderSpeeds")
{
if (ddBoard.view(ienc))
{
yInfo("Initializing a getSpeeds thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetSpeeds.setInterface(ienc);
if (ddBoard.view(istmp))
{
yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
myGetSpeeds.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetSpeeds);
}
}
else if (dataToDump[i] == "getEncoderAccelerations")
{
if (ddBoard.view(ienc))
{
yInfo("Initializing a getAccs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetAccs.setInterface(ienc);
if (ddBoard.view(istmp))
{
yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetAccs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetAccs);
}
}
else if (dataToDump[i] == "getPosPidReferences")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPidRefs.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getPosPidReferences::The time stamp initalization interfaces was successfull! \n");
myGetPidRefs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPidRefs);
}
}
else if (dataToDump[i] == "getTrqPidReferences")
{
if (ddBoard.view(itrq))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqRefs.setInterface(itrq);
if (ddBoard.view(istmp))
{
yInfo("getTrqPidReferences::The time stamp initalization interfaces was successfull! \n");
myGetTrqRefs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqRefs);
}
}
else if (dataToDump[i] == "getControlModes")
{
if (ddBoard.view(icmod))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetControlModes.setInterface(icmod, nJoints);
if (ddBoard.view(istmp))
{
yInfo("getControlModes::The time stamp initalization interfaces was successfull! \n");
myGetControlModes.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetControlModes);
}
}
else if (dataToDump[i] == "getInteractionModes")
{
if (ddBoard.view(iimod))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetInteractionModes.setInterface(iimod, nJoints);
if (ddBoard.view(istmp))
{
yInfo("getInteractionModes::The time stamp initalization interfaces was successfull! \n");
myGetInteractionModes.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetInteractionModes);
}
}
else if (dataToDump[i] == "getPositionErrors")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPosErrs.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getPositionErrors::The time stamp initalization interfaces was successfull! \n");
myGetPosErrs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPosErrs);
}
}
else if (dataToDump[i] == "getOutputs")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getOuts thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetOuts.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getOutputs::The time stamp initalization interfaces was successfull! \n");
myGetOuts.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetOuts);
}
}
else if (dataToDump[i] == "getCurrents")
{
if (ddBoard.view(iamp))
{
yInfo("Initializing a getCurrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetCurrs.setInterface(iamp);
if (ddBoard.view(istmp))
{
yInfo("getCurrents::The time stamp initalization interfaces was successfull! \n");
myGetCurrs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetCurrs);
}
}
else if (dataToDump[i] == "getTorques")
{
if (ddBoard.view(itrq))
{
yInfo("Initializing a getTorques thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqs.setInterface(itrq);
if (ddBoard.view(istmp))
{
yInfo("getTorques::The time stamp initalization interfaces was successfull! \n");
myGetTrqs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqs);
}
}
else if (dataToDump[i] == "getTorqueErrors")
{
if (ddBoard.view(itrq))
{
yInfo("Initializing a getTorqueErrors thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqErrs.setInterface(itrq);
if (ddBoard.view(istmp))
{
yInfo("getTorqueErrors::The time stamp initalization interfaces was successfull! \n");
myGetTrqErrs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqErrs);
}
}
else if (dataToDump[i] == "getRotorPositions")
{
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
yInfo("Initializing a getRotorPosition thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorPoss.setInterface(idbg);
if (ddDebug.view(istmp))
{
yInfo("getRotorPositions::The time stamp initalization interfaces was successfull! \n");
myGetRotorPoss.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorPoss);
}
else
{
yError("Debug Interface not available, cannot dump %s.\n", dataToDump[i].c_str());
Network::fini();
return false;
}
}
}
else if (dataToDump[i] == "getRotorSpeeds")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
yInfo("Initializing a getRotorSpeed thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorVels.setInterface(idbg);
if (ddDebug.view(istmp))
{
yInfo("getRotorSpeeds::The time stamp initalization interfaces was successfull! \n");
myGetRotorVels.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorVels);
}
else
{
printf("Debug Interface not available, cannot dump %s.\n", dataToDump[i].c_str());
Network::fini();
return false;
}
}
else if (dataToDump[i] == "getRotorAccelerations")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
yInfo("Initializing a getRotorAcceleration thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorAccs.setInterface(idbg);
if (ddDebug.view(istmp))
{
yInfo("getRotorAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetRotorAccs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorAccs);
}
else
{
printf("Debug Interface not available, cannot dump %s.\n", dataToDump[i].c_str());
Network::fini();
return false;
}
}
else if (dataToDump[i] == "getTemperatures")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getTemps thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTemps.setInterface(imot);
if (ddBoard.view(istmp))
{
yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetTemps.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTemps);
}
}
else if (dataToDump[i] == "getMotorEncoders")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getEncs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotorEncs.setInterface(imotenc);
if (ddBoard.view(istmp))
{
yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetMotorEncs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotorEncs);
}
}
else if (dataToDump[i] == "getMotorEncoderSpeeds")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getSpeeds thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotorSpeeds.setInterface(imotenc);
if (ddBoard.view(istmp))
{
yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
myGetMotorSpeeds.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotorSpeeds);
}
}
else if (dataToDump[i] == "getMotorEncoderAccelerations")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getAccs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotorAccs.setInterface(imotenc);
if (ddBoard.view(istmp))
{
yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetMotorAccs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotorAccs);
}
}
}
Time::delay(1);
for (int i = 0; i < nData; i++)
myDumper[i].start();
return true;
}
virtual bool open(Searchable &s)
{
Time::turboBoost();
// get command line options
options.fromString(s.toString());
if (!options.check("robot") || !options.check("part"))
{
printf("Missing either --robot or --part options. Quitting!\n");
return false;
}
// get command file options
Value& robot = options.find("robot");
Value& part = options.find("part");
configFileRobotPart = "config_";
configFileRobotPart = configFileRobotPart + robot.asString();
configFileRobotPart = configFileRobotPart + "_";
configFileRobotPart = configFileRobotPart + part.asString();
configFileRobotPart = configFileRobotPart + ".ini";
//get the joints to be dumped
getRate(options, rate);
fprintf(stderr, "Selected rate is: %d\n", rate);
if (getNumberUsedJoints(options, nJoints) == 0)
return false;
thetaMap = new int[nJoints];
if (getUsedJointsMap(options, nJoints, thetaMap) == 0)
return false;
//get the type of data to be dumped
if (getNumberDataToDump(options, nData) == 0)
return false;
dataToDump = new ConstString[nData];
if (getDataToDump(options, dataToDump, nData) == 0)
return false;
for (int i = 0; i < nData; i++)
fprintf(stderr, "%s \n", dataToDump[i].c_str());
//if (!fileOptions.check("d"))
// {
// fprintf(stderr, "Missing option 'd' in given config file\n");
// return 1;
// }
//Value& d = fileOptions.find("d");
//open remote control board
ddBoardOptions.put("device", "remote_controlboard");
ddDebugOptions.put("device", "debugInterfaceClient");
ConstString localPortName = name;
ConstString localDebugPortName = name;
localPortName = localPortName + "/controlBoardDumper/";
localDebugPortName = localPortName + "debug/";
//localPortName = localPortName + robot.asString();
localPortName = localPortName + part.asString();
localDebugPortName = localDebugPortName + part.asString();
ddBoardOptions.put("local", localPortName.c_str());
ddDebugOptions.put("local", localDebugPortName.c_str());
ConstString remotePortName = "/";
remotePortName = remotePortName + robot.asString();
remotePortName = remotePortName + "/";
remotePortName = remotePortName + part.asString();
ddBoardOptions.put("remote", remotePortName.c_str());
ddDebugOptions.put("remote", remotePortName.c_str());
fprintf(stderr, "%s", ddBoardOptions.toString().c_str());
// create a device
ddBoard.open(ddBoardOptions);
if (!ddBoard.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
ddDebug.open(ddDebugOptions);
if (!ddDebug.isValid()) {
printf("Debug Interface is mandatary to run this module. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
bool logToFile = false;
if (options.check("logToFile")) logToFile = true;
portPrefix = "/";
portPrefix= portPrefix + "controlBoardDumper/" + part.asString() + "/";
//boardDumperThread *myDumper = new boardDumperThread(&dd, rate, portPrefix, dataToDump[0]);
//myDumper->setThetaMap(thetaMap, nJoints);
myDumper = new boardDumperThread[nData];
for (int i = 0; i < nData; i++)
{
if (dataToDump[i] == "getEncoders")
if (ddBoard.view(ienc))
{
fprintf(stderr, "Initializing a getEncs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetEncs.setInterface(ienc);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetEncs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetEncs);
}
if (dataToDump[i] == "getEncoderSpeeds")
if (ddBoard.view(ienc))
{
fprintf(stderr, "Initializing a getSpeeds thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetSpeeds.setInterface(ienc);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
myGetSpeeds.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetSpeeds);
}
if (dataToDump[i] == "getEncoderAccelerations")
if (ddBoard.view(ienc))
{
fprintf(stderr, "Initializing a getAccs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetAccs.setInterface(ienc);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetAccs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetAccs);
}
if (dataToDump[i] == "getPidReferences")
if (ddBoard.view(ipid))
{
fprintf(stderr, "Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPidRefs.setInterface(ipid);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getPidReferences::The time stamp initalization interfaces was successfull! \n");
myGetPidRefs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPidRefs);
}
if (dataToDump[i] == "getPositionErrors")
if (ddBoard.view(ipid))
{
fprintf(stderr, "Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPosErrs.setInterface(ipid);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getPositionErrors::The time stamp initalization interfaces was successfull! \n");
myGetPosErrs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPosErrs);
}
if (dataToDump[i] == "getOutputs")
if (ddBoard.view(ipid))
{
fprintf(stderr, "Initializing a getOuts thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetOuts.setInterface(ipid);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getOutputs::The time stamp initalization interfaces was successfull! \n");
myGetOuts.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetOuts);
}
if (dataToDump[i] == "getCurrents")
if (ddBoard.view(iamp))
{
fprintf(stderr, "Initializing a getCurrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetCurrs.setInterface(iamp);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getCurrents::The time stamp initalization interfaces was successfull! \n");
myGetCurrs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetCurrs);
}
if (dataToDump[i] == "getTorques")
if (ddBoard.view(itrq))
{
fprintf(stderr, "Initializing a getTorques thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqs.setInterface(itrq);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getTorques::The time stamp initalization interfaces was successfull! \n");
myGetTrqs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqs);
}
if (dataToDump[i] == "getTorqueErrors")
if (ddBoard.view(itrq))
{
fprintf(stderr, "Initializing a getTorqueErrors thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqErrs.setInterface(itrq);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getTorqueErrors::The time stamp initalization interfaces was successfull! \n");
myGetTrqErrs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqErrs);
}
if (dataToDump[i] == "getRotorPositions")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
fprintf(stderr, "Initializing a getRotorPosition thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorPoss.setInterface(idbg);
if (ddDebug.view(istmp))
{
fprintf(stderr, "getRotorPositions::The time stamp initalization interfaces was successfull! \n");
myGetRotorPoss.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorPoss);
}
else
{
printf("Debug Interface not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
}
if (dataToDump[i] == "getRotorSpeeds")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
fprintf(stderr, "Initializing a getRotorSpeed thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorVels.setInterface(idbg);
if (ddDebug.view(istmp))
{
fprintf(stderr, "getRotorSpeeds::The time stamp initalization interfaces was successfull! \n");
myGetRotorVels.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorVels);
}
else
{
printf("Debug Interface not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
}
if (dataToDump[i] == "getRotorAccelerations")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
fprintf(stderr, "Initializing a getRotorAcceleration thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorAccs.setInterface(idbg);
if (ddDebug.view(istmp))
{
fprintf(stderr, "getRotorAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetRotorAccs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorAccs);
}
else
{
printf("Debug Interface not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
}
}
Time::delay(1);
for (int i = 0; i < nData; i++)
myDumper[i].start();
return true;
}
