// controller main loop static int run(int ms) { static double ds_value[NB_SENSORS]; int i; for (i = 0; i < NB_SENSORS; i++) // read sensor values ds_value[i] = distance_sensor_get_value(ps[i]); // range: 0 (far) to 3800 (close) // choose behavior double left_speed, right_speed; int duration; double brait[NB_SENSORS][2] = { {-0.5,0.5}, //Front right {-0.5, 0.5}, //F right {-0.4,0.0}, //Right {1,1}, // Behind right {1,1}, // Behind left {0.0,-0.4}, // Left {0.5,-0.5}, // F left {0.5,-0.5} //Front left }; left_speed = 0.0; right_speed = 0.0; // Matrice multiplication for (i=0; i<NB_SENSORS;i++){ left_speed += ds_value[i]*brait[i][0]; right_speed += ds_value[i]*brait[i][1]; } duration = TIME_STEP; // update fitness fitness_step(left_speed, right_speed, ds_value); // actuate wheel motors differential_wheels_set_speed(left_speed, right_speed); // compute and display fitness every 1000 controller steps if (step_count % 1000 == 999) { double fitness = fitness_compute(); printf("fitness = %f\n", fitness); fitness_reset(); } return duration; }
// controller main loop static int run(int ms) { static double ds_value[NB_SENSORS]; int i; for (i = 0; i < NB_SENSORS; i++) // read sensor values // reads the handle in ps[i] and saves its value in ds_value[i] ds_value[i] = wb_distance_sensor_get_value(ps[i]); // range: 0 (far) to 4095 (0 distance (in theory)) // choose behavior double left_speed, right_speed; int duration; //printf("ds_value[0] = %f\n", ds_value[0]); int front_threshold = 1024; int side_threshold = 2048; if (ds_value[0] > front_threshold ) { // we detected an obstacle on the front right left_speed = -400; right_speed = +400; // turn around duration = 10 * TIME_STEP; // for 1280 milliseconds } else if ( ds_value[1] > front_threshold ) { left_speed = 200; right_speed = 400; duration = 10 * TIME_STEP; } else if ( ds_value[6] > front_threshold ) { // we detected an obstacle on the front right right_speed = -400; left_speed = +400; //turn around the other side duration = 10 * TIME_STEP; // for 1280 millisecondss } else if ( ds_value[7] > front_threshold) { right_speed = -400; left_speed = +400; //turn around the other side duration = 10 * TIME_STEP; // for 1280 millisecondss } else if ( ds_value[2] > side_threshold) { // obstacle from the right // move away left a bit left_speed = 100; right_speed = 200; duration = 5 * TIME_STEP; } else if ( ds_value[5] > side_threshold) { // obstacle from the left // move away right a bit left_speed = 200; right_speed = 100; duration = 5 * TIME_STEP; } else if ( ds_value[3] > side_threshold || ds_value[4] > side_threshold) { // detected on the back // run away from the obstacle left_speed = right_speed = 500; // go straight duration = TIME_STEP; // for 64 milliseconds } else { left_speed = right_speed = 500; // go straight duration = TIME_STEP; // for 64 milliseconds } // update fitness fitness_step(left_speed, right_speed, ds_value); // actuate wheel motors // sets the e-pucks wheel speeds to left_speed (left wheel) and right_speed (right wheel) // max speed is 1000 == 2 turns per second wb_differential_wheels_set_speed(left_speed, right_speed); // compute and display fitness every 1000 controller steps if (step_count % 1000 == 999) { double fitness = fitness_compute(); printf("fitness = %f\n", fitness); fitness_reset(); } return duration; }