// controller initialization static void reset(void) { fitness_reset(); int i; char name[] = "ps0"; for(i = 0; i < NB_SENSORS; i++) { ps[i]=wb_robot_get_device(name); // get sensor handle // perform distance measurements every TIME_STEP millisecond wb_distance_sensor_enable(ps[i], TIME_STEP); name[2]++; // increase the device name to "ps1", "ps2", etc. } }
// // Reset the robot controller and initiate the sensors and emitter/receiver // static int reset() { int i; mode =1; emitter = wb_robot_get_device("emitter"); //buffer = (double *) emitter_get_buffer(emitter); receiver = wb_robot_get_device("receiver"); wb_receiver_enable(receiver, TIME_STEP); char text[5]="led0"; for(i=0;i<NB_LEDS;i++) { led[i]=wb_robot_get_device(text); // get a handler to the sensor text[3]++; // increase the device name to "ps1", "ps2", etc. } text[0]='p'; text[1]='s'; text[3]='\0'; text[2]='0'; ps[0] = wb_robot_get_device(text); // proximity sensors text[2]='7'; ps[1] = wb_robot_get_device(text); // proximity sensors text[2]='1'; ps[2] = wb_robot_get_device(text); // proximity sensors text[2]='6'; ps[3] = wb_robot_get_device(text); // proximity sensors text[2]='2'; ps[4] = wb_robot_get_device(text); // proximity sensors text[2]='5'; ps[5] = wb_robot_get_device(text); // proximity sensors text[2]='3'; ps[6] = wb_robot_get_device(text); // proximity sensors text[2]='4'; ps[7] = wb_robot_get_device(text); // proximity sensors // Enable proximity and floor sensors for(i=0;i<NB_DIST_SENS;i++) { wb_distance_sensor_enable(ps[i],TIME_STEP); printf("ps[%d] is active\n",i); } // Enable GPS sensor to determine position gps=wb_robot_get_device("gps"); wb_gps_enable(gps, TIME_STEP); printf("gps is active\n"); return 1; }
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); }
/* * Reset the robot's devices and get its ID */ static void reset() { int i; wb_robot_init(); 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 } for(i=0; i<FLOCK_SIZE; i++) { timestamp[i] = 0; relative_pos[i][0] = 0; relative_pos[i][1] = 0; relative_pos[i][2] = 0; } maxTimestamp = 1; robot_name=(char*) wb_robot_get_name(); for(i=0;i<NB_SENSORS;i++) wb_distance_sensor_enable(ds[i],64); my_position[0] = 0; my_position[1] = 0; my_position[2] = 0; //printf("reset: my pos: %f, %f\n", my_position[0], my_position[1]); wb_differential_wheels_enable_encoders(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 sprintf(robot_number, "%d", robot_id_u); receiver = wb_robot_get_device("receiver"); receiver0 = wb_robot_get_device("receiver0"); emitter = wb_robot_get_device("emitter"); emitter0 = wb_robot_get_device("emitter0"); wb_receiver_enable(receiver,32); wb_receiver_enable(receiver0,32); for(i=0; i<FLOCK_SIZE; i++) { initialized[i] = 0; // Set initialization to 0 (= not yet initialized) } }
void reset() { int i; robotName = wb_robot_get_name(); currentState = FORWARD; // Make sure to initialize the RNG with different values for each thread srand(time(NULL) + (int)&robotName); char e_puck_name[] = "ps0"; char sensorsName[5]; // Emitter and receiver device tags emitterTag = wb_robot_get_device("emitter"); receiverTag = wb_robot_get_device("receiver"); // Configure communication devices wb_receiver_enable(receiverTag, TIME_STEP); wb_emitter_set_range(emitterTag, COMM_RADIUS); wb_emitter_set_channel(emitterTag, COMMUNICATION_CHANNEL); wb_receiver_set_channel(receiverTag, COMMUNICATION_CHANNEL); sprintf(sensorsName, "%s", e_puck_name); for (i = 0; i < NB_SENSORS; i++) { sensors[i] = wb_robot_get_device(sensorsName); wb_distance_sensor_enable(sensors[i], TIME_STEP); if ((i + 1) >= 10) { sensorsName[2] = '1'; sensorsName[3]++; if ((i + 1) == 10) { sensorsName[3] = '0'; sensorsName[4] = (char) '\0'; } } else { sensorsName[2]++; } } wb_differential_wheels_enable_encoders(TIME_STEP); printf("Robot %s is reset\n", robotName); return; }
/* * 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) } }
int main(){ reset(); int msl,msr; // motor speed left and right double distances[NB_SENSORS]; // array keeping the distance sensor readings int elapsed_time; // elapsed time during one time_step int obstacle_near; // flag which becomes true if an obstacle is nearby int hit_flag; // flag which becomes true as soon as the robot hits an obstacle int last_ts_was_hit = 0; // flag memorizing whether the robot was already in a collision during the last // time step int time_steps; // counter for simulated time steps int hit_counter; // counter for time steps involving collisions int collision_duration; // duration of a single collision (in time steps unit) double average_collision_duration;// average duration of a single collision const double* gpsvalues; // matrix containing gps readings double actx,acty; // current position double oldx,oldy; // previous position double speed; // average robot speed in free space int collision_started = 0; // iteration variables int i; int sensor_nb; FILE* f; // file handle char fname[]="/tmp/collision_pos.txt"; // file name to store the positions of the collisions if(fmod(TIME_STEP,50)!=0){ printf("ERROR: The TIME_STEP needs to be a multiple of 50ms\n"); return(0); } for(i=0;i<NB_SENSORS;i++) { wb_distance_sensor_enable(ps[i],50); // this function wb_*_enable() allows to specify an update interval for the sensor readings in milliseconds } wb_gps_enable(gps,50); wb_robot_step(50); gpsvalues = wb_gps_get_values(gps); // returns a pointer to three double values. The pointer is the address of an array allocated by the function internally. wb_robot_step(50); oldx=gpsvalues[0]; oldy=gpsvalues[2]; time_steps=0; hit_counter=0; hit_flag=0; average_collision_duration=0; speed=0; int ts_2=0, ts_3=0, ts_4=0, ts_5=0; for(;;){ elapsed_time=0; // reset elapsed time and hit flag ts_5 = ts_4; ts_4 = ts_3; ts_3 = ts_2; ts_2 = last_ts_was_hit; last_ts_was_hit=hit_flag; // copying the status of the last time step if(!last_ts_was_hit) { collision_duration=1; // if not actually colliding, always reset counter } hit_flag=0; // every time step while(elapsed_time<TIME_STEP){ msl=0; msr=0; for(sensor_nb=0;sensor_nb<NB_SENSORS;sensor_nb++){ // read sensor values and calculate motor speeds distances[sensor_nb] = wb_distance_sensor_get_value(ps[sensor_nb]); //printf("t=%d, distance[%d] = %f\n",time_steps,sensor_nb,distances[sensor_nb]); msr += distances[sensor_nb] * Interconn[sensor_nb]; msl += distances[sensor_nb] * Interconn[sensor_nb + NB_SENSORS]; } msl /= 350; msr /= 350; // Normalizing speeds actx=gpsvalues[0]; acty=gpsvalues[2]; //printf("x = %f, y = %f\n",actx, acty); obstacle_near=(abs(msl)>10 || abs(msr)>10); if(obstacle_near){ // if motor speed changes (if obstacle is hit) //printf("obstacle_near=%d msr = %d msl = %d \n", obstacle_near, msr, msl); hit_flag=1; f=fopen(fname,"a"); fprintf(f,"%f %f\n", actx, acty); fclose(f); } else { // measure speed only when no obstacles near double dist = sqrt((actx-oldx)*(actx-oldx)+(acty-oldy)*(acty-oldy)); // distance traveled in 50ms if(dist<0.01) // don't consider fast speed due to wrap around speed=speed*0.999+0.001*dist*1000.0/50.0; } oldx=actx; oldy=acty; msl += (BIAS_SPEED); msr += (BIAS_SPEED); wb_differential_wheels_set_speed(3*msl,3*msr); gpsvalues = wb_gps_get_values(gps); wb_robot_step(50); // Executing the simulation for 50ms elapsed_time+=50; } // end time step time_steps++; if(!(last_ts_was_hit || ts_2 || ts_3 || ts_4 || ts_5) && hit_flag){// increase hit counter only for new collisions collision_started = 1; hit_counter++; //printf("hit_counter=%d msr = %d msl = %d \n",hit_counter, msr, msl); } else if(collision_started && hit_flag){ collision_duration++; // otherwise measure duration of a collision //printf("collision_duration=%d msr = %d msl = %d \n", collision_duration, msr, msl); } if(!hit_flag && collision_started){ // at the end of a collision, we calculate the running average if(collision_duration>4){ collision_started = 0; printf("end of collision, duration=%d msr = %d msl = %d \n", collision_duration, msr, msl); collision_duration = collision_duration + 4; average_collision_duration= (average_collision_duration*(hit_counter-1)+collision_duration)/hit_counter; f=fopen("/tmp/collision_duration_webots.txt","a"); fprintf(f,"%d \n",collision_duration); fclose(f);} else{ //printf("collision too short = %d, thus removed \n", collision_duration); collision_started = 0; hit_counter--; } } if(fmod(time_steps,20*60*1) == 0){ // every 1 minutes printf("Hits: %d Time [time steps]): %d p: %f Avg. Ta [time steps]: %0.2f v (m/s):%0.3f\n", hit_counter, time_steps, (double) hit_counter/(time_steps-hit_counter*ROUND(average_collision_duration)), average_collision_duration, speed); f=fopen("/tmp/collision_probability.txt","a"); fprintf(f,"%f\n", (double) hit_counter/(time_steps-hit_counter*ROUND(average_collision_duration))); fclose(f); } } }
int main(int argc, const char *argv[]) { WbDeviceTag speaker, microphone; WbDeviceTag sensors[NUM_SENSORS]; int role = UNKNOWN; int prev_audible = -1; int i, j; // for loops // initialize webots wb_robot_init(); // determine role if (argc >= 2) { if (! strcmp(argv[1], "speaker")) role = SPEAKER; else if (! strcmp(argv[1], "microphone")) role = MICROPHONE; } // use speaker or microphone according to specified role if (role == SPEAKER) speaker = wb_robot_get_device("speaker"); else if (role == MICROPHONE) { // we only use "mic0" in this demo microphone = wb_robot_get_device("mic0"); wb_microphone_enable(microphone, TIME_STEP / 2); } // find distance sensors for (i = 0; i < NUM_SENSORS; i++) { char sensor_name[64]; sprintf(sensor_name, "ps%d", i); sensors[i] = wb_robot_get_device(sensor_name); wb_distance_sensor_enable(sensors[i], TIME_STEP); } // main loop for (;;) { if (role == SPEAKER) { wb_speaker_emit_sample(speaker, SAMPLE, sizeof(SAMPLE)); } else if (role == MICROPHONE) { const short *rec_buffer = (const short *)wb_microphone_get_sample_data(microphone); int numSamples = wb_microphone_get_sample_size(microphone) / sizeof(SAMPLE[0]); if (rec_buffer) { int audible = 0; for (i = 0; i < numSamples; i++) { // warning this demo assumes no noise and therefore depends on the sound plugin configuration ! if (rec_buffer[i] != 0) audible = 1; } if (audible != prev_audible) { printf(audible ? "I hear you now !\n" : "I can't hear you !\n"); prev_audible = audible; } } else printf("received: nothing this time ...\n"); } // read distance sensor values double sensor_values[NUM_SENSORS]; for (i = 0; i < NUM_SENSORS; i++) sensor_values[i] = wb_distance_sensor_get_value(sensors[i]); // compute braitenberg collision avoidance double speed[2]; for (i = 0; i < 2; i++) { speed[i] = 0.0; for (j = 0; j < NUM_SENSORS; j++) speed[i] += EPUCK_MATRIX[j][i] * (1 - (sensor_values[j] / RANGE)); } // set the motors speed wb_differential_wheels_set_speed(speed[0], speed[1]); // simulation step wb_robot_step(TIME_STEP); } return 0; }
// // Reset the robot controller and initiate the sensors and emitter/receiver // static int reset() { int i; mode =1; emitter = wb_robot_get_device("emitter"); //no emitter_enable here on purpose! receiver = wb_robot_get_device("receiver"); wb_receiver_enable(receiver, TIME_STEP); char text[5]="led0"; for(i=0;i<NB_LEDS;i++) { led[i]=wb_robot_get_device(text); // get a handler to the sensor text[3]++; // increase the device name to "ps1", "ps2", etc. } text[0]='p'; text[1]='s'; text[3]='\0'; text[2]='0'; ps[0] = wb_robot_get_device(text); // proximity sensors text[2]='7'; ps[1] = wb_robot_get_device(text); // proximity sensors text[2]='6'; ps[2] = wb_robot_get_device(text); // proximity sensors text[2]='5'; ps[3] = wb_robot_get_device(text); text[2]='4'; ps[4] = wb_robot_get_device(text); text[2]='3'; ps[5] = wb_robot_get_device(text); text[2]='2'; ps[6] = wb_robot_get_device(text); text[2]='1'; ps[7] = wb_robot_get_device(text); // Enable proximity and floor sensors for(i=0;i<NB_DIST_SENS;i++) { wb_distance_sensor_enable(ps[i],TIME_STEP); //printf("ps[%d] is active\n",i); } //get handles for all the floor sensors text[0]='f'; text[2]='1'; fs[0]=wb_robot_get_device(text); //note that fs[0] is a 1x1 array that holds the center floor sensor DeviceTag //enable the center fs just to try wb_distance_sensor_enable(fs[0],TIME_STEP); //enable center floor sensor //printf("fs[%d] is active\n",1); //if this sensor is active increase the fitness // open files for writing //pF1=fopen("act_in.txt","w+"); pF2=fopen("decoded_pop.txt","w+"); // pF3=fopen("nn_values.txt","w+"); return 1; }
//////////////////////////////////////////// // Main int main() { int i, speed[2], ps_offset[NB_DIST_SENS]={0,0,0,0,0,0,0,0}, Mode=1; /* intialize Webots */ wb_robot_init(); /* initialization */ char name[20]; for (i = 0; i < NB_LEDS; i++) { sprintf(name, "led%d", i); led[i] = wb_robot_get_device(name); /* get a handler to the sensor */ } for (i = 0; i < NB_DIST_SENS; i++) { sprintf(name, "ps%d", i); ps[i] = wb_robot_get_device(name); /* proximity sensors */ wb_distance_sensor_enable(ps[i],TIME_STEP); } for (i = 0; i < NB_GROUND_SENS; i++) { sprintf(name, "gs%d", i); gs[i] = wb_robot_get_device(name); /* ground sensors */ wb_distance_sensor_enable(gs[i],TIME_STEP); } for(;;) // Main loop { // Run one simulation step wb_robot_step(TIME_STEP); // Reset all BB variables when switching from simulation to real robot and back if (Mode!=wb_robot_get_mode()) { oam_reset = TRUE; llm_active = FALSE; llm_past_side = NO_SIDE; ofm_active = FALSE; lem_active = FALSE; lem_state = LEM_STATE_STANDBY; Mode = wb_robot_get_mode(); if (Mode == SIMULATION) { for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_SIMULATION[i]; wb_differential_wheels_set_speed(0,0); wb_robot_step(TIME_STEP); // Just run one step to make sure we get correct sensor values printf("\n\n\nSwitching to SIMULATION and reseting all BB variables.\n\n"); } else if (Mode == REALITY) { for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_REALITY[i]; wb_differential_wheels_set_speed(0,0); wb_robot_step(TIME_STEP); // Just run one step to make sure we get correct sensor values printf("\n\n\nSwitching to REALITY and reseting all BB variables.\n\n"); } } // read sensors value for(i=0;i<NB_DIST_SENS;i++) ps_value[i] = (((int)wb_distance_sensor_get_value(ps[i])-ps_offset[i])<0)?0:((int)wb_distance_sensor_get_value(ps[i])-ps_offset[i]); for(i=0;i<NB_GROUND_SENS;i++) gs_value[i] = wb_distance_sensor_get_value(gs[i]); // Reset all BB variables when switching from simulation to real robot and back if (Mode!=wb_robot_get_mode()) { oam_reset = TRUE; llm_active = FALSE; llm_past_side = NO_SIDE; ofm_active = FALSE; lem_active = FALSE; lem_state = LEM_STATE_STANDBY; Mode = wb_robot_get_mode(); if (Mode == SIMULATION) printf("\nSwitching to SIMULATION and reseting all BB variables.\n\n"); else if (Mode == REALITY) printf("\nSwitching to REALITY and reseting all BB variables.\n\n"); } // Speed initialization speed[LEFT] = 0; speed[RIGHT] = 0; // *** START OF SUBSUMPTION ARCHITECTURE *** // LFM - Line Following Module LineFollowingModule(); // Speed computation speed[LEFT] = lfm_speed[LEFT]; speed[RIGHT] = lfm_speed[RIGHT]; // *** END OF SUBSUMPTION ARCHITECTURE *** // Debug display printf("OAM %d side %d \n", oam_active, oam_side); // Set wheel speeds wb_differential_wheels_set_speed(speed[LEFT],speed[RIGHT]); } 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 }
// 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; }
static void find_and_enable_devices() { // camera CameraTop = wb_robot_get_device("CameraTop"); CameraBottom = wb_robot_get_device("CameraBottom"); wb_camera_enable(CameraTop, 4 * time_step); wb_camera_enable(CameraBottom, 4 * time_step); // accelerometer accelerometer = wb_robot_get_device("accelerometer"); wb_accelerometer_enable(accelerometer, time_step); // gyro gyro = wb_robot_get_device("gyro"); wb_gyro_enable(gyro, time_step); // inertial unit inertial_unit = wb_robot_get_device("inertial unit"); wb_inertial_unit_enable(inertial_unit, time_step); // ultrasound sensors us[0] = wb_robot_get_device("USSensor1"); us[1] = wb_robot_get_device("USSensor2"); us[2] = wb_robot_get_device("USSensor3"); us[3] = wb_robot_get_device("USSensor4"); int i; for (i = 0; i < 4; i++) wb_distance_sensor_enable(us[i], time_step); // foot sensors fsr3d[0] = wb_robot_get_device("LFsr"); wb_touch_sensor_enable(fsr3d[0], time_step); fsr3d[1] = wb_robot_get_device("RFsr"); wb_touch_sensor_enable(fsr3d[1], time_step); // foot bumpers lfoot_lbumper = wb_robot_get_device("BumperLFootLeft"); lfoot_rbumper = wb_robot_get_device("BumperLFootRight"); rfoot_lbumper = wb_robot_get_device("BumperRFootLeft"); rfoot_rbumper = wb_robot_get_device("BumperRFootRight"); wb_touch_sensor_enable(lfoot_lbumper, time_step); wb_touch_sensor_enable(lfoot_rbumper, time_step); wb_touch_sensor_enable(rfoot_lbumper, time_step); wb_touch_sensor_enable(rfoot_rbumper, time_step); // There are 7 controlable LED groups in Webots leds[0] = wb_robot_get_device("ChestBoard/Led"); leds[1] = wb_robot_get_device("RFoot/Led"); leds[2] = wb_robot_get_device("LFoot/Led"); leds[3] = wb_robot_get_device("Face/Led/Right"); leds[4] = wb_robot_get_device("Face/Led/Left"); leds[5] = wb_robot_get_device("Ears/Led/Right"); leds[6] = wb_robot_get_device("Ears/Led/Left"); // get phalanx motor tags // the real Nao has only 2 motors for RHand/LHand // but in Webots we must implement RHand/LHand with 2x8 motors for (i = 0; i < PHALANX_MAX; i++) { char name[32]; sprintf(name, "LPhalanx%d", i + 1); lphalanx[i] = wb_robot_get_device(name); sprintf(name, "RPhalanx%d", i + 1); rphalanx[i] = wb_robot_get_device(name); } // shoulder pitch motors RShoulderPitch = wb_robot_get_device("RShoulderPitch"); LShoulderPitch = wb_robot_get_device("LShoulderPitch"); // shoulder pitch motors HeadYaw = wb_robot_get_device("HeadYaw"); HeadPitch = wb_robot_get_device("HeadPitch"); //LaserHead = wb_robot_get_device("URG-04LX-UG01"); //wb_camera_enable(LaserHead, 4 * time_step); // keyboard //wb_robot_keyboard_enable(10 * time_step); }