void send_data(double tstamp) { buffer[0] = tstamp; buffer[1] = robot_id; buffer[2] = sensor_value; wb_emitter_send(emitter,buffer,3*sizeof(double)); pkt_count++; }
int main() { double buffer[255]; double fit; double *rbuffer; int i; wb_robot_init(); reset(); receiver_enable(rec,32); differential_wheels_enable_encoders(64); robot_step(64); while (1) { // Wait for data while (receiver_get_queue_length(rec) == 0) { robot_step(64); } rbuffer = (double *)wb_receiver_get_data(rec); // Check for pre-programmed avoidance behavior if (rbuffer[DATASIZE] == -1.0) { fitfunc(gwaypoints,100); // Otherwise, run provided controller } else { fit = fitfunc(rbuffer,rbuffer[DATASIZE]); buffer[0] = fit; wb_emitter_send(emitter,(void *)buffer,sizeof(double)); } wb_receiver_next_packet(rec); } return 0; }
int main(int argc, char *args[]) { char buffer[255]; // Buffer for sending data int i; // Index if (argc == 4) { // Get parameters offset = atoi(args[1]); migrx = atof(args[2]); migrz = atof(args[3]); //migration goal point comes from the controller arguments. It is defined in the world-file, under "controllerArgs" of the supervisor. printf("Migratory instinct : (%f, %f)\n", migrx, migrz); } else { printf("Missing argument\n"); return 1; } orient_migr = -atan2f(migrx,migrz); if (orient_migr<0) { orient_migr+=2*M_PI; // Keep value within 0, 2pi } reset(); send_init_poses(); // Compute reference fitness values float fit_cluster; // Performance metric for aggregation float fit_orient; // Performance metric for orientation for(;;) { wb_robot_step(TIME_STEP); if (t % 10 == 0) { for (i=0;i<FLOCK_SIZE;i++) { // Get data loc[i][0] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0]; // X loc[i][1] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2]; // Z loc[i][2] = wb_supervisor_field_get_sf_rotation(robs_rotation[i])[3]; // THETA // Sending positions to the robots, comment the following two lines if you don't want the supervisor sending it sprintf(buffer,"%1d#%f#%f#%f##%f#%f",i+offset,loc[i][0],loc[i][1],loc[i][2], migrx, migrz); wb_emitter_send(emitter,buffer,strlen(buffer)); } //Compute and normalize fitness values compute_fitness(&fit_cluster, &fit_orient); fit_cluster = fit_cluster_ref/fit_cluster; fit_orient = 1-fit_orient/M_PI; printf("time:%d, Topology Performance: %f\n", t, fit_cluster); } t += TIME_STEP; } }
// the main function int main(){ int msl, msr; // Wheel speeds double *rbuffer; double buffer[255]; int j; for(;;) { reset(); // Resetting the robot while (wb_receiver_get_queue_length(receiver0) == 0) wb_robot_step(64); rbuffer = (double *)wb_receiver_get_data(receiver0); COEF_KP = rbuffer[0]; COEF_B = rbuffer[1]; COEF_R = rbuffer[2]; COEF_S = rbuffer[3]; COEF_T = rbuffer[4]; msl = 0; msr = 0; // Forever for(j = 0; j < NB_STEPS; ++j) { /* Send and get information */ send_ping(); // sending a ping to other robot, so they can measure their distance to this robot process_received_ping_messages(); // Compute self position prev_my_position[0] = my_position[0]; prev_my_position[1] = my_position[1]; update_self_motion(msl,msr); speed[robot_id][0] = (1/DELTA_T)*(my_position[0]-prev_my_position[0]); speed[robot_id][1] = (1/DELTA_T)*(my_position[1]-prev_my_position[1]); // Flocking behavior of the paper with the wheels speed flocking_behavior(&msl, &msr); // Set speed wb_differential_wheels_set_speed(msl,msr); // Continue one step wb_robot_step(TIME_STEP); } wb_emitter_send(emitter0,(void *)buffer,sizeof(double)); wb_receiver_next_packet(receiver0); } }
void report_step_state_to_supervisor(){ // send message to supervisor int width = wb_camera_get_width(CameraTop); int height = wb_camera_get_height(CameraTop); // read rgb pixel values from the camera const unsigned char *image = wb_camera_get_image(CameraTop); // printf("player\n"); //printf_ByteArray(image, width*height); wb_emitter_send(emitter, image, height * width * 8); //free(data_values); }
int main(int argc, char *argv[]) { /* define variables */ WbDeviceTag turret_sensor; double perf_data[3]; double clock = 0; /* initialize Webots */ wb_robot_init(); // read robot id from the robot's name char* robot_name; robot_name=(char*) wb_robot_get_name(); sscanf(robot_name,"e-puck%d",&robot_id); emitter = wb_robot_get_device("emitter"); wb_emitter_set_range(emitter,COM_RANGE); receiver = wb_robot_get_device("receiver"); wb_receiver_set_channel(receiver,0); wb_receiver_enable(receiver,TIME_STEP); wb_robot_step(TIME_STEP); turret_sensor = wb_robot_get_device("ts"); wb_light_sensor_enable(turret_sensor,TIME_STEP); /* main loop */ while (wb_robot_step(TIME_STEP) != -1) { clock += (double)TIME_STEP/1000; /* get light sensor value */ sensor_value = wb_light_sensor_get_value(turret_sensor); if(wb_receiver_get_queue_length(receiver) > 0) { wb_emitter_set_channel(emitter,3); perf_data[0] = (double)robot_id; perf_data[1] = (double)(pkt_count-1); perf_data[2] = 0; wb_emitter_send(emitter,perf_data,3*sizeof(double)); break; // stop node } else { send_data(clock); // send measurement data } } wb_robot_cleanup(); return 0; }
void send_init_poses(void) { char buffer[255]; // Buffer for sending data int i; for (i=0;i<FLOCK_SIZE;i++) { // Get data loc[i][0] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0]; // X loc[i][1] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2]; // Z loc[i][2] = wb_supervisor_field_get_sf_rotation(robs_rotation[i])[3]; // THETA printf("Supervisor %d %d %d/n",loc[i][0],loc[i][0],loc[i][0]); // Send it out sprintf(buffer,"%1d#%f#%f#%f##%f#%f",i+offset,loc[i][0],loc[i][1],loc[i][2], migrx, migrz); wb_emitter_send(emitter,buffer,strlen(buffer)); // Run one step wb_robot_step(TIME_STEP); } }
/* * sends a ping of a certain robot value * without updating the timestamp */ void send_ping_robot(int other_robot_id){ char out[32]; //strcpy(out,robot_number); // in the ping message we send the name of the robot. sprintf(out, "%d", other_robot_id); sprintf( (out+8), "%u", timestamp[other_robot_id] ); //printf("timestamp sent: %s\n", out+8); if(other_robot_id != robot_id){ sprintf( (out+16), "%3.4f", relative_pos[other_robot_id][0] ); sprintf( (out+24), "%3.4f", relative_pos[other_robot_id][1] ); } else{ //no relative pos with the current robot sprintf( (out+16), "%3.4f", 0.0f ); sprintf( (out+24), "%3.4f", 0.0f ); } //printf("%f.4\n", relative_pos[other_robot_id][0] ); wb_emitter_send(emitter,out,32); }
int main(int argc, char **argv) { wb_robot_init(); int time_step = wb_robot_get_basic_time_step(); WbDeviceTag gps = wb_robot_get_device("GPS"); wb_gps_enable(gps, time_step); WbDeviceTag emitter = wb_robot_get_device("emitter"); wb_emitter_set_channel(emitter,13); double* gps_value; char message[32]; while (wb_robot_step(time_step) != -1){ gps_value = wb_gps_get_values(gps); //Something's wrong with deserialization //So we make a fixed width string here sprintf(message,"{%0.3f,%0.3f}",gps_value[0]/2+5.0,-gps_value[2]/2+5.0); wb_emitter_send(emitter,message,strlen(message)+1); // printf("%s\n",message); } wb_robot_cleanup(); return 0; }
//////////////////////////////////////////// // Main static int run(int ms) { int i; if (mode!=wb_robot_get_mode()) { mode = wb_robot_get_mode(); if (mode == SIMULATION) { for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_SIMULATION[i]; printf("Switching to SIMULATION.\n\n"); } else if (mode == REALITY) { for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_REALITY[i]; printf("\nSwitching to REALITY.\n\n"); } } // if we're testing a new genome, receive weights and initialize trial if (step == 0) { int n = wb_receiver_get_queue_length(receiver); printf("queue length %d\n",n); //wait for new genome if (n) { const double *genes = (double *) wb_receiver_get_data(receiver); //set neural network weights for (i=0;i<NB_WEIGHTS;i++) weights[i]=genes[i]; printf("wt[%d]: %g\n",i,weights[i]); wb_receiver_next_packet(receiver); } else { // printf("time step %d\n",TIME_STEP); return TIME_STEP;} const double *gps_matrix = wb_gps_get_values(gps); robot_initial_position[0] = gps_matrix[0];//gps_position_x(gps_matrix); robot_initial_position[1] = gps_matrix[2];//gps_position_z(gps_matrix); printf("rip[0]: %g, rip[1]: %g\n",robot_initial_position[0],robot_initial_position[1]); fitness = 0; } step++; printf("Step: %d\n", step); if(step < TRIAL_DURATION/(double)TIME_STEP) { //drive robot fitness+=run_trial(); //send message with current fitness double msg[2] = {fitness, 0.0}; wb_emitter_send(emitter, (void *)msg, 2*sizeof(double)); } else { //stop robot wb_differential_wheels_set_speed(0, 0); //send message to indicate end of trial double msg[2] = {fitness, 1.0}; wb_emitter_send(emitter, (void *)msg, 2*sizeof(double)); //reinitialize counter step = 0; } return TIME_STEP; return TIME_STEP; }
/* * The call to wb_robot_step is mandatory for the function * supervisor_node_was_found to be able to work correctly. */ static void reset(void) { int i; for(i=0; i<POP_SIZE; i++) fitness[i]=-1; srand(time(0)); pF1= fopen("initPop.txt","w+"); //pF2= fopen("real2IntGenome.txt","w+"); //pF3= fopen("encodedPop.txt","w+"); // Initiate the emitter used to send genomes to the experiment emitter = wb_robot_get_device("emittersupervisor"); // Initiate the receiver used to receive fitness receiver = wb_robot_get_device("receiversupervisor"); wb_receiver_enable(receiver, TIME_STEP); // Create a supervised node for the robot robot = wb_supervisor_node_get_from_def("EPUCK"); assert(robot!=NULL); pattern=wb_supervisor_node_get_from_def("FLOOR"); pattern_rotation = wb_supervisor_node_get_field(pattern,"rotation"); assert(pattern!=NULL); assert(pattern_rotation!=NULL); trans_field = wb_supervisor_node_get_field(robot,"translation"); rot_field = wb_supervisor_node_get_field(robot,"rotation"); ctrl_field= wb_supervisor_node_get_field(robot,"controller"); wb_robot_step(0); wb_robot_step(0); //this is magic // set the robot controller to nn const char *controller_name = "drive2"; wb_supervisor_field_set_sf_string(ctrl_field,controller_name); //check whether robot was found if (wb_supervisor_node_get_type(robot) == WB_NODE_NO_NODE) puts("Error: node EPUCK not found!!!"); if(EVOLVING) { //Open log files f1= fopen ("../../data/fitness.txt", "wt"); f2= fopen ("../../data/genomes.txt", "wt"); //pR= fopen("savePop.txt","w+"); //initial weights randomly initializePopulation(); //rtoi(); //real to int conversion before encoding popEncoder(); //puts("NEW EVOLUTION"); //puts("GENERATION 0"); // send genomes to experiment resetRobotPosition(); wb_emitter_send(emitter, (void *)pop_bin[evaluated_inds], GENOME_LENGTH*sizeof(_Bool)); //instead save binary genomes to a file //puts("Genes sent."); } else { // testing best individual // Read best genome from bestgenome.txt and initialize weights. f3= fopen ("../../data/bestgenome.txt", "rt"); fscanf(f3,"%d %d", &generation, &evaluated_inds); //TODO either read binary genome or make sure it is decoded prior to storage for(i=0;i<GENOME_LENGTH;i++) fscanf(f3,"%d ",&pop_bin[0][i]); //printf("TESTING INDIVIDUAL %d, GENERATION %d\n", evaluated_inds, generation); // send genomes to experiment resetRobotPosition(); // wb_emitter_send(emitter, (void *)pop[0], NB_GENES*sizeof(double)); } return; }
// //main controller of the evolution, this function never returns // static int run() { const double *fit; double finished=0; //double values[4]= {0.0,1.0,0.0,0.0}; //int epoch, reset_position; // as long as individual is being evaluated, print current fitness and return int n = wb_receiver_get_queue_length(receiver); if (n) { fit = (double *)wb_receiver_get_data(receiver); //gets msg[4]={fitness, finished,epoch,reset} fitness[evaluated_inds]=fit[0]; finished=fit[1]; //epoch= (int) fit[2]; //reset_position= (int) fit[3]; //if(reset_position) resetRobotPosition(); //if(1 == epoch || 3 == epoch){ //wb_supervisor_field_set_sf_rotation(pattern_rotation, values); //printf("Flag: %d, world is set.\n", epoch); //}else{ //values[3]=pi; //wb_supervisor_field_set_sf_rotation(pattern_rotation, values); //printf("\n Flag: %d, world is rotated.\n", epoch); //} // print generational info on screen char message[100]; sprintf(message, "Gen: %d Ind: %d Fit: %.2f", generation, evaluated_inds, fit[0]); wb_supervisor_set_label(0, message, 0, 0, 0.1, 0xff0000,0); wb_receiver_next_packet(receiver); } // if still evaluating the same individual, return. if (!finished) return TIME_STEP; //if not finished, !finished ==1. // when evaluation is done, an extra flag is returned in the message if(EVOLVING) { // if whole population has been evaluated if ((evaluated_inds+1) == POP_SIZE ) { // sort population by fitness sortPopulation(); // find and log current and absolute best individual bestfit=sortedfitness[0][0]; bestind=(int)sortedfitness[0][1]; if (bestfit > abs_bestfit) { abs_bestfit=bestfit; abs_bestind=bestind; logBest(); } //printf("best fit: %f\n", bestfit); //write data to files logPopulation(); //rank population, select best individuals and create new generation createNewPopulation(); generation++; if(200==generation) return 0; //printf("\nGENERATION %d\n", generation); evaluated_inds = 0; avgfit = 0.0; bestfit = -1.0; bestind = -1.0; resetRobotPosition(); wb_emitter_send(emitter, (void *)pop_bin[evaluated_inds], GENOME_LENGTH*sizeof(_Bool)); } else { // assign received fitness to individual //printf("fitness: %f\n", fitness[evaluated_inds]); evaluated_inds++; // send next genome to experiment resetRobotPosition(); wb_emitter_send(emitter, (void *)pop_bin[evaluated_inds], GENOME_LENGTH*sizeof(_Bool)); } } return TIME_STEP; }
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; }
// the main function int main(){ int msl, msr, bmsl, bmsr; // Wheel speeds int sum_sensors; // Braitenberg parameters int i, j; // Loop counter int max_sens; // Store highest sensor value double *rbuffer; double buffer[255]; int distances[NB_SENSORS]; // Array for the distance sensor readings for(;;) { reset(); // Resetting the robot while (wb_receiver_get_queue_length(receiver0) == 0) { wb_robot_step(64); } rbuffer = (double *)wb_receiver_get_data(receiver0); for(i=0; i<FLOCK_SIZE; i++) { timestamp[i] = 0; } msl = 0; msr = 0; max_sens = 0; // Forever for(j = 0; j < NB_STEPS; ++j){ bmsl = 0; bmsr = 0; sum_sensors = 0; max_sens = 0; /* Braitenberg */ for(i=0;i<NB_SENSORS;i++) { distances[i]=wb_distance_sensor_get_value(ds[i]); //Read sensor values sum_sensors += distances[i]; // Add up sensor values max_sens = max_sens>distances[i]?max_sens:distances[i]; // Check if new highest sensor value // Weighted sum of distance sensor values for Braitenburg vehicle bmsr += rbuffer[i] * distances[i]; bmsl += rbuffer[i+NB_SENSORS] * distances[i]; } // Adapt Braitenberg values (empirical tests) bmsl/=MIN_SENS; bmsr/=MIN_SENS; bmsl+=66; bmsr+=72; /* Send and get information */ timestamp[robot_id]++; send_ping_robot(robot_id); process_received_ping_messages(); //printf("ROBOT %d: wheels %d, %d\n", robot_id, bmsl, bmsr); // Compute self position prev_my_position[0] = my_position[0]; prev_my_position[1] = my_position[1]; update_self_motion(msl,msr); speed[robot_id][0] = (1/DELTA_T)*(my_position[0]-prev_my_position[0]); speed[robot_id][1] = (1/DELTA_T)*(my_position[1]-prev_my_position[1]); // Reynold's rules with all previous info (updates the speed[][] table) reynolds_rules(); //printf("ROBOT %d: speed: %f, %f\n", robot_id, speed[robot_id][0], speed[robot_id][1]); // Compute wheels speed from reynold's speed compute_wheel_speeds(&msl, &msr); //printf("wheels: %d, %d\n", msl, msr); // Adapt speed instinct to distance sensor values if (sum_sensors > NB_SENSORS*MIN_SENS) { msl -= msl*max_sens/(2*MAX_SENS); msr -= msr*max_sens/(2*MAX_SENS); } // Add Braitenberg msl += bmsl; msr += bmsr; // Set speed wb_differential_wheels_set_speed(msl,msr); // Continue one step wb_robot_step(TIME_STEP); } wb_emitter_send(emitter0,(void *)buffer,sizeof(double)); wb_receiver_next_packet(receiver0); } }
/* ************************* COMMUNICATION ****************************** */ void sendMessage(char const * message) { wb_emitter_send(emitterTag, message, strlen(message) + 1); // printf("Robot %s tried to send message (status %d, 0 means fail)\n", robotName, status); }
/* * each robot sends a ping message, so the other robots can measure relative range and bearing to the sender. * the message contains the robot's name * the range and bearing will be measured directly out of message RSSI and direction */ void send_ping(void) { char out[100]; sprintf(out,"%s;%.5f", robot_name, get_bearing_in_degrees()); wb_emitter_send(emitter,out,strlen(out)+1); }
static int run(int ms) { int i; if (mode!=wb_robot_get_mode()) { mode = wb_robot_get_mode(); if (mode == SIMULATION) { for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_SIMULATION[i]; //offset/baseline noise is ~35 in test world puts("Switching to SIMULATION.\n"); } else if (mode == REALITY) { for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_REALITY[i]; puts("\nSwitching to REALITY.\n"); } } // if we're testing a new genome, receive weights and initialize trial if (step == 0) { int n = wb_receiver_get_queue_length(receiver); // printf("queue length %d\n",n); reset_neural_network(); //wait for new genome if (n) { const _Bool *genes_bin = (_Bool *) wb_receiver_get_data(receiver); //decode obtained boolean genes into real numbers and set the NN weights. double *genes= popDecoder(genes_bin); //is const necessary here? //set neural network weights for (i=0;i<NB_WEIGHTS;i++) { weights[i]=genes[i]; // printf("wt[%d]: %g\n",i,weights[i]); } wb_receiver_next_packet(receiver); } else { return TIME_STEP;} fitness = 0; } step++; //printf("Step: %d\n", step); //first trial has 360 steps available, subsequent trials have 180. //At each epoch, first 360 steps (e.g. the first trial) have the number step_max=360 associated with them. if(step < TRIAL_STEPS) { //try the robot fitness+= run_trial(); //printf("Fitness in step %d: %g\n",step,fitness); double msg[2] = {fitness, 0.0}; wb_emitter_send(emitter, (void *)msg, 2*sizeof(double)); } else { //stop robot wb_differential_wheels_set_speed(0, 0); //send message to indicate end of trial double msg[2] = {fitness, 1.0}; wb_emitter_send(emitter, (void *)msg, 2*sizeof(double)); //reinitialize counter step = 0; } return TIME_STEP; return TIME_STEP; }