17 #include <yarp/os/Time.h>
18 #include <yarp/os/Log.h>
19 #include <yarp/os/LogStream.h>
22 #include <yarp/dev/PolyDriver.h>
23 #include <ace/config.h>
24 #include <ace/Log_Msg.h>
30 #include <yarp/os/LogStream.h>
32 #include "EoMeasures.h"
33 #include "EoMotionControl.h"
36 #include "EoProtocol.h"
37 #include "EoProtocolMN.h"
39 #include "EoProtocolMC.h"
41 #include <yarp/os/NetType.h>
42 #include <yarp/conf/environment.h>
45 #pragma warning(once:4355)
51 using namespace yarp::os;
57 yWarning() << std::string(txt) <<
" not yet implemented for embObjMultiEnc\n";
66 Bottle &
tmp=input.findGroup(key1.c_str(), txt.c_str());
69 yError (
"%s not found\n", key1.c_str());
75 yError(
"%s incorrect number of entries\n", key1.c_str());
85 bool embObjMultiEnc::fromConfig(yarp::os::Searchable &_config)
87 yDebug()<<
"configurazione: ";;
88 yDebug() << _config.toString();
90 Bottle general = _config.findGroup(
"JOINTS");
93 yError() <<
"embObjMultiEnc cannot find general group";
97 Bottle jointsbottle = general.findGroup(
"listofjoints");
98 if (jointsbottle.isNull())
100 yError() <<
"embObjMultiEnc cannot find listofjoints param";
104 Bottle encsbottle = general.findGroup(
"encoderConversionFactor");
105 if (encsbottle.isNull())
107 yError() <<
"embObjMultiEnc cannot find encoderConversionFactor param";
113 numofjoints = jointsbottle.size() -1;
115 listofjoints.clear();
116 for (
int i = 1; i < jointsbottle.size(); i++) listofjoints.push_back(jointsbottle.get(i).asInt32());
118 yDebug()<<
" embObjMultiEnc List of joints: " << numofjoints;
119 for(
int i=0; i<numofjoints; i++) yDebug() <<
"pos="<< i <<
"val="<< listofjoints[i];
121 analogdata.resize(numofencperjoint*numofjoints, 0.0);
122 encoderConversionFactor.resize(numofencperjoint*numofjoints, 1.0);
124 if (numofencperjoint*numofjoints!=encsbottle.size()-1)
126 yError() <<
"embObjMultiEnc invalid size of encoderConversionFactor param";
129 for (
int i=0; i<encsbottle.size()-1; i++)
131 encoderConversionFactor[i]=encsbottle.get(i+1).asFloat64();
138 embObjMultiEnc::embObjMultiEnc()
140 memset(&serviceConfig.ethservice, 0,
sizeof(serviceConfig.ethservice));
148 status = IAnalogSensor::AS_OK;
153 analogdata.resize(0);
155 numofjoints = default_numofjoints;
156 numofencperjoint = default_numofencperjoint;
159 std::string
tmp = yarp::conf::environment::get_string(
"ETH_VERBOSEWHENOK");
162 verbosewhenok = (bool)(yarp::conf::numeric::from_string(
tmp, 0U));
166 verbosewhenok =
false;
174 embObjMultiEnc::~embObjMultiEnc()
176 analogdata.resize(0);
187 bool embObjMultiEnc::initialised()
195 bool embObjMultiEnc::open(yarp::os::Searchable &config)
200 if(NULL == ethManager)
202 yFatal() <<
"embObjMultiEnc::open() fails to instantiate ethManager";
207 if(
false == ethManager->verifyEthBoardInfo(config, ipv4addr, boardIPstring, boardName))
209 yError() <<
"embObjMultiEnc::open(): object TheEthManager fails in parsing ETH propertiex from xml file";
232 if(!fromConfig(config))
234 yError() <<
"embObjMultiEnc missing some configuration parameter. Check logs and your config file.";
240 res = ethManager->requestResource2(
this, config);
243 yError() <<
"embObjMultiEnc::open() fails because could not instantiate the ethResource for BOARD w/ IP = " << boardIPstring <<
" ... unable to continue";
247 printServiceConfig();
250 if(!res->verifyEPprotocol(eoprot_endpoint_motioncontrol))
296 if(
false == initRegulars())
373 bool embObjMultiEnc::initRegulars()
377 vector<eOprotID32_t> id32v(0);
378 eOprotID32_t id32 = eo_prot_ID32dummy;
381 for(
int j=0; j<numofjoints; j++)
384 id32 = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_joint, listofjoints[j], eoprot_tag_mc_joint_status_addinfo_multienc);
388 id32v.push_back(id32);
392 if(
false == res->serviceSetRegulars(eomn_serv_category_mc, id32v))
394 yError() <<
"embObjMultiEnc::initRegulars() fails to add its variables to regulars: cannot proceed any further";
401 yDebug() <<
"embObjMultiEnc::initRegulars() added" << id32v.size() <<
"regular rops to BOARD" << res->getProperties().boardnameString <<
"with IP" << res->getProperties().ipv4addrString;
403 for (
size_t r = 0; r<id32v.size(); r++)
405 uint32_t item = id32v.at(r);
406 eoprot_ID2information(item, nvinfo,
sizeof(nvinfo));
407 yDebug() <<
"\t it added regular rop for" << nvinfo;
430 std::lock_guard<std::mutex> lck(mtx);
433 if (status != IAnalogSensor::AS_OK)
437 case IAnalogSensor::AS_OVF:
441 case IAnalogSensor::AS_ERROR:
445 case IAnalogSensor::AS_TIMEOUT:
457 out.resize(analogdata.size());
458 for (
size_t k = 0; k<analogdata.size(); k++)
460 out[k] = analogdata[k];
467 void embObjMultiEnc::resetCounters()
475 void embObjMultiEnc::getCounters(
unsigned int &
sat,
unsigned int &err,
unsigned int &to)
483 int embObjMultiEnc::getState(
int ch)
485 printf(
"getstate\n");
490 int embObjMultiEnc::getChannels()
492 return analogdata.size();
496 int embObjMultiEnc::calibrateSensor()
502 int embObjMultiEnc::calibrateSensor(
const yarp::sig::Vector& value)
508 int embObjMultiEnc::calibrateChannel(
int ch)
514 int embObjMultiEnc::calibrateChannel(
int ch,
double v)
526 bool embObjMultiEnc::update(eOprotID32_t id32,
double timestamp,
void* rxdata)
528 timestamp = timestamp;
530 int joint = eoprot_ID2index(id32);
542 eOmeas_position_t* multienc = (eOmeas_position_t*)rxdata;
544 int startindex = joint * numofencperjoint;
546 std::lock_guard<std::mutex> lck(mtx);
547 for(
int i=0; i< numofencperjoint; i++)
549 analogdata[startindex + i]=((double) multienc[i])/encoderConversionFactor[startindex + i];
558 bool embObjMultiEnc::close()
567 void embObjMultiEnc::printServiceConfig(
void)
586 void embObjMultiEnc::cleanup(
void)
588 if(ethManager == NULL)
return;
590 int ret = ethManager->releaseResource2(res,
this);
593 ethManager->killYourself();
static TheEthManager * instance()
bool NOT_YET_IMPLEMENTED(const char *txt)
double sat(const double val, const double min, const double max)
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Copyright (C) 2008 RobotCub Consortium.
static bool extractGroup(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)