void SpecificWorker::setPosition(const MotorGoalPosition& goal) { QString name = QString::fromStdString(goal.name); if( mParams.contains( name ) ) { Servo *servo = motorsName[name]; int busId = name2id[name]; float position = truncatePosition(name,goal.position); int pInt = 0; // pInt = servo->rads2Steps(position); pInt = servo->convertir_Radianes_Pasos(position,servo->params.invertedSign); qDebug()<<"setting position"<<pInt<<"of motor"<<name<<busId; Servo::TMotorData &data = motorsId[busId]->data; data.currentPosRads = position; } else { uFailed.what = std::string("Exception: muecasJointComp::setPosition:: Unknown motor name") + goal.name; throw uFailed; } qDebug()<<"done:setting position of motor"<<name; }