int MoveandRotate(float LinVel, float AngVel, float radius, float lengthWheelAxis, int clientID, int leftMotorHandle, int rightMotorHandle)
{
float leftMotorAngVel=(LinVel-AngVel*(lengthWheelAxis/2))/radius;
float rightMotorAngVel=(LinVel+AngVel*(lengthWheelAxis/2))/radius;
std::cout << AngVel << ", " << leftMotorAngVel << ", " << rightMotorAngVel << "\n";
simxSetJointTargetVelocity(clientID,leftMotorHandle,leftMotorAngVel,simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID,rightMotorHandle,rightMotorAngVel,simx_opmode_oneshot);
}
int main(int argc,char* argv[])
{
int portNb=0;
int leftMotorHandle;
int rightMotorHandle;
int sensorHandle;
if (argc>=5)
{
portNb=atoi(argv[1]);
leftMotorHandle=atoi(argv[2]);
rightMotorHandle=atoi(argv[3]);
sensorHandle=atoi(argv[4]);
}
else
{
printf("Indicate following arguments: 'portNumber leftMotorHandle rightMotorHandle sensorHandle'!\n");
extApi_sleepMs(5000);
return 0;
}
int clientID=simxStart((simxChar*)"127.0.0.1",portNb,true,true,2000,5);
if (clientID!=-1)
{
int driveBackStartTime=-99000;
float motorSpeeds[2];
while (simxGetConnectionId(clientID)!=-1)
{
simxUChar sensorTrigger=0;
if (simxReadProximitySensor(clientID,sensorHandle,&sensorTrigger,NULL,NULL,NULL,simx_opmode_streaming)==simx_return_ok)
{ // We succeeded at reading the proximity sensor
int simulationTime=simxGetLastCmdTime(clientID);
if (simulationTime-driveBackStartTime<3000)
{ // driving backwards while slightly turning:
motorSpeeds[0]=-3.1415f*0.5f;
motorSpeeds[1]=-3.1415f*0.25f;
}
else
{ // going forward:
motorSpeeds[0]=3.1415f;
motorSpeeds[1]=3.1415f;
if (sensorTrigger)
driveBackStartTime=simulationTime; // We detected something, and start the backward mode
}
simxSetJointTargetVelocity(clientID,leftMotorHandle,motorSpeeds[0],simx_opmode_oneshot);
simxSetJointTargetVelocity(clientID,rightMotorHandle,motorSpeeds[1],simx_opmode_oneshot);
}
extApi_sleepMs(5);
}
simxFinish(clientID);
}
return(0);
}
void YouBotBaseService::setTwistSetpoints()
{
cmd_twist.read(m_cmd_twist);
std::vector<quantity<angular_velocity> > wheelVelocities;
quantity<si::velocity> longitudinalVelocity;
quantity<si::velocity> transversalVelocity;
quantity<si::angular_velocity> angularVelocity;
longitudinalVelocity = m_cmd_twist.linear.x * meter_per_second;
transversalVelocity = m_cmd_twist.linear.y * meter_per_second;
angularVelocity = m_cmd_twist.angular.z * radian_per_second;
youBotBaseKinematic.cartesianVelocityToWheelVelocities(longitudinalVelocity, transversalVelocity, angularVelocity, wheelVelocities);
// Little hack
int sign[4] = {1,-1,1,-1};
simxPauseCommunication(clientID,1);
for(unsigned int i=0; i < wheelVelocities.size() ; i++)
{
simxSetJointTargetVelocity(clientID,vrep_joint_handle[i],wheelVelocities[i].value() * sign[i], simx_opmode_oneshot);
}
simxPauseCommunication(clientID,0);
}
void RobotVREP::setJointTargetVelocity(Joint * joint, double joint_vel, simxInt operationMode)
{
int uniqueID = joint->getUniqueObjectId();
int vrepHandlerVectorPosition =jointIdToVrepHandler_map.at ( uniqueID );
int * handler = VrepHandlerVector.at( vrepHandlerVectorPosition );
int error = simxSetJointTargetVelocity(clientID, *handler, (float)joint_vel, operationMode);
if(error != 0) vrep_error << "simxSetJointTargetVelocity: " << *handler << " : " << error << endl;
}
void Joint::setTargetVelocity(const float velocity) {
try {
ReturnCodesExceptions::handleReturnCode(
simxSetJointTargetVelocity(m_ClientId, m_Handle, velocity, simx_opmode_oneshot));
}
catch (ReturnCodesExceptions *ex) {
throw ex;
}
}
void Robot3R::MoveLink(int linkNum, float qf){
int *handler = 0;
switch (linkNum){
case 1: handler = handler1;
case 2: handler = handler2;
case 3: handler = handler3;
}
simxSetJointTargetVelocity(clientID, *handler, qf, simx_opmode_oneshot_wait);
}
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);
}
void Individuo::action() {
switch (state) {
case IDLE:
prevState = state;
if ((double)rand() / (double)RAND_MAX < this->probAtk) {
state = ALIGN_ATK;
}
else {
state = ALIGN_DODGE;
}
//Iniciar cronometragem da ação de ataque ou defesa
startTime = clock();
break;
case ALIGN_ATK:
prevState = state;
endTime = clock();
//Verificar se o tempo de ataque acabou
if ((endTime - startTime) >= this->tAtk) {
state = IDLE;
}else {
activeSensorCounterFront = 0;
activeSensorCounterBack = 0;
//Checar sensores dianteiros
for (int i = 0; i < 2; i++) {
if (simxReadProximitySensor(this->clientId, laserPointerFront[i], &res,
coord, NULL, NULL, simx_opmode_buffer) == simx_return_ok) {
//Checar coordenada z(pois está que aponta pra frente)
this->sensorDist = coord[2];
if ((this->sensorDist < MAX_DIST) && (res > 0)) {
activeSensorCounterFront++;
}
}
}
//Checar sensores traseiros
for (int i = 0; i < 2; i++) {
if (simxReadProximitySensor(this->clientId, laserPointerBack[i], &res,
coord, NULL, NULL, simx_opmode_buffer) == simx_return_ok) {
//Coordenada Z
this->sensorDist = coord[2];
if ((this->sensorDist < MAX_DIST) && (res > 0)) {
activeSensorCounterBack++;
}
}
}
//Se os dois sensores da frente estao apontando para o inimigo entao atacar pela frente
if (activeSensorCounterFront == 2) {
state = ATK;
dir = FRONT;
/*simxSetJointTargetVelocity(this->clientId, motorLeft, 0,
simx_opmode_streaming);
simxSetJointTargetVelocity(this->clientId, motorRight, 0,
simx_opmode_streaming);*/
}//Senao atacar dando ré
else if (activeSensorCounterBack == 2) {
state = ATK;
dir = BACK;
/*simxSetJointTargetVelocity(this->clientId, motorLeft, 0,
simx_opmode_streaming);
simxSetJointTargetVelocity(this->clientId, motorRight, 0,
simx_opmode_streaming);*/
}
else {//Se nada for encontrado, entao continuar procurando
simxSetJointTargetVelocity(this->clientId, motorLeft, V_ROT,
simx_opmode_streaming);
simxSetJointTargetVelocity(this->clientId, motorRight, V_ROT,
simx_opmode_streaming);
//extApi_sleepMs(100);
}
}
break;
case ATK:
prevState = state;
//time(&endTime);
endTime = clock();
//Se tempo de ataque acabou entao voltar ao estado inicial
if ((endTime- startTime) >= this->tAtk) {
state = IDLE;
//clock = false;
}else {
//Atacar
simxSetJointTargetVelocity(this->clientId, motorLeft, dir*velAtk,
simx_opmode_streaming);
simxSetJointTargetVelocity(this->clientId, motorRight, dir*velDef,
simx_opmode_streaming);
//extApi_sleepMs(100);
state = ALIGN_ATK;
}
break;
case ALIGN_DODGE:
prevState = state;
endTime = clock();
if ((endTime - startTime) >= this->tDef) {
state = IDLE;
}else {
activeSensorCounterRight = 0;
activeSensorCounterLeft = 0;
//Checar sensores da direita
for (int i = 0; i < 2; i++) {
if (simxReadProximitySensor(this->clientId, laserPointerRight[i], &res,
coord, NULL, NULL, simx_opmode_buffer) == simx_return_ok) {
//Checar coordenada z(pois está que aponta pra frente)
this->sensorDist = coord[2];
if ((this->sensorDist < MAX_DIST) && (res > 0)) {
activeSensorCounterRight++;
}
}
}
//Checar sensores da esquerda
for (int i = 0; i < 2; i++) {
if (simxReadProximitySensor(this->clientId, laserPointerLeft[i], &res,
coord, NULL, NULL, simx_opmode_buffer) == simx_return_ok) {
//Coordenada Z
this->sensorDist = coord[2];
if ((this->sensorDist < MAX_DIST) && (res > 0)) {
activeSensorCounterLeft++;
}
}
}
//Se os dois sensores da direita estao apontando para o inimigo entao recuar
if (activeSensorCounterRight > 1) {
state = DODGE;
dir = FRONT;
}//Senao atacar dando ré
else if (activeSensorCounterLeft > 1) {
state = DODGE;
dir = FRONT;
}
else {//Se nada for encontrado, entao continuar procurando
simxSetJointTargetVelocity(this->clientId, motorLeft, -V_ROT,
simx_opmode_streaming);
simxSetJointTargetVelocity(this->clientId, motorRight, V_ROT,
simx_opmode_streaming);
//extApi_sleepMs(100);
}
}
break;
case DODGE:
prevState = state;
endTime = clock();
//Se tempo de ataque acabou entao voltar ao estado inicial
if ((endTime - startTime) >= tDef) {
state = IDLE;
//clock = false;
}
else {
//Esquivar
simxSetJointTargetVelocity(this->clientId, motorLeft, dir*velDef,
simx_opmode_streaming);
simxSetJointTargetVelocity(this->clientId, motorRight, dir*velDef,
simx_opmode_streaming);
}
break;
}
/*if (state != prevState ) {
cout << this->robotName<<": Mudanca de estado: " << stateName(prevState) << " para " << stateName(state) << endl;
}*/
}
int main(int argc,char* argv[])
{
int portNb=0;
int leftMotorHandle;
int rightMotorHandle;
int cuboidHandle;
int goalHandle;
if (argc>=6)
{
portNb=atoi(argv[1]);
leftMotorHandle=atoi(argv[2]);
rightMotorHandle=atoi(argv[3]);
cuboidHandle=atoi(argv[4]);
goalHandle=atoi(argv[5]);
}
else
{
extApi_sleepMs(5000);
return 0;
}
int clientID=simxStart((simxChar*)"127.0.0.1",portNb,true,true,2000,5);
if (clientID!=-1)
{
int driveBackStartTime=-99000;
float motorSpeeds[2];
float leftMotorAngle;
float ObjectPosition[3];
float GoalPosition[3];
float ObjectOrientation[3];
float minDistance=0.3;
if (simxGetConnectionId(clientID)!=-1)
{
simxUChar sensorTrigger=0;
simxGetObjectHandle(clientID, argv[2], &leftMotorHandle, simx_opmode_blocking);
simxGetObjectHandle(clientID, argv[3], &rightMotorHandle, simx_opmode_blocking);
simxGetObjectHandle(clientID, argv[4], &cuboidHandle, simx_opmode_blocking);
simxGetObjectHandle(clientID, argv[5], &goalHandle, simx_opmode_blocking);
simxGetObjectPosition(clientID,goalHandle,-1,GoalPosition,simx_opmode_oneshot_wait);
MovetoPoint(GoalPosition, minDistance, clientID, leftMotorHandle, rightMotorHandle, cuboidHandle);
simxSetJointTargetVelocity(clientID,leftMotorHandle,0,simx_opmode_blocking);
simxSetJointTargetVelocity(clientID,rightMotorHandle,0,simx_opmode_blocking);
extApi_sleepMs(5);
}
printf("Fin!\n");
simxFinish(clientID);
}
return(0);
}
