void find_new_spot(double distance_value[8], int DIST_THRESHOLD)
{
if(twice == 2) // Reverse, Turn, Forward, Turn(opposite), Forward.
{
has_recovered = TRUE;
green_LED_state = OFF;
align_counter = 0;
}
else if(reverse_counter != REVERSE_LIMIT) // Make space by moving away from the box
{
reverse_counter = reverse_counter +1;
left_wheel_speed = -800;
right_wheel_speed = -800;
}
else if(turn_counter != TURN_LIMIT) // Line up with one of the sides of the box
{
turn_counter = turn_counter +1;
forward_counter = 0;
if(turn_left == NEUTRAL)
{
// Roll a dice, left or right?
double ran = rand()/((double)(RAND_MAX)+1);
if (ran > 0.5)
turn_left = FALSE;
else
turn_left = TRUE;
}
if(turn_left) // Turn left
{
left_wheel_speed = -300;
right_wheel_speed = 700;
}
else // Turn right
{
left_wheel_speed = 700;
right_wheel_speed = -300;
}
}
else if(forward_counter != FORWARD_LIMIT)
{
forward_counter = forward_counter +1;
if(forward_counter == FORWARD_LIMIT-1)
{
twice = twice +1;
turn_counter = 0;
if(turn_left)
turn_left = FALSE;
else
turn_left = TRUE;
}
update_search_speed(distance_value, DIST_THRESHOLD);
left_wheel_speed = get_search_left_wheel_speed();
right_wheel_speed = get_search_right_wheel_speed();
if((left_wheel_speed > 0) && (right_wheel_speed> 0) )
{
right_wheel_speed = 1000;
left_wheel_speed = 1000;
}
}
}
// entry point of the controller
int main(int argc, char **argv)
{
// initialize the Webots API
wb_robot_init();
// internal variables
int i;
WbDeviceTag ps[8];
char ps_names[8][4] = {
"ps0", "ps1", "ps2", "ps3",
"ps4", "ps5", "ps6", "ps7"
};
// initialize devices
for (i=0; i<8 ; i++) {
ps[i] = wb_robot_get_device(ps_names[i]);
wb_distance_sensor_enable(ps[i], TIME_STEP);
}
WbDeviceTag ls[8];
char ls_names[8][4] = {
"ls0", "ls1", "ls2", "ls3",
"ls4", "ls5", "ls6", "ls7"
};
// initialize devices
for (i=0; i<8 ; i++) {
ls[i] = wb_robot_get_device(ls_names[i]);
wb_light_sensor_enable(ls[i], TIME_STEP);
}
WbDeviceTag led[8];
char led_names[8][5] = {
"led0", "led1", "led2", "led3",
"led4", "led5", "led6", "led7"
};
// initialize devices
for (i=0; i<8 ; i++) {
led[i] = wb_robot_get_device(led_names[i]);
}
// feedback loop
while (1) {
// step simulation
int delay = wb_robot_step(TIME_STEP);
if (delay == -1) // exit event from webots
break;
// read sensors outputs
double ps_values[8];
for (i=0; i<8 ; i++)
ps_values[i] = wb_distance_sensor_get_value(ps[i]);
update_search_speed(ps_values, 250);
// set speeds
double left_speed = get_search_left_wheel_speed();
double right_speed = get_search_right_wheel_speed();
// read IR sensors outputs
double ls_values[8];
for (i=0; i<8 ; i++){
ls_values[i] = wb_light_sensor_get_value(ls[i]);
}
int active_ir = FALSE;
for(i=0; i<8; i++){
if(ls_values[i] < 2275){
active_ir = TRUE;
}
}
if(active_ir == TRUE){
swarm_retrieval(ls_values, ps_values, 2275);
left_speed = get_retrieval_left_wheel_speed();
right_speed = get_retrieval_right_wheel_speed();
}
if(is_pushing() == TRUE || stagnation == TRUE){
// check for stagnation
stagnation_counter = stagnation_counter + 1;
if(stagnation_counter == min((50 + positive_feedback * 50), 300) && stagnation == FALSE){
stagnation_counter = 0; // reset counter
stagnation_check = TRUE;
for(i=0; i<8; i++)
prev_dist_values[i] = ps_values[i];
}
if(stagnation_check == TRUE){
left_speed = 0;
right_speed = 0;
}
if(stagnation_check == TRUE && stagnation_counter == 5){
stagnation_counter = 0; // reset counter
reset_stagnation();
valuate_pushing(ps_values, prev_dist_values);
stagnation = get_stagnation_state();
stagnation_check = FALSE;
if(stagnation == TRUE)
positive_feedback = 0;
else
positive_feedback = positive_feedback + 1;
}
if(stagnation == TRUE){
stagnation_recovery(ps_values, 300);
left_speed = get_stagnation_left_wheel_speed();
right_speed = get_stagnation_right_wheel_speed();
if(get_stagnation_state() == FALSE){
reset_stagnation();
stagnation = FALSE;
stagnation_counter = 0;
}
}
}
// write actuators inputs
wb_differential_wheels_set_speed(left_speed, right_speed);
for(i=0; i<8; i++){
wb_led_set(led[i], get_LED_state(i));
}
}
// cleanup the Webots API
wb_robot_cleanup();
return 0; //EXIT_SUCCESS
}
