//Returns the minimum value from all sampled sensors.
double getMinimum(int size, WbDeviceTag* tags)
{
double min = 1000000;
int i;
for(i=0; i < size; i++)
{
WbDeviceTag current = tags[i];
double currentValue = wb_light_sensor_get_value(current);
if(currentValue < min)
min = currentValue;
}
return min;
}
int main(int argc, char *argv[]) {
/* define variables */
WbDeviceTag turret_sensor;
double perf_data[3];
double clock = 0;
/* initialize Webots */
wb_robot_init();
// read robot id from the robot's name
char* robot_name;
robot_name=(char*) wb_robot_get_name();
sscanf(robot_name,"e-puck%d",&robot_id);
emitter = wb_robot_get_device("emitter");
wb_emitter_set_range(emitter,COM_RANGE);
receiver = wb_robot_get_device("receiver");
wb_receiver_set_channel(receiver,0);
wb_receiver_enable(receiver,TIME_STEP);
wb_robot_step(TIME_STEP);
turret_sensor = wb_robot_get_device("ts");
wb_light_sensor_enable(turret_sensor,TIME_STEP);
/* main loop */
while (wb_robot_step(TIME_STEP) != -1) {
clock += (double)TIME_STEP/1000;
/* get light sensor value */
sensor_value = wb_light_sensor_get_value(turret_sensor);
if(wb_receiver_get_queue_length(receiver) > 0) {
wb_emitter_set_channel(emitter,3);
perf_data[0] = (double)robot_id;
perf_data[1] = (double)(pkt_count-1);
perf_data[2] = 0;
wb_emitter_send(emitter,perf_data,3*sizeof(double));
break; // stop node
}
else {
send_data(clock); // send measurement data
}
}
wb_robot_cleanup();
return 0;
}
// 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
}
