void compute_gps_speed() {
const double *coords = wb_gps_get_values(gps);
double vel[3] = { coords[X] - gps_coords[X], coords[Y] - gps_coords[Y], coords[Z] - gps_coords[Z] };
double dist = sqrt(vel[X] * vel[X] + vel[Y] * vel[Y] + vel[Z] * vel[Z]);
// store into global variables
gps_speed = dist / TIME_STEP * 3600.0;
memcpy(gps_coords, coords, sizeof(gps_coords));
}
//
// Trial to test the robot's behavior (according to NN) in the environment
//
double run_trial() {
double inputs[NB_INPUTS];
double outputs[NB_OUTPUTS];
//Get position of the e-puck
static double position[3]={0.0,0.0,0.0};
const double *gps_matrix = wb_gps_get_values(gps);
position[0] = gps_matrix[0];//gps_position_x(gps_matrix);
position[1] = gps_matrix[2];//gps_position_z(gps_matrix);
position[2] = gps_matrix[1];//gps_position_y(gps_matrix);
//Speed of the robot's wheels within the +SPEED_RANGE and -SPEED_RANGE values
int speed[2]={0,0};
//Maximum activation of all IR proximity sensors [0,1]
double maxIRActivation = 0;
//get sensor data, i.e., NN inputs
int i, j;
for(j=0;j<NB_INPUTS;j++) {
inputs[j]=(((double)wb_distance_sensor_get_value(ps[j])-ps_offset[j])<0)?0:((double)wb_distance_sensor_get_value(ps[j])-ps_offset[j])/((double)PS_RANGE);
//get max IR activation
if(inputs[j]>maxIRActivation) maxIRActivation=inputs[j];
printf("sensor #: %d, sensor read: %g, maxIR: %g\n", j, inputs[j], maxIRActivation);
}
//printf("max IR activation: %f\n", maxIRActivation);
// Run the neural network and computes the output speed of the robot
run_neural_network(inputs, outputs);
speed[LEFT] = SPEED_RANGE*outputs[0];
speed[RIGHT] = SPEED_RANGE*outputs[1];
// If you are running against an obstacle, stop the wheels
if ( maxIRActivation > 0.9 ) {
speed[LEFT]=0;
speed[RIGHT]=0;
}
// Set wheel speeds to output values
wb_differential_wheels_set_speed(speed[LEFT], speed[RIGHT]); //left, right
// Stop the robot if it is against an obstacle
for (i=0;i<NB_DIST_SENS;i++) {
int tmpps=(((double)wb_distance_sensor_get_value(ps[i])-ps_offset[i])<0)?0:((double)wb_distance_sensor_get_value(ps[i])-ps_offset[i]);
if(OBSTACLE_THRESHOLD<tmpps ) {// proximity sensors
//printf("%d \n",tmpps);
speed[LEFT] = 0;
speed[RIGHT] = 0;
break;
}
}
return compute_fitness(speed, position, maxIRActivation);
}
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;
}
int main(){
reset();
int msl,msr; // motor speed left and right
double distances[NB_SENSORS]; // array keeping the distance sensor readings
int elapsed_time; // elapsed time during one time_step
int obstacle_near; // flag which becomes true if an obstacle is nearby
int hit_flag; // flag which becomes true as soon as the robot hits an obstacle
int last_ts_was_hit = 0; // flag memorizing whether the robot was already in a collision during the last
// time step
int time_steps; // counter for simulated time steps
int hit_counter; // counter for time steps involving collisions
int collision_duration; // duration of a single collision (in time steps unit)
double average_collision_duration;// average duration of a single collision
const double* gpsvalues; // matrix containing gps readings
double actx,acty; // current position
double oldx,oldy; // previous position
double speed; // average robot speed in free space
int collision_started = 0;
// iteration variables
int i;
int sensor_nb;
FILE* f; // file handle
char fname[]="/tmp/collision_pos.txt"; // file name to store the positions of the collisions
if(fmod(TIME_STEP,50)!=0){
printf("ERROR: The TIME_STEP needs to be a multiple of 50ms\n");
return(0);
}
for(i=0;i<NB_SENSORS;i++) {
wb_distance_sensor_enable(ps[i],50); // this function wb_*_enable() allows to specify an update interval for the sensor readings in milliseconds
}
wb_gps_enable(gps,50);
wb_robot_step(50);
gpsvalues = wb_gps_get_values(gps); // returns a pointer to three double values. The pointer is the address of an array allocated by the function internally.
wb_robot_step(50);
oldx=gpsvalues[0];
oldy=gpsvalues[2];
time_steps=0;
hit_counter=0;
hit_flag=0;
average_collision_duration=0;
speed=0;
int ts_2=0, ts_3=0, ts_4=0, ts_5=0;
for(;;){
elapsed_time=0; // reset elapsed time and hit flag
ts_5 = ts_4;
ts_4 = ts_3;
ts_3 = ts_2;
ts_2 = last_ts_was_hit;
last_ts_was_hit=hit_flag; // copying the status of the last time step
if(!last_ts_was_hit) {
collision_duration=1; // if not actually colliding, always reset counter
}
hit_flag=0; // every time step
while(elapsed_time<TIME_STEP){
msl=0; msr=0;
for(sensor_nb=0;sensor_nb<NB_SENSORS;sensor_nb++){ // read sensor values and calculate motor speeds
distances[sensor_nb] = wb_distance_sensor_get_value(ps[sensor_nb]);
//printf("t=%d, distance[%d] = %f\n",time_steps,sensor_nb,distances[sensor_nb]);
msr += distances[sensor_nb] * Interconn[sensor_nb];
msl += distances[sensor_nb] * Interconn[sensor_nb + NB_SENSORS];
}
msl /= 350; msr /= 350; // Normalizing speeds
actx=gpsvalues[0];
acty=gpsvalues[2];
//printf("x = %f, y = %f\n",actx, acty);
obstacle_near=(abs(msl)>10 || abs(msr)>10);
if(obstacle_near){ // if motor speed changes (if obstacle is hit)
//printf("obstacle_near=%d msr = %d msl = %d \n", obstacle_near, msr, msl);
hit_flag=1;
f=fopen(fname,"a");
fprintf(f,"%f %f\n",
actx,
acty);
fclose(f);
} else { // measure speed only when no obstacles near
double dist = sqrt((actx-oldx)*(actx-oldx)+(acty-oldy)*(acty-oldy)); // distance traveled in 50ms
if(dist<0.01) // don't consider fast speed due to wrap around
speed=speed*0.999+0.001*dist*1000.0/50.0;
}
oldx=actx;
oldy=acty;
msl += (BIAS_SPEED);
msr += (BIAS_SPEED);
wb_differential_wheels_set_speed(3*msl,3*msr);
gpsvalues = wb_gps_get_values(gps);
wb_robot_step(50); // Executing the simulation for 50ms
elapsed_time+=50;
} // end time step
time_steps++;
if(!(last_ts_was_hit || ts_2 || ts_3 || ts_4 || ts_5) && hit_flag){// increase hit counter only for new collisions
collision_started = 1;
hit_counter++;
//printf("hit_counter=%d msr = %d msl = %d \n",hit_counter, msr, msl);
}
else if(collision_started && hit_flag){
collision_duration++; // otherwise measure duration of a collision
//printf("collision_duration=%d msr = %d msl = %d \n", collision_duration, msr, msl);
}
if(!hit_flag && collision_started){ // at the end of a collision, we calculate the running average
if(collision_duration>4){
collision_started = 0;
printf("end of collision, duration=%d msr = %d msl = %d \n", collision_duration, msr, msl);
collision_duration = collision_duration + 4;
average_collision_duration= (average_collision_duration*(hit_counter-1)+collision_duration)/hit_counter;
f=fopen("/tmp/collision_duration_webots.txt","a");
fprintf(f,"%d \n",collision_duration);
fclose(f);}
else{
//printf("collision too short = %d, thus removed \n", collision_duration);
collision_started = 0;
hit_counter--;
}
}
if(fmod(time_steps,20*60*1) == 0){ // every 1 minutes
printf("Hits: %d Time [time steps]): %d p: %f Avg. Ta [time steps]: %0.2f v (m/s):%0.3f\n",
hit_counter, time_steps,
(double) hit_counter/(time_steps-hit_counter*ROUND(average_collision_duration)),
average_collision_duration,
speed);
f=fopen("/tmp/collision_probability.txt","a");
fprintf(f,"%f\n", (double) hit_counter/(time_steps-hit_counter*ROUND(average_collision_duration)));
fclose(f);
}
}
}
////////////////////////////////////////////
// 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;
}
// Main function
int main(int argc, char **argv)
{
// Initialize webots
wb_robot_init();
// GPS tick data
int red_line_tick = 0;
int green_circle_tick = 0;
// Get robot devices
WbDeviceTag left_wheel = wb_robot_get_device("left_wheel");
WbDeviceTag right_wheel = wb_robot_get_device("right_wheel");
// Get robot sensors
WbDeviceTag forward_left_sensor = wb_robot_get_device("so3");
wb_distance_sensor_enable(forward_left_sensor, TIME_STEP);
WbDeviceTag forward_right_sensor = wb_robot_get_device("so4");
wb_distance_sensor_enable(forward_right_sensor, TIME_STEP);
WbDeviceTag left_sensor = wb_robot_get_device("so1");
wb_distance_sensor_enable(left_sensor, TIME_STEP);
// Get the compass
WbDeviceTag compass = wb_robot_get_device("compass");
wb_compass_enable(compass, TIME_STEP);
// Get the GPS data
WbDeviceTag gps = wb_robot_get_device("gps");
wb_gps_enable(gps, TIME_STEP);
// Prepare robot for velocity commands
wb_motor_set_position(left_wheel, INFINITY);
wb_motor_set_position(right_wheel, INFINITY);
wb_motor_set_velocity(left_wheel, 0.0);
wb_motor_set_velocity(right_wheel, 0.0);
// Begin in mode 0, moving forward
int mode = 0;
// Main loop
while (wb_robot_step(TIME_STEP) != -1) {
// Get the sensor data
double forward_left_value = wb_distance_sensor_get_value(forward_left_sensor);
double forward_right_value = wb_distance_sensor_get_value(forward_right_sensor);
double left_value = wb_distance_sensor_get_value(left_sensor);
// Read compass and convert to angle
const double *compass_val = wb_compass_get_values(compass);
double compass_angle = convert_bearing_to_degrees(compass_val);
// Read in the GPS data
const double *gps_val = wb_gps_get_values(gps);
// Debug
printf("Sensor input values:\n");
printf("- Forward left: %.2f.\n",forward_left_value);
printf("- Forward right: %.2f.\n",forward_right_value);
printf("- Right: %.2f.\n",left_value);
printf("- Compass angle (degrees): %.2f.\n", compass_angle);
printf("- GPS values (x,z): %.2f, %.2f.\n", gps_val[0], gps_val[2]);
// Send acuator commands
double left_speed, right_speed;
left_speed = 0;
right_speed = 0;
// List the current modes
printf("Mode: %d.\n", mode);
printf("Action: ");
/*
* There are four modes for this controller.
* They are listed as below:
* 0: Finding initial correct angle
* 1: Moving forward
* 2: Wall following
* 3: Rotating after correct point
* 4: Finding green circle + moving on
*/
// If it reaches GPS coords past the red line
if (gps_val[2] > 8.0) {
// Up the GPS tick
red_line_tick = red_line_tick + 1;
}
// If the red line tick tolerance reaches
// more than 10 per cycle, begin mode 3
if (red_line_tick > 10) {
mode = 3;
}
if (mode == 0) { // Mode 0: Find correct angle
printf("Finding correct angle\n");
if (compass_angle < (DESIRED_ANGLE - 1.0)) {
// Turn right
left_speed = MAX_SPEED;
right_speed = 0;
} else if (compass_angle > (DESIRED_ANGLE + 1.0)) {
// Turn left
left_speed = 0;
right_speed = MAX_SPEED;
} else {
// Reached the desired angle, move in a straight line
mode = 1;
}
} else if(mode == 1) { // Mode 1: Move forward
printf("Moving forward.\n");
left_speed = MAX_SPEED;
right_speed = MAX_SPEED;
// When sufficiently close to a wall in front of robot, switch mode to wall following
if ((forward_right_value > 500) || (forward_left_value > 500)) {
mode = 2;
}
}
else if (mode == 2) { // Mode 2: Wall following
if ((forward_right_value > 200) || (forward_left_value > 200)) {
printf("Backing up and turning right.\n");
left_speed = MAX_SPEED / 4.0;
right_speed = - MAX_SPEED / 2.0;
}
else {
if (left_value > 300) {
printf("Turning right away from wall.\n");
left_speed = MAX_SPEED;
right_speed = MAX_SPEED / 1.75;
}
else {
if (left_value < 200) {
printf("Turning left towards wall.\n");
left_speed = MAX_SPEED / 1.75;
right_speed = MAX_SPEED;
}
else {
printf("Moving forward along wall.\n");
left_speed = MAX_SPEED;
right_speed = MAX_SPEED;
}
}
}
} else if (mode == 3) {
// Once arrived, turn to the right
printf("Finding correct angle (again)\n");
if (compass_angle < (90 - 1.0)) {
// Turn right
left_speed = MAX_SPEED;
right_speed = MAX_SPEED / 1.75;
} else if (compass_angle > (90 + 1.0)) {
// Turn left
left_speed = MAX_SPEED / 1.75;
right_speed = MAX_SPEED;
} else {
// Reached the desired angle, move in a straight line
if (green_circle_tick > 10) {
left_speed = 0;
right_speed = 0;
} else {
left_speed = MAX_SPEED;
right_speed = MAX_SPEED;
}
if (gps_val[0] < -3.2) {
green_circle_tick = green_circle_tick + 1;
}
}
}
// Set the motor speeds.
wb_motor_set_velocity(left_wheel, left_speed);
wb_motor_set_velocity(right_wheel, right_speed);
// Perform simple simulation step
} while (wb_robot_step(TIME_STEP) != -1);
// Clean up webots
wb_robot_cleanup();
return 0;
}