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;
}
