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;
}
// 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);
}
}
/*
* processing all the received ping messages, and calculate range and bearing to the other robots
* the range and bearing are measured directly out of message RSSI and direction
*/
void process_received_ping_messages(void)
{
const double *message_direction;
double message_rssi; // Received Signal Strength indicator
double theta;
double range;
char *inbuffer; // Buffer for the receiver node
int other_robot_id;
double other_robot_bearing;
while (wb_receiver_get_queue_length(receiver) > 0) {
inbuffer = (char*) wb_receiver_get_data(receiver);
message_direction = wb_receiver_get_emitter_direction(receiver);
message_rssi = wb_receiver_get_signal_strength(receiver);
//should be x and z position (y is up)
double x = message_direction[0];
double z = message_direction[2];
//printf("ROBOT %d: message_direction: %f, %f, %f\n", robot_id, message_direction[0], message_direction[1], message_direction[2]);
theta = -atan2(z,x);
theta = theta + my_position[2]; // find the relative theta;
range = sqrt((1/message_rssi));
other_robot_id = (int)(inbuffer[5]-'0'); // since the name of the sender is in the received message. Note: this does not work for robots having id bigger than 9!
other_robot_bearing = 0;
sscanf(inbuffer+7, "%lf", &other_robot_bearing);
// Get position update
prev_relative_pos[other_robot_id][0] = relative_pos[other_robot_id][0];
prev_relative_pos[other_robot_id][1] = relative_pos[other_robot_id][1];
relative_pos[other_robot_id][0] = range*cos(theta); // relative x pos
relative_pos[other_robot_id][1] = -1.0 * range*sin(theta); // relative y pos
// Get bearing
neighboors_bearing[other_robot_id] = other_robot_bearing;
//printf("Robot %s, from robot %d, x: %g, y: %g, theta %g, my theta %g\n",robot_name,other_robot_id,relative_pos[other_robot_id][0],relative_pos[other_robot_id][1],my_position[2]*180.0/3.141592,my_position[2]*180.0/3.141592);
relative_speed[other_robot_id][0] = (1/DELTA_T)*(relative_pos[other_robot_id][0]-prev_relative_pos[other_robot_id][0]);
relative_speed[other_robot_id][1] = (1/DELTA_T)*(relative_pos[other_robot_id][1]-prev_relative_pos[other_robot_id][1]);
wb_receiver_next_packet(receiver);
}
}
//check whether slave have sent values to the supervisor
void check_for_slaves_data(w){
if (wb_receiver_get_queue_length(receiver) > 0) {
int i, j, m, n;
const unsigned char *image_values = wb_receiver_get_data(receiver);
const int received_data_size = wb_receiver_get_data_size(receiver)/sizeof(unsigned char);
// printf("supervisor\n");
//printf_ByteArray(image_values, 160*120);
ImageData image_data;
image_data.image_values = image_values;
image_data.image_size = received_data_size;
wb_robot_window_custom_function(&image_data);
wb_receiver_next_packet(receiver);
}
}
int countNeighbors() {
// Resent counters
for(int i = 1; i <= NUM_ROBOTS; ++i)
isPresent[i] = false;
int n = 0;
while(wb_receiver_get_queue_length(receiverTag) > 0) {
char * neighborName = (char *)wb_receiver_get_data(receiverTag);
int id = (int)strtol(neighborName, NULL, 10);
isPresent[id] = true;
//printf("Robot %s received: %d present!\n", robotName, id);
wb_receiver_next_packet(receiverTag);
}
for(int i = 1; i <= NUM_ROBOTS; ++i) {
if(isPresent[i])
n++;
}
assert(n <= NUM_ROBOTS - 1);
return n;
}
//
//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;
}
// 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);
}
}
/*
* processing all the received ping messages, and calculate range and bearing to the other robots
* the range and bearing are measured directly out of message RSSI and direction
*/
void process_received_ping_messages(void)
{
const double *message_direction;
double message_rssi; // Received Signal Strength indicator
double theta;
double range;
char *inbuffer; // Buffer for the receiver node
int other_robot_id;
unsigned int recv_timestamp;
while (wb_receiver_get_queue_length(receiver) > 0) {
inbuffer = (char*) wb_receiver_get_data(receiver);
message_direction = wb_receiver_get_emitter_direction(receiver);
message_rssi = wb_receiver_get_signal_strength(receiver);
//should be x and z position (y is up)
double x = message_direction[0];
double z = message_direction[2];
//printf("ROBOT %d: message_direction: %f, %f, %f\n", robot_id, message_direction[0], message_direction[1], message_direction[2]);
theta = -atan2(z,x);
theta = theta + my_position[2]; // find the relative theta;
range = sqrt((1/message_rssi));
//other_robot_id = (int)(inbuffer[5]-'0'); // old: since the name of the sender is in the received message. Note: this does not work for robots having id bigger than 9!
sscanf(inbuffer, "%d", &other_robot_id);
sscanf( (inbuffer+8), "%u", &recv_timestamp);
//printf("recieved: id%d, ts%d\n", other_robot_id, recv_timestamp);
//only update the position if there is something new
if(timestamp[other_robot_id] < recv_timestamp){
if(maxTimestamp < recv_timestamp){
maxTimestamp = recv_timestamp;
}
float relativ_pos_x = 0.0f;
float relativ_pos_z = 0.0f;
sscanf( (inbuffer+16), "%f", &relativ_pos_x);
sscanf( (inbuffer+24), "%f", &relativ_pos_z);
// Get position update
prev_relative_pos[other_robot_id][0] = relative_pos[other_robot_id][0];
prev_relative_pos[other_robot_id][1] = relative_pos[other_robot_id][1];
//get relativ_pos of the sending robot
relative_pos[other_robot_id][0] = range*cos(theta); // relative x pos
relative_pos[other_robot_id][1] = -1.0 * range*sin(theta); // relative y pos
//add the relativ pos the sending robot gave us
relative_pos[other_robot_id][0] += relativ_pos_x;
relative_pos[other_robot_id][1] += relativ_pos_z;
//printf("Robot %s, from robot %d, x: %g, y: %g, theta %g, my theta %g\n",robot_name,other_robot_id,relative_pos[other_robot_id][0],relative_pos[other_robot_id][1],my_position[2]*180.0/3.141592,my_position[2]*180.0/3.141592);
relative_speed[other_robot_id][0] = (1/DELTA_T)*(relative_pos[other_robot_id][0]-prev_relative_pos[other_robot_id][0]);
relative_speed[other_robot_id][1] = (1/DELTA_T)*(relative_pos[other_robot_id][1]-prev_relative_pos[other_robot_id][1]);
timestamp[other_robot_id] = recv_timestamp;
//sends info to other robots
send_ping_robot(other_robot_id);
}
wb_receiver_next_packet(receiver);
}
}
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;
}
////////////////////////////////////////////
// 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;
}
