// controller initialization
static void reset(void) {
fitness_reset();
int i;
char name[] = "ps0";
for(i = 0; i < NB_SENSORS; i++) {
ps[i]=wb_robot_get_device(name); // get sensor handle
// perform distance measurements every TIME_STEP millisecond
wb_distance_sensor_enable(ps[i], TIME_STEP);
name[2]++; // increase the device name to "ps1", "ps2", etc.
}
}
// 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;
}
