/* * 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) } }
// 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; }