//Heads straight, adjusting speed based on //whether or not a collision is imminent. void headStraight() { currentState = FORWARD; int speed = collisionPending(); if(speed == 2) { wb_differential_wheels_set_speed(200, 200); wb_led_set(led4, 1); } else if(speed == 1) { wb_differential_wheels_set_speed(0, 0); wb_led_set(led4, 1); } else { wb_differential_wheels_set_speed(MAX_SPEED, MAX_SPEED); wb_led_set(led4, 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); }
static void reset(void) { int i; char text[5]="led0"; for(i=0;i<NB_LEDS;i++) { led[i]=wb_robot_get_device(text); text[3]++; wb_led_set(led[i],0); } // enable the camera cam = wb_robot_get_device("camera"); wb_camera_enable(cam,TIME_STEP_CAM); width = wb_camera_get_width(cam); height = wb_camera_get_height(cam); }
int main(int argc, char **argv) { wb_robot_init(); int i; WbDeviceTag servos[NSERVOS]; WbDeviceTag head_led; for (i=0; i<NSERVOS; i++) servos[i] = wb_robot_get_device(servo_names[i]); head_led = wb_robot_get_device("HeadLed"); wb_led_set(head_led, 0x40C040); do { double t = wb_robot_get_time(); for (i=0; i<6; i++) wb_servo_set_position(servos[i], amplitudes[i]*sin(frequency*t) + offsets[i]); } while (wb_robot_step(TIME_STEP) != -1); wb_robot_cleanup(); return 0; }
//Turns right by setting speed void turnRight() { wb_led_set(led4, 0); currentState = TURNING; wb_differential_wheels_set_speed(-MAX_SPEED, MAX_SPEED); }
// 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 }