void SpecificWorker::setSyncPosition(const MotorGoalPositionList& listGoals){
bool correct = true;
int ids[ listGoals.size()];
int positions[ listGoals.size()];
float position = 0;
for(uint i=0;i< listGoals.size();i++)
{
QString name = QString::fromStdString( listGoals[i].name);
if ( !motorsName.contains(name))
{
correct = false;
break;
}
else
{
Servo *servo = motorsName[name];
ids[i] = name2id[name];
position = truncatePosition(name, listGoals[i].position);
positions[i] = servo->rads2Steps(position);
}
}
if(correct)
{
for(uint i=0;i< listGoals.size();i++)
setPosition( listGoals[i]);
}
else
{
uFailed.what = std::string("Exception: muecasJointComp::setSyncPosition:: Unknown motor name");
throw uFailed;
}
qDebug()<<"done:setting sync position of motors";
}
