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