void SpecificWorker::saccadic4D(float leftPan, float rightPan, float tilt, float neck){
RoboCompJointMotor::MotorGoalPositionList list;
RoboCompJointMotor::MotorGoalPosition g;
g.maxSpeed = 0;
g.name = leftPanMotorName;
g.position = leftPan;
list.push_back( g );
g.maxSpeed = 0;
g.name = rightPanMotorName;
g.position = rightPan;
list.push_back( g );
g.maxSpeed = 0;
g.name = tiltMotorName;
g.position = tilt;
list.push_back( g );
g.maxSpeed = 0;
g.name = neckMotorName;
g.position = neck;
list.push_back( g );
QMutexLocker lock(mutex);
try
{
jointmotor1_proxy->setSyncPosition( list );
}
catch( RoboCompJointMotor::UnknownMotorException & ex)
{
std::cout << ex;
throw ex;
}
catch( RoboCompJointMotor::HardwareFailedException & ex)
{
std::cout << ex;
throw ex;
}
}
void SpecificWorker::moverangles(QVec angles)
{
RoboCompJointMotor::MotorGoalPositionList mg;
RoboCompJointMotor::MotorGoalPosition p;
float q1=angles(0),
q2=angles(1)*-1,
q3=angles(2)*-1;
qDebug()<<"q1 = "<<q1<<" q2 = "<<q2<<" q3 = "<<q3;
MotorState m=jointmotor_proxy->getMotorState(m1);
p.name=m1;
p.maxSpeed=fabs(q1-m.pos);
p.position=q1;
mg.push_back(p);
jointmotor_proxy->getMotorState(m2);
p.name=m2;
p.maxSpeed=fabs(q2-m.pos);
p.position=q2;
mg.push_back(p);
jointmotor_proxy->getMotorState(m3);
p.name=m3;
p.maxSpeed=fabs(q3-m.pos);
p.position=q3;
mg.push_back(p);
jointmotor_proxy->setSyncPosition(mg);
}
void SpecificWorker::moverangles(QVec angles,double vel)
{
if(!isnan(angles(0))&&!isnan(angles(1))&&!isnan(angles(2)))
{
RoboCompJointMotor::MotorGoalPositionList mg;
RoboCompJointMotor::MotorGoalPosition p;
RoboCompJointMotor::MotorGoalVelocityList mv;
RoboCompJointMotor::MotorGoalVelocity v;
double q1=angles(0),
q2=angles(1) *signleg,
q3=angles(2) *signleg;
qDebug()<<"Leg: "<<foot<<"q1 = "<<q1<<" q2 = "<<q2<<" q3 = "<<q3;
MotorState m=jointmotor_proxy->getMotorState(motores.at(0).toStdString());
v.name = p.name = motores.at(0).toStdString();
v.velocity = p.maxSpeed=fabs(q1-m.pos)*vel;
p.position=q1;
mg.push_back(p);
mv.push_back(v);
m=jointmotor_proxy->getMotorState(motores.at(1).toStdString());
v.name = p.name=motores.at(1).toStdString();
v.velocity = p.maxSpeed=fabs(q2-m.pos)*vel;
p.position=q2;
mg.push_back(p);
mv.push_back(v);
m=jointmotor_proxy->getMotorState(motores.at(2).toStdString());
v.name = p.name=motores.at(2).toStdString();
v.velocity = p.maxSpeed=fabs(q3-m.pos)*vel;
p.position=q3;
mg.push_back(p);
mv.push_back(v);
// jointmotor_proxy->setSyncVelocity(mv);
jointmotor_proxy->setSyncPosition(mg);
}
else
qDebug()<< "Posicion no alcanzada";
}
///JoystickAdapter
void SpecificWorker::sendData(const TData& data)
{
qDebug()<<"jdata0"<<data.axes[0].value;
qDebug()<<"jdata1"<<data.axes[1].value;
qDebug()<<"jdata2"<<data.axes[2].value;
if (data.buttons[0].clicked)
{
qDebug()<<"button 0 clicked";
float joy_y = normalize(data.axes[1].value, -1., 1., -0.6,0.5);
float joy_x = normalize(data.axes[0].value, -1., 1., -0.5,0.5);
float rotate = normalize(data.axes[2].value, -1., 1., -0.5,0.5);
qDebug()<<"normalize position 0"<<"pitch"<<joy_x<<"yaw"<<joy_y;
float POS_Z = joy_y;
float POS_X = -joy_y + joy_x;
float POS_Y = -joy_y - joy_x;
int step_rotate = rotate * 17960.f / 6.28;
int pasos_z = POS_Z * 17960.f / 6.28;
int pasos_x = POS_X * 17960.f / 6.28;
int pasos_y = POS_Y * 17960.f / 6.28;
qDebug()<<"pasos"<<pasos_z<<pasos_x<<pasos_y<<step_rotate;
if (data.axes[1].value!=joy_x_antiguo || data.axes[0].value != joy_y_antiguo || data.axes[2].value != joy_z_antiguo)
{
// limites de los recorridos de los motores
if ( pasos_x > MIN_X && pasos_x < MAX_X && pasos_y > MIN_Y && pasos_y < MAX_Y && pasos_z > MIN_Z && pasos_z < MAX_Z && step_rotate > MIN_TILT && step_rotate < MAX_TILT)
{
qDebug()<<"stting positios"<<"front"<<POS_Z<<"right"<<POS_X<<"left"<<POS_Y;
RoboCompJointMotor::MotorGoalPosition p_goal;
RoboCompJointMotor::MotorGoalPositionList list;
p_goal.name = "neck";
p_goal.position = rotate;
list.push_back(p_goal);
p_goal.name = "neckRight";
p_goal.position = POS_X;
list.push_back(p_goal);
p_goal.name = "neckLeft";
p_goal.position = POS_Y;
list.push_back(p_goal);
p_goal.name = "neckFront";
p_goal.position = POS_Z;
list.push_back(p_goal);
joy_x_antiguo = data.axes[1].value;
joy_y_antiguo = data.axes[0].value;
joy_z_antiguo = data.axes[2].value;
mutex->lock();
try
{
jointmotor1_proxy->setSyncPosition( list );
}
catch( Ice::Exception & ex)
{
std::cout<< ex.what()<< std::endl;
}
mutex->unlock();
}
}
}
else if (data.buttons[1].clicked)
{
// eyebrown and mouth
qDebug()<<"button 1 clicked";
float axes_y = normalize(data.axes[1].value, -1., 1., -0.6,0.5);
float axes_x = normalize(data.axes[0].value, -1., 1., -0.5,0.5);
float axes_mouth = normalize(data.axes[2].value, -1., 1., -0.5,0.5);
qDebug()<<"normalize position 1"<<"pitch"<<axes_x<<"yaw"<<axes_y<<"roll"<<axes_mouth;
move(axes_mouth/100, mouthMotorName);
move(-axes_x/100, eyeBLeftYAWMotorName );
move(axes_x/100, eyeBRightYAWMotorName );
move(axes_y/100, eyeBLeftUPMotorName );
move(axes_y/100, eyeBRightUPMotorName );
}
else if (data.buttons[2].clicked)
{
qDebug()<<"button 2 clicked";
float rot_tilt = normalize(data.axes[1].value, -1., 1., -0.5,0.5) /3;
float rotations_eyeA = normalize(data.axes[0].value, -1., 1., -0.5,0.5) /2;
RoboCompJointMotor::MotorGoalPosition p_goal;
RoboCompJointMotor::MotorGoalPositionList list;
p_goal.name = "rightPan";
p_goal.position = rotations_eyeA;
list.push_back(p_goal);
p_goal.name = "leftPan";
p_goal.position = rotations_eyeA;
list.push_back(p_goal);
p_goal.name = "tilt";
p_goal.position = rot_tilt;
list.push_back(p_goal);
mutex->lock();
try
{
jointmotor1_proxy->setSyncPosition( list );
}
catch( Ice::Exception & ex)
{
std::cout<< ex.what()<< std::endl;
}
mutex->unlock();
}
}
void SpecificWorker::moveneck()
{
float rotations_eyeA = righteyepan->value();
float rotations_eyeB = lefteyepan->value();
float eyestilt = eyetilt->value();
float ROTATE = neckyaw->value();
float POS_Zz = neckpitch->value();
float POS_Xx = -neckpitch->value() + neckroll->value();
float POS_Yy = -neckpitch->value() - neckroll->value();
int pasos_rotate = ROTATE * 17960.f / 6.28;
int pasos_zz = POS_Zz * 17960.f / 6.28;
int pasos_xx = POS_Xx * 17960.f / 6.28;
int pasos_yy = POS_Yy * 17960.f / 6.28;
qDebug()<<"pasos"<<pasos_zz<<pasos_xx<<pasos_yy<<pasos_rotate;
// limites de los recorridos de los motores
if ( pasos_xx > MIN_X && pasos_xx < MAX_X && pasos_yy > MIN_Y && pasos_yy < MAX_Y && pasos_zz > MIN_Z && pasos_zz < MAX_Z && pasos_rotate > MIN_TILT && pasos_rotate < MAX_TILT)
{
qDebug()<<"stting positios"<<"front"<<POS_Zz<<"right"<<POS_Xx<<"left"<<POS_Yy;
RoboCompJointMotor::MotorGoalPosition p_goal;
RoboCompJointMotor::MotorGoalPositionList list;
p_goal.name = "neckRight";
p_goal.position = POS_Xx;
list.push_back(p_goal);
p_goal.name = "neckLeft";
p_goal.position = POS_Yy;
list.push_back(p_goal);
p_goal.name = "neckFront";
p_goal.position = POS_Zz;
list.push_back(p_goal);
p_goal.name = "neck";
p_goal.position = ROTATE;
list.push_back(p_goal);
p_goal.name = "rightPan";
p_goal.position = rotations_eyeA;
list.push_back(p_goal);
p_goal.name = "leftPan";
p_goal.position = rotations_eyeB;
list.push_back(p_goal);
p_goal.name = "tilt";
p_goal.position = eyestilt;
list.push_back(p_goal);
mutex->lock();
try
{
jointmotor1_proxy->setSyncPosition( list );
}
catch( Ice::Exception & ex)
{
std::cout<< ex.what()<< std::endl;
}
mutex->unlock();
}
}
void SpecificWorker::setNMotorsPosition(const RoboCompJointMotor::MotorGoalPositionList& listGoals){
RoboCompJointMotor::MotorGoalPositionList list;
RoboCompJointMotor::MotorGoalPosition g;
for(uint i=0; i<listGoals.size(); i++)
{
if(listGoals[i].name=="leftPan")
{
g.maxSpeed = 0;
g.name = leftPanMotorName;
g.position = listGoals[i].position;
list.push_back(g);
}
else
{
if(listGoals[i].name=="rightPan")
{
g.maxSpeed = 0;
g.name = rightPanMotorName;
g.position = listGoals[i].position;
list.push_back(g);
}
else
{
if(listGoals[i].name=="tilt")
{
g.maxSpeed = 0;
g.name = tiltMotorName;
g.position = listGoals[i].position;
list.push_back(g);
}
else
{
if(listGoals[i].name=="neck")
{
g.maxSpeed = 0;
g.name = neckMotorName;
g.position = listGoals[i].position;
list.push_back(g);
}
}
}
}
}
if(list.size()>0)
{
QMutexLocker lock(mutex);
try
{
jointmotor1_proxy->setSyncPosition( list );
}
catch( RoboCompJointMotor::UnknownMotorException & ex)
{
throw ex;
}
catch( RoboCompJointMotor::HardwareFailedException & ex)
{
throw ex;
}
}
}
