// Find the fitness for obstacle avoidance of the passed controller
double fitfunc(double waypoints[DATASIZE],int its) {
double fitness = 0.0; // Fitness of waypoint set
int j,i;
int left_encoder, right_encoder, old_left_encoder = 0, old_right_encoder = 0;
double theta = 0.0, old_theta = 0.0;
double x = 0.0, y = 0.0;
int k = 0;
int ds_l, ds_r, lspeed, rspeed;
double fit_ds, fit_speed, fit_totalds, fitness_distance, fitness_stuck, fitness_goal, fitness_waypoints;
fit_speed = 0.0;
fit_ds = 0.0;
fit_totalds = 0.0;
int count_stuck = 0;
int count_waypoints = 0;
int count_waypoints_actual = 0;
fitness_goal = 0.333;
double waypoints_reached[DATASIZE];
for (i=0; i<DATASIZE; i++) {
waypoints_reached[i] = 0.0;
}
wb_differential_wheels_set_encoders(0,0);
// Evaluate fitness repeatedly
for (j=0; j<its; j++) {
// if close enough to current waypoint select next waypoint
if ((x - waypoints[k])*(x - waypoints[k])+(y - waypoints[k+1])*(y - waypoints[k+1]) < 0.0001) {
//printf("WAYPOINT REACHED (%.2f,%.2f) YESSS \n",waypoints[k], waypoints[k+1]);
count_waypoints += 1;
count_waypoints_actual += 1;
waypoints_reached[k] = waypoints[k];
waypoints_reached[k+1] = waypoints[k+1];
k += 2;
loc_state = ALIGN;
if (k == DATASIZE) {
k = 0;
}
}
// compute wheel speeds
compute_wheel_speeds(x,y,theta,waypoints[k],waypoints[k+1],&lspeed,&rspeed);
wb_differential_wheels_set_speed(lspeed,rspeed);
robot_step(128); // run one step
// compute current position
left_encoder = wb_differential_wheels_get_left_encoder();
right_encoder = wb_differential_wheels_get_right_encoder();
ds_l = left_encoder - old_left_encoder;
ds_r = right_encoder - old_right_encoder;
odometry(ds_l, ds_r, x, y, old_theta, &x, &y, &theta, &fit_ds);
old_left_encoder = left_encoder;
old_right_encoder = right_encoder;
old_theta = theta;
fit_speed += (fabs(lspeed) + fabs(rspeed))/(2.0*MAX_SPEED);
fit_totalds += fit_ds;
if(fit_ds < 0.0001) {
count_stuck += 1;
}
if (y > 1.2) { // when this coordinate is reached the robot is considered escaped and the evaluation stops
printf("\nROBOT ESCAPED!! :)\n");
for (i=0; i<DATASIZE; i++) {
printf("%f ",waypoints_reached[i]);
}
printf("\n");
count_waypoints = 5; //this acts as a bonus for reaching the goal
fitness_goal = 1.0;
break;
}
}
fit_speed /= its;
fitness_distance = 1/(1+exp(fit_totalds-4));
fitness_stuck = (1 - (double)count_stuck/its);
fitness_goal = fitness_goal;
fitness_waypoints = pow((((double)count_waypoints/10)+0.5),2); //1.2 are assumed to be the length of sides of the world
fitness = pow(fitness_distance,0.75)*pow(fitness_stuck,1.5)*fitness_goal*fitness_waypoints;
/*
if (fitness > fitness_best) {
fitness_best = fitness;
printf("\nFITNESS: %f\nTotal Distance: %.2f\nStuck Ratio: %.2f\n\n", fitness, fit_totalds, fitness_stuck);
}
*/
printf("FITNESS: %f waypoints: %d ditance: %.2f stuck: %.2f\n", fitness, count_waypoints_actual, fit_totalds, fitness_stuck);
return fitness;
}
// 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);
}
}
