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";
}
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;
}
int Board::randomMove(int pos_now) {
int min = pos_now - 1;
int max = pos_now + 1;
return truncatePosition(min + (rand() % (int) (max - min + 1)));
}
