/*
 * Reset the robot's devices and get its ID
 */
static void reset() 
{
	wb_robot_init();

	receiver = wb_robot_get_device("receiver");
	emitter  = wb_robot_get_device("emitter");
	compass2  = wb_robot_get_device("compass2");
	receiver0 = wb_robot_get_device("receiver0");
   	emitter0 = wb_robot_get_device("emitter0");

	int i;
	char s[4]="ps0";
	for(i=0; i<NB_SENSORS;i++) {
		ds[i]=wb_robot_get_device(s);  // the device name is specified in the world file
		s[2]++;				           // increases the device number
	}
	robot_name=(char*) wb_robot_get_name(); 

   	for(i=0; i<FLOCK_SIZE; i++) {
    	relative_pos[i][0] = my_position[0] = 0;
    	relative_pos[i][1] = my_position[1] = 0;
    	relative_pos[i][2] = my_position[2] = 0;
	}

	for(i=0;i<NB_SENSORS;i++)
    	wb_distance_sensor_enable(ds[i],64);

	wb_receiver_enable(receiver,64);
	wb_receiver_enable(receiver0,64);
	wb_compass_enable(compass2, 64);
	
	//Reading the robot's name. Pay attention to name specification when adding robots to the simulation!
	sscanf(robot_name,"epuck%d",&robot_id_u); // read robot id from the robot's name
	robot_id = robot_id_u%FLOCK_SIZE;	  // normalize between 0 and FLOCK_SIZE-1
  
	for(i=0; i<FLOCK_SIZE; i++) {
		initialized[i] = 0;		  // Set initialization to 0 (= not yet initialized)
	}
}
Пример #2
0
// Main function
int main(int argc, char **argv)
{
  // Initialize webots
  wb_robot_init();
  
  // GPS tick data
  int red_line_tick = 0;
  int green_circle_tick = 0;
  
  // Get robot devices
  WbDeviceTag left_wheel  = wb_robot_get_device("left_wheel");
  WbDeviceTag right_wheel = wb_robot_get_device("right_wheel");
  
  // Get robot sensors
  WbDeviceTag forward_left_sensor = wb_robot_get_device("so3");
  wb_distance_sensor_enable(forward_left_sensor, TIME_STEP);
  WbDeviceTag forward_right_sensor = wb_robot_get_device("so4");
  wb_distance_sensor_enable(forward_right_sensor, TIME_STEP);
  WbDeviceTag left_sensor = wb_robot_get_device("so1");
  wb_distance_sensor_enable(left_sensor, TIME_STEP);
  
  // Get the compass
  WbDeviceTag compass = wb_robot_get_device("compass");
  wb_compass_enable(compass, TIME_STEP);
  
  // Get the GPS data
  WbDeviceTag gps = wb_robot_get_device("gps");
  wb_gps_enable(gps, TIME_STEP);
  
  // Prepare robot for velocity commands
  wb_motor_set_position(left_wheel, INFINITY);
  wb_motor_set_position(right_wheel, INFINITY);
  wb_motor_set_velocity(left_wheel, 0.0);
  wb_motor_set_velocity(right_wheel, 0.0);
  
  
  // Begin in mode 0, moving forward
  int mode  = 0;
  
  // Main loop
  while (wb_robot_step(TIME_STEP) != -1) {
  
    // Get the sensor data
    double forward_left_value = wb_distance_sensor_get_value(forward_left_sensor);
    double forward_right_value = wb_distance_sensor_get_value(forward_right_sensor);
    double left_value = wb_distance_sensor_get_value(left_sensor);
      
    // Read compass and convert to angle
    const double *compass_val = wb_compass_get_values(compass);
    double compass_angle = convert_bearing_to_degrees(compass_val);
    
    // Read in the GPS data
    const double *gps_val = wb_gps_get_values(gps);
    
    // Debug
    printf("Sensor input values:\n");
    printf("- Forward left: %.2f.\n",forward_left_value);
    printf("- Forward right: %.2f.\n",forward_right_value);
    printf("- Right: %.2f.\n",left_value);
    printf("- Compass angle (degrees): %.2f.\n", compass_angle);
    printf("- GPS values (x,z): %.2f, %.2f.\n", gps_val[0], gps_val[2]);
    
    // Send acuator commands
    double left_speed, right_speed;
    left_speed = 0;
    right_speed = 0;
    
    // List the current modes
    printf("Mode: %d.\n", mode);
    printf("Action: ");
    
    /*
     * There are four modes for this controller.
     * They are listed as below:
     * 0: Finding initial correct angle
     * 1: Moving forward
     * 2: Wall following
     * 3: Rotating after correct point
     * 4: Finding green circle + moving on
     */
     
    // If it reaches GPS coords past the red line
    if (gps_val[2] > 8.0) {
      
      // Up the GPS tick
      red_line_tick = red_line_tick + 1;
      
    }
    
    // If the red line tick tolerance reaches
    // more than 10 per cycle, begin mode 3
    if (red_line_tick > 10) {
      mode = 3;
    }
    
    if (mode == 0) { // Mode 0: Find correct angle
    
      printf("Finding correct angle\n");
      
      if (compass_angle < (DESIRED_ANGLE - 1.0)) {
      
        // Turn right
        left_speed = MAX_SPEED;
        right_speed = 0;
      
      } else if (compass_angle > (DESIRED_ANGLE + 1.0)) {
        
        // Turn left
        left_speed = 0;
        right_speed = MAX_SPEED;
      
      } else {
      
        // Reached the desired angle, move in a straight line
        mode = 1;
      
      }
    
    } else if(mode == 1) { // Mode 1: Move forward
    
      printf("Moving forward.\n");
    
      left_speed = MAX_SPEED;
      right_speed = MAX_SPEED;
            
      // When sufficiently close to a wall in front of robot, switch mode to wall following
      if ((forward_right_value > 500) || (forward_left_value > 500)) {
        mode = 2;
      }
      
    } 
    else if (mode == 2) { // Mode 2: Wall following
        
      if ((forward_right_value > 200) || (forward_left_value > 200)) {
        printf("Backing up and turning right.\n"); 
        
        left_speed = MAX_SPEED / 4.0;
        right_speed = - MAX_SPEED / 2.0;
      }
      else {
      
        if (left_value > 300) {
          printf("Turning right away from wall.\n"); 
          
          left_speed = MAX_SPEED;
          right_speed = MAX_SPEED / 1.75;
        } 
        else {
        
          if (left_value < 200) {
            printf("Turning left towards wall.\n");
              
            left_speed = MAX_SPEED / 1.75;
            right_speed = MAX_SPEED;
          } 
          else {
            printf("Moving forward along wall.\n");
            
            left_speed = MAX_SPEED;
            right_speed = MAX_SPEED;
          } 
          
        }
        
      }
      
    } else if (mode == 3) {
    
      // Once arrived, turn to the right
      printf("Finding correct angle (again)\n");
      
      if (compass_angle < (90 - 1.0)) {
      
        // Turn right
        left_speed = MAX_SPEED;
        right_speed = MAX_SPEED / 1.75;
      
      } else if (compass_angle > (90 + 1.0)) {
        
        // Turn left
        left_speed = MAX_SPEED / 1.75;
        right_speed = MAX_SPEED;
      
      } else {
      
        // Reached the desired angle, move in a straight line
        if (green_circle_tick > 10) {
          left_speed = 0;
          right_speed = 0;
          
          
        } else {
          left_speed = MAX_SPEED;
          right_speed = MAX_SPEED;
        }
        
        if (gps_val[0] < -3.2) {
          green_circle_tick = green_circle_tick + 1;
        }
      
      }
      
    }
     
    // Set the motor speeds.
    wb_motor_set_velocity(left_wheel, left_speed);
    wb_motor_set_velocity(right_wheel, right_speed);
    
  // Perform simple simulation step
  } while (wb_robot_step(TIME_STEP) != -1);

  
  // Clean up webots
  wb_robot_cleanup();
  
  return 0;
}