Esempio n. 1
0
static void initialize()
{
     wb_robot_init();
     
     currentBehavior = RANDOM_MOVER;
     
     ps0 = wb_robot_get_device("ps0");
     ps1 = wb_robot_get_device("ps1");
     ps2 = wb_robot_get_device("ps2");
     ps3 = wb_robot_get_device("ps3");
     ps4 = wb_robot_get_device("ps4");
     ps5 = wb_robot_get_device("ps5");
     ps6 = wb_robot_get_device("ps6");
     ps7 = wb_robot_get_device("ps7");
     
     ls0 = wb_robot_get_device("ls0");
     ls1 = wb_robot_get_device("ls1");
     ls2 = wb_robot_get_device("ls2");
     ls3 = wb_robot_get_device("ls3");
     ls4 = wb_robot_get_device("ls4");
     ls5 = wb_robot_get_device("ls5");
     ls6 = wb_robot_get_device("ls6");
     ls7 = wb_robot_get_device("ls7");
     
     led4 = wb_robot_get_device("led4");
     
     int sensorTimeout = 100;
      
     wb_distance_sensor_enable(ps0, sensorTimeout);
     wb_distance_sensor_enable(ps1, sensorTimeout);
     wb_distance_sensor_enable(ps2, sensorTimeout);
     wb_distance_sensor_enable(ps3, sensorTimeout);
     wb_distance_sensor_enable(ps4, sensorTimeout);
     wb_distance_sensor_enable(ps5, sensorTimeout);
     wb_distance_sensor_enable(ps6, sensorTimeout);
     wb_distance_sensor_enable(ps7, sensorTimeout);
    
     wb_light_sensor_enable(ls0, sensorTimeout);
     wb_light_sensor_enable(ls1, sensorTimeout);
     wb_light_sensor_enable(ls2, sensorTimeout);
     wb_light_sensor_enable(ls3, sensorTimeout);
     wb_light_sensor_enable(ls4, sensorTimeout);
     wb_light_sensor_enable(ls5, sensorTimeout);
     wb_light_sensor_enable(ls6, sensorTimeout);
     wb_light_sensor_enable(ls7, sensorTimeout);

     wb_led_set(led4, 1); 
}
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;
}
Esempio n. 3
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
}