void change_robot_positions(){
int i;
WbFieldRef rotfield;
int location[2];
int rotation;
double newlocation[3]={0.0, 0.0, 0.0}; //3D vector (x,z,y)
double newrotation[4]={0.0, 1.0, 0.0, 0.0}; //3D unit vector (rotation axis), angle (rad)
for(i=0;i<NUM_ROBOTS;i++){
//randomize e-puck location inside the arena
location[0]=(rand()%(ARENASIDE-10))-ARENASIDE/2+5;
location[1]=(rand()%(ARENASIDE-10))-ARENASIDE/2+5;
newlocation[0]=((double)location[0])/100;
newlocation[1]=0.0;
newlocation[2]=((double)location[1])/100;
wb_supervisor_field_set_sf_vec3f(locfield[i],newlocation);
//randomize e-puck rotation (steps of 0.0628rad)
rotation=rand()%100;
newrotation[3]=(double)rotation/100*2*M_PI;
rotfield=wb_supervisor_node_get_field(epucks[i],"rotation");
wb_supervisor_field_set_sf_rotation(rotfield,newrotation);
}
}
// move robot to a 3d position
static void move_robot_3d(int robot, double tx, double ty, double tz, double alpha) {
if (robot_translation[robot] && robot_rotation[robot]) {
double trans[3] = { tx, ty, tz };
double rot[4] = { 0, 1, 0, alpha };
wb_supervisor_field_set_sf_vec3f(robot_translation[robot], trans);
wb_supervisor_field_set_sf_rotation(robot_rotation[robot], rot);
}
}
//
//resets the position of the robot before each trial
//
void resetRobotPosition(void)
{ //TODO: Add random rotation angle about z-axis
if (wb_supervisor_node_get_type(robot) == WB_NODE_NO_NODE)
printf("Error: node %s not found\n", "EPUCK");
//Initial position of the robot at each time step..
int i;
for (i=0; i<2; i++) robot_initial_position[i] = 0.0;//0.6*(double)rand()/(double)RAND_MAX-0.3;
robot_initial_position[2] = 0.4;
robot_initial_rotation[3]=((pi/2)*((double) rand()/(double) RAND_MAX))-pi/4;
wb_supervisor_field_set_sf_vec3f(trans_field, robot_initial_position);
wb_supervisor_field_set_sf_rotation(rot_field,robot_initial_rotation);
return;
}
/* Reset the supervisor */
void reset(void) {
int i;
char s[50];
char *eventprefix = (char *) "e";
printed = false;
for (i=0;i<ROBOTS;i++)
{
char aux[15];
/* Get and save a reference to the robot. */
sprintf(aux,"%s%d",rob_prefix,i+1);
// rob_name << rob_prefix << i+1; // << "_0";
rob[i] = wb_supervisor_node_get_from_def(aux);
wb_supervisor_field_set_sf_vec3f(wb_supervisor_node_get_field(rob[i],"translation"), initLoc[i]);
wb_supervisor_field_set_sf_rotation(wb_supervisor_node_get_field(rob[i],"rotation"), initRot[i]);
robTrans[i] = wb_supervisor_node_get_field(rob[i],"translation");
previous_x[i] = wb_supervisor_field_get_sf_vec3f(robTrans[i])[0];
previous_y[i] = wb_supervisor_field_get_sf_vec3f(robTrans[i])[2];
/* Get robot emitters */
strcpy(aux,emi_prefix);
sprintf(aux,"%s%d",aux,i+1);
}
printf("Initializing events...\n");
for (i=0; i<MAX_EVENTS; i++)
{
sprintf(s, "%s%d", eventprefix, i);
// Define position of event and place it there
events[i].event = wb_supervisor_node_get_from_def(s);
events[i].eventTrans = wb_supervisor_node_get_field(events[i].event,"translation");
events[i].state = 1.0;
randomize_event_position(i);
randomize_event_color(i);
}
}
int main() {
printf("hello from supervisor\n");
const char *robot_name[ROBOTS] = {"NAO"};
WbNodeRef node;
WbFieldRef robot_translation_field[ROBOTS],robot_rotation_field[ROBOTS],ball_translation_field;
//WbDeviceTag emitter, receiver;
int i,j;
int score[2] = { 0, 0 };
double time = 10 * 60; /* a match lasts for 10 minutes */
double ball_reset_timer = 0;
double ball_initial_translation[3] = { -2.5, 0.0324568, 0 };
double robot_initial_translation[ROBOTS][3] = {
{-4.49515, 0.234045, -0.0112415},
{0.000574037, 0.332859, -0.00000133636}};
double robot_initial_rotation[ROBOTS][4] = {
{0.0604202, 0.996035, -0.0652942, 1.55047},
{0.000568956, 0.70711, 0.707104, 3.14045}};
double packet[ROBOTS * 3 + 2];
char time_string[64];
const double *robot_translation[ROBOTS], *robot_rotation[ROBOTS], *ball_translation;
wb_robot_init();
time_step = wb_robot_get_basic_time_step();
emitter = wb_robot_get_device("emitter");
wb_receiver_enable(emitter, time_step);
receiver = wb_robot_get_device("receiver");
wb_receiver_enable(receiver, time_step);
for (i = 0; i < ROBOTS; i++) {
node = wb_supervisor_node_get_from_def(robot_name[i]);
robot_translation_field[i] = wb_supervisor_node_get_field(node,"translation");
robot_translation[i] = wb_supervisor_field_get_sf_vec3f(robot_translation_field[i]);
for(j=0;j<3;j++) robot_initial_translation[i][j]=robot_translation[i][j];
robot_rotation_field[i] = wb_supervisor_node_get_field(node,"rotation");
robot_rotation[i] = wb_supervisor_field_get_sf_rotation(robot_rotation_field[i]);
for(j=0;j<4;j++) robot_initial_rotation[i][j]=robot_rotation[i][j];
}
node = wb_supervisor_node_get_from_def("BALL");
ball_translation_field = wb_supervisor_node_get_field(node,"translation");
ball_translation = wb_supervisor_field_get_sf_vec3f(ball_translation_field);
for(j=0;j<3;j++) ball_initial_translation[j]=ball_translation[j];
/* printf("ball initial translation = %g %g %g\n",ball_translation[0],ball_translation[1],ball_translation[2]); */
set_scores(0, 0);
while(wb_robot_step(TIME_STEP)!=-1) {
//printf("supervisor commands START!\n");
check_for_slaves_data();
ball_translation = wb_supervisor_field_get_sf_vec3f(ball_translation_field);
for (i = 0; i < ROBOTS; i++) {
robot_translation[i]=wb_supervisor_field_get_sf_vec3f(robot_translation_field[i]);
/* printf("coords for robot %d: %g %g %g\n",i,robot_translation[i][0],robot_translation[i][1],robot_translation[i][2]); */
packet[3 * i] = robot_translation[i][0]; /* robot i: X */
packet[3 * i + 1] = robot_translation[i][2]; /* robot i: Z */
if (robot_rotation[i][1] > 0) { /* robot i: rotation Ry axis */
packet[3 * i + 2] = robot_rotation[i][3]; /* robot i: alpha */
} else { /* Ry axis was inverted */
packet[3 * i + 2] = -robot_rotation[i][3];
}
}
packet[3 * ROBOTS] = ball_translation[0]; /* ball X */
packet[3 * ROBOTS + 1] = ball_translation[2]; /* ball Z */
wb_emitter_send(emitter, packet, sizeof(packet));
/* Adds TIME_STEP ms to the time */
time -= (double) TIME_STEP / 1000;
if (time < 0) {
time = 10 * 60; /* restart */
}
sprintf(time_string, "%02d:%02d", (int) (time / 60), (int) time % 60);
wb_supervisor_set_label(2, time_string, 0.45, 0.01, 0.1, 0x000000, 0.0); /* black */
if (ball_reset_timer == 0) {
if (ball_translation[0] > GOAL_X_LIMIT) { /* ball in the blue goal */
set_scores(++score[0], score[1]);
ball_reset_timer = 3; /* wait for 3 seconds before reseting the ball */
} else if (ball_translation[0] < -GOAL_X_LIMIT) { /* ball in the yellow goal */
set_scores(score[0], ++score[1]);
ball_reset_timer = 3; /* wait for 3 seconds before reseting the ball */
}
} else {
ball_reset_timer -= (double) TIME_STEP / 1000.0;
if (ball_reset_timer <= 0) {
ball_reset_timer = 0;
wb_supervisor_field_set_sf_vec3f(ball_translation_field, ball_initial_translation);
for (i = 0; i < ROBOTS; i++) {
wb_supervisor_field_set_sf_vec3f(robot_translation_field[i], robot_initial_translation[i]);
wb_supervisor_field_set_sf_rotation(robot_rotation_field[i], robot_initial_rotation[i]);
}
}
}
}
wb_robot_cleanup();
return 0;
}
/*
* This is the main program.
* The arguments of the main function can be specified by the
* "controllerArgs" field of the Robot node
*/
int main(int argc, char **argv)
{
//int cont=0;
using_shared_memory();
/* necessary to initialize webots stuff */
wb_robot_init();
// do this once only Darwin
WbNodeRef robot_node = wb_supervisor_node_get_from_def("Darwin");
WbFieldRef trans_field = wb_supervisor_node_get_field(robot_node, "translation");
WbFieldRef rot_field = wb_supervisor_node_get_field(robot_node, "rotation");
int caiu_cont = 0;
// do this once only Darwin2
WbNodeRef robot_node_2 = wb_supervisor_node_get_from_def("Darwin2");
WbFieldRef trans_field_2 = wb_supervisor_node_get_field(robot_node_2, "translation");
WbFieldRef rot_field_2 = wb_supervisor_node_get_field(robot_node_2, "rotation");
// do this once only Darwin3
WbNodeRef robot_node_3 = wb_supervisor_node_get_from_def("Darwin3");
WbFieldRef trans_field_3 = wb_supervisor_node_get_field(robot_node_3, "translation");
WbFieldRef rot_field_3 = wb_supervisor_node_get_field(robot_node_3, "rotation");
while (1) {
// this is done repeatedly
const double *trans = wb_supervisor_field_get_sf_vec3f(trans_field);
const double *trans_2 = wb_supervisor_field_get_sf_vec3f(trans_field_2);
const double *trans_3 = wb_supervisor_field_get_sf_vec3f(trans_field_3);
TRANS1 = trans[0];
TRANS2 = trans[1];
TRANS3 = trans[2];
TRANS1_2 = trans_2[0];
TRANS2_2 = trans_2[1];
TRANS3_2 = trans_2[2];
TRANS1_3 = trans_3[0];
TRANS2_3 = trans_3[1];
TRANS3_3 = trans_3[2];
//printf("MY_ROBOT is at position: %g %g %g\n", trans[0], trans[1], trans[2]);
wb_robot_step(32);
if(RESET_ROBOT == 1)
{
caiu_cont++;
CAIU_CONT = caiu_cont;
// reset robot position
const double INITIAL[3] = { 0, 0.32004, 0 };
const double INITIAL_ROT[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field, INITIAL);
wb_supervisor_field_set_sf_rotation(rot_field, INITIAL_ROT);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT = 0;
if(caiu_cont > 25)
{
caiu_cont = 0;
wb_supervisor_simulation_revert(); // restart the simulation
// A medida que o tempo vai passando o robo Darwin vai ficando
// cada vez mais devagar como se houvesse algum tipo de desgaste fisico
//entao e necessario restarta a simulaçao
}
}
if(RESET_ROBOT_2 == 1)
{
// reset robot position
const double INITIAL_2[3] = { 0.2, 0.32004, 4.7 };
const double INITIAL_ROT_2[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field_2, INITIAL_2);
wb_supervisor_field_set_sf_rotation(rot_field_2, INITIAL_ROT_2);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT_2 = 0;
}
if(RESET_ROBOT_3 == 1)
{
// reset robot position
const double INITIAL_3[3] = { 3.2, 0.32004, -3.7 };
const double INITIAL_ROT_3[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field_3, INITIAL_3);
wb_supervisor_field_set_sf_rotation(rot_field_3, INITIAL_ROT_3);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT_3 = 0;
}
}
/*
* You should declare here WbDeviceTag variables for storing
* robot devices like this:
* WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
* WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
*/
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}
