void Individuo::initRobotVRep() {
//inicializar torque
if (simxSetJointForce(clientId, this->motorLeft, torque, simx_opmode_oneshot_wait) == simx_return_ok) {
//cout << this->robotName << ": torque do motor esquerdo alterado com sucesso!" << endl;
}else {
cout << this->robotName << ": nao foi possível alterar o torque do motor esquerdo!" << endl;
}
if (simxSetJointForce(clientId, this->motorRight, torque, simx_opmode_oneshot_wait) == simx_return_ok) {
//cout << this->robotName << ": torque do motor direito alterado com sucesso!" << endl;
}
else {
cout << this->robotName << ": nao foi possível alterar o torque do motor direito!" << endl;
}
//inicializar massa
if (simxSetObjectFloatParameter(clientId, this->body, 3005, this->weight, simx_opmode_oneshot_wait) == simx_return_ok) {
//cout << this->robotName << ": massa alterada com sucesso!"<<endl;
}else {
cout << this->robotName << ": nao foi possível alterar a massa do robo!" << endl;
}
}
void YouBotBaseService::setJointSetpoints()
{
joint_position_command.read(m_joint_position_command);
joint_velocity_command.read(m_joint_velocity_command);
joint_effort_command.read(m_joint_effort_command);
FlowStatus f = joint_position_command.read(m_joint_position_command);
FlowStatus f1 = joint_velocity_command.read(m_joint_velocity_command);
FlowStatus f2 = joint_effort_command.read(m_joint_effort_command);
simxPauseCommunication(clientID,1);
for (unsigned int joint_nr = 0; joint_nr < YouBot::NR_OF_BASE_SLAVES; ++joint_nr)
{
switch (m_joint_ctrl_modes[joint_nr])
{
case(Hilas::PLANE_ANGLE):
{
//if(f != NewData) break;
simxSetJointTargetPosition(clientID,vrep_joint_handle[joint_nr],m_joint_position_command.positions[joint_nr], simx_opmode_oneshot);
break;
}
case(Hilas::ANGULAR_VELOCITY):
{
//if(f1 != NewData) break;
// Little hack
int sign[4] = {1,-1,1,-1};
simxSetJointTargetVelocity(clientID,vrep_joint_handle[joint_nr],m_joint_velocity_command.velocities[joint_nr] * sign[joint_nr], simx_opmode_oneshot);
break;
}
case(Hilas::TORQUE):
{
//if(f2 != NewData) break;
simxSetJointForce(clientID,vrep_joint_handle[joint_nr],m_joint_effort_command.efforts[joint_nr], simx_opmode_oneshot);
break;
}
case(Hilas::MOTOR_STOP):
{
simxSetJointTargetPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
simxSetJointTargetPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
simxSetJointTargetPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
simxSetJointTargetPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
break;
}
case(Hilas::TWIST):
{
log(Error) << "Cannot be in TWIST ctrl_mode (programming error)"
<< endlog();
this->getOwner()->error();
break;
}
default:
{
log(Error) << "Case not recognized." << endlog();
this->getOwner()->error();
break;
}
}
}
simxPauseCommunication(clientID,0);
}
