// // Trial to test the robot's behavior (according to NN) in the environment // double run_trial() { double inputs[NB_INPUTS]; double outputs[NB_OUTPUTS]; //Get position of the e-puck static double position[3]={0.0,0.0,0.0}; const double *gps_matrix = wb_gps_get_values(gps); position[0] = gps_matrix[0];//gps_position_x(gps_matrix); position[1] = gps_matrix[2];//gps_position_z(gps_matrix); position[2] = gps_matrix[1];//gps_position_y(gps_matrix); //Speed of the robot's wheels within the +SPEED_RANGE and -SPEED_RANGE values int speed[2]={0,0}; //Maximum activation of all IR proximity sensors [0,1] double maxIRActivation = 0; //get sensor data, i.e., NN inputs int i, j; for(j=0;j<NB_INPUTS;j++) { inputs[j]=(((double)wb_distance_sensor_get_value(ps[j])-ps_offset[j])<0)?0:((double)wb_distance_sensor_get_value(ps[j])-ps_offset[j])/((double)PS_RANGE); //get max IR activation if(inputs[j]>maxIRActivation) maxIRActivation=inputs[j]; printf("sensor #: %d, sensor read: %g, maxIR: %g\n", j, inputs[j], maxIRActivation); } //printf("max IR activation: %f\n", maxIRActivation); // Run the neural network and computes the output speed of the robot run_neural_network(inputs, outputs); speed[LEFT] = SPEED_RANGE*outputs[0]; speed[RIGHT] = SPEED_RANGE*outputs[1]; // If you are running against an obstacle, stop the wheels if ( maxIRActivation > 0.9 ) { speed[LEFT]=0; speed[RIGHT]=0; } // Set wheel speeds to output values wb_differential_wheels_set_speed(speed[LEFT], speed[RIGHT]); //left, right // Stop the robot if it is against an obstacle for (i=0;i<NB_DIST_SENS;i++) { int tmpps=(((double)wb_distance_sensor_get_value(ps[i])-ps_offset[i])<0)?0:((double)wb_distance_sensor_get_value(ps[i])-ps_offset[i]); if(OBSTACLE_THRESHOLD<tmpps ) {// proximity sensors //printf("%d \n",tmpps); speed[LEFT] = 0; speed[RIGHT] = 0; break; } } return compute_fitness(speed, position, maxIRActivation); }
double run_trial() { int speed[2]={0,0}; //Maximum activation of all IR proximity sensors [0,1] //double maxIRActivation = 0; //get sensor data, i.e., NN inputs int j; for(j=0;j<NB_DIST_SENS;j++) { //printf("sensor %d reads %g\n",j,wb_distance_sensor_get_value(ps[j])); temp[j]=(((double)wb_distance_sensor_get_value(ps[j])-ps_offset[j])<0)?0:(((double)wb_distance_sensor_get_value(ps[j])-ps_offset[j])/((double)PS_RANGE)); } for(j=0;j<NB_PS;j++) { //printf("sensor %d reads %g\n",j,wb_distance_sensor_get_value(ps[j])); inputs[j]=(temp[2*j]+temp[(2*j)+1])/2.0; } inputs[4]= wb_distance_sensor_get_value(fs[0])-fs_offset[0]; //printf("floor sensor:%g\n",inputs[4]); // Run the neural network and computes the output speed of the robot run_neural_network(inputs, outputs); speed[LEFT] = clip_value(outputs[0],max_speed); //scale outputs between 0 and 1 speed[RIGHT] = clip_value(outputs[1],max_speed); //printf("L: %d; R: %d\n",speed[LEFT],speed[RIGHT]); // Set wheel speeds to output values wb_differential_wheels_set_speed(speed[LEFT], speed[RIGHT]); //left, right //printf("Set wheels to outputs.\n"); return compute_fitness(inputs[4]); return compute_fitness(inputs[4]); }
//Determines whether or not the puck is extremely close, //moderately close, or far from something in front of it. int collisionPending() { WbDeviceTag frontDistanceSensors[] = {ps0, ps7}; int tooHot = 0; int i; for(i=0; i < 2; i++) { double currentSensorValue = wb_distance_sensor_get_value(frontDistanceSensors[i]); if(currentSensorValue >= STOP_THRESHOLD) tooHot = 1; else if(currentSensorValue > SLOW_THRESHOLD) tooHot = 2; } return tooHot; }
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); } } }
// the main function int main(){ int msl, msr, bmsl, bmsr; // Wheel speeds int sum_sensors; // Braitenberg parameters int i, j; // Loop counter int max_sens; // Store highest sensor value double *rbuffer; double buffer[255]; int distances[NB_SENSORS]; // Array for the distance sensor readings for(;;) { reset(); // Resetting the robot while (wb_receiver_get_queue_length(receiver0) == 0) { wb_robot_step(64); } rbuffer = (double *)wb_receiver_get_data(receiver0); for(i=0; i<FLOCK_SIZE; i++) { timestamp[i] = 0; } msl = 0; msr = 0; max_sens = 0; // Forever for(j = 0; j < NB_STEPS; ++j){ bmsl = 0; bmsr = 0; sum_sensors = 0; max_sens = 0; /* Braitenberg */ for(i=0;i<NB_SENSORS;i++) { distances[i]=wb_distance_sensor_get_value(ds[i]); //Read sensor values sum_sensors += distances[i]; // Add up sensor values max_sens = max_sens>distances[i]?max_sens:distances[i]; // Check if new highest sensor value // Weighted sum of distance sensor values for Braitenburg vehicle bmsr += rbuffer[i] * distances[i]; bmsl += rbuffer[i+NB_SENSORS] * distances[i]; } // Adapt Braitenberg values (empirical tests) bmsl/=MIN_SENS; bmsr/=MIN_SENS; bmsl+=66; bmsr+=72; /* Send and get information */ timestamp[robot_id]++; send_ping_robot(robot_id); process_received_ping_messages(); //printf("ROBOT %d: wheels %d, %d\n", robot_id, bmsl, bmsr); // Compute self position prev_my_position[0] = my_position[0]; prev_my_position[1] = my_position[1]; update_self_motion(msl,msr); speed[robot_id][0] = (1/DELTA_T)*(my_position[0]-prev_my_position[0]); speed[robot_id][1] = (1/DELTA_T)*(my_position[1]-prev_my_position[1]); // Reynold's rules with all previous info (updates the speed[][] table) reynolds_rules(); //printf("ROBOT %d: speed: %f, %f\n", robot_id, speed[robot_id][0], speed[robot_id][1]); // Compute wheels speed from reynold's speed compute_wheel_speeds(&msl, &msr); //printf("wheels: %d, %d\n", msl, msr); // Adapt speed instinct to distance sensor values if (sum_sensors > NB_SENSORS*MIN_SENS) { msl -= msl*max_sens/(2*MAX_SENS); msr -= msr*max_sens/(2*MAX_SENS); } // Add Braitenberg msl += bmsl; msr += bmsr; // Set speed wb_differential_wheels_set_speed(msl,msr); // Continue one step wb_robot_step(TIME_STEP); } wb_emitter_send(emitter0,(void *)buffer,sizeof(double)); wb_receiver_next_packet(receiver0); } }
void flocking_behavior(int *msl, int *msr) { int i; float a[2]; // desired heading vector float b[2]; // current heading vector float h[2]; // haeding alignment vector float p[2]; // proximal control vector float r[2]; // migration control vector float s[2]; // flocking control vector float u = 0.0; // linear velocity of the e-puck float w = 0.0; // angular velocity of the e-puck float Nl = 0.0; // angular velocity of the left wheel float Nr = 0.0; // angular velocity of the right wheel float fk = 0.0; // buffer variable to compute the virtual force from IR sensor float norm = 0.0; // buffer variable for vector normalization float sens = 0.0; // buffer variable to get senser values float xloc = 0.0; // buffer variable for the robots relative x position float zloc = 0.0; // buffer variable for the robots relative z position float aloc = 0.0; // buffer variable for the robots relative anlge float dloc = 0.0; // buffer variable for the robots relative distance float coef_k = COEF_KP *(COEF_K_U - COEF_K_L) + COEF_K_L; // De-normalization of coef_k float coef_b = COEF_B *(COEF_B_U - COEF_B_L) + COEF_B_L; // De-normalization of coef_b float coef_s = COEF_S *(COEF_S_U - COEF_S_L) + COEF_S_L; // De-normalization of coef_s float coef_r = COEF_R *(COEF_R_U - COEF_R_L) + COEF_R_L; // De-normalization of coef_r float coef_t = COEF_T *(COEF_T_U - COEF_T_L) + COEF_T_L; // De-normalization of coef_t /* * HEADING VECTOR */ h[0] = h[1] = 0.0f; for(i = 0; i < FLOCK_SIZE; ++i) { if (i != robot_id) { h[0] += cosf( ( get_bearing_in_degrees() - neighboors_bearing[i] ) * M_PI / 180.0 ); h[1] += sinf( ( get_bearing_in_degrees() - neighboors_bearing[i] ) * M_PI / 180.0 ); } } norm = sqrtf(h[0]*h[0] + h[1]*h[1]); h[0] /= norm; h[1] /= norm; /* * PROXIMAL CONTROL */ p[0] = p[1] = .0f; for(i = 0; i < NB_SENSORS; ++i) { sens = wb_distance_sensor_get_value(ds[i]); if (sens >= MIN_SENS) fk = -(sens*sens)/MAX_SENS; if (sens < MIN_SENS) fk = 0.0; p[0] += cosf( sensor_degrees[i] * M_PI / 180.0 ) * fk / ((float)NB_SENSORS); p[1] += sinf( sensor_degrees[i] * M_PI / 180.0 ) * fk / ((float)NB_SENSORS); } s[0] = s[1] = .0f; for(i = 0; i < FLOCK_SIZE; i++) { fk = 0.0; xloc = -relative_pos[i][1]; zloc = -relative_pos[i][0]; dloc = sqrtf(xloc*xloc + zloc*zloc); if (i != robot_id) { if ( xloc >= 0.0) aloc = +atanf(zloc/xloc) * 180/M_PI; if ( xloc < 0.0) aloc = +atanf(zloc/xloc) * 180/M_PI + 180; if ( dloc >= coef_t) fk = +(dloc - coef_t)*(dloc - coef_t); if ( dloc < coef_t) fk = -(dloc - coef_t)*(dloc - coef_t); s[0] += cosf(aloc * M_PI/180) * fk; s[1] += sinf(aloc * M_PI/180) * fk; } } /* * MIGRATION URGE */ r[0] = cosf(( get_bearing_in_degrees() - get_migr_bearing_in_degrees() - 90)* M_PI/180); r[1] = sinf(( get_bearing_in_degrees() - get_migr_bearing_in_degrees() - 90)* M_PI/180); /* * HEADING VECTOR (desired = a, current = b) */ a[0] = h[0] + coef_b * p[0] + coef_r * r[0] + coef_s * s[0]; a[1] = h[1] + coef_b * p[1] + coef_r * r[1] + coef_s * s[1]; norm = sqrtf(a[0]*a[0] + a[1]*a[1]); a[0] /= norm; a[1] /= norm; b[0] = cosf( 0.0 * M_PI / 180 ); b[1] = sinf( 0.0 * M_PI / 180 ); /* * MOTION CONTROL */ float phase_a = atanf(a[1] / a[0]); float phase_b = atanf(b[1] / b[0]); float dphase = phase_b - phase_a; u = (a[0]*b[0] + a[1]*b[1]) * MAX_SPEED_MS; w = dphase * coef_k; if (u < 0.0){ u = 0.0; if (phase_a > 0) dphase = phase_b - (phase_a - M_PI); if (phase_a <= 0) dphase = phase_b - (phase_a + M_PI); w = dphase * coef_k; } /* * WHEEL VELOCITY */ Nl = (u + AXLE_LENGTH * w / 2.0 ) / (2 * M_PI * WHEEL_RADIUS); Nr = (u - AXLE_LENGTH * w / 2.0 ) / (2 * M_PI * WHEEL_RADIUS); *msl = (int) ( Nl / ( SPEED_UNIT_RADS )) ; *msr = (int) ( Nr / ( SPEED_UNIT_RADS )) ; limit(msl,MAX_SPEED); limit(msr,MAX_SPEED); /* * PRINT */ if (DISPLAY == 2) { if (robot_id == 3){ printf(" ------ ROBOT ID ------ \n"); printf(" robot id : %d \n " , robot_id); printf(" ------ VECTORS ------ \n "); printf(" h : %.6lf , %.6lf \n" , h[0], h[1]); printf(" p : %.6lf , %.6lf \n" , p[0], p[1]); printf(" r : %.6lf , %.6lf \n" , r[0], r[1]); printf(" a : %.6lf , %.6lf \n" , a[0], a[1]); printf(" b : %.6lf , %.6lf \n" , b[0], b[1]); printf(" s : %.6lf , %.6lf \n" , s[0], s[1]); printf(" ------ CONTROL ------ \n "); printf(" pa , pb : %.6lf , %.6lf \n" , phase_a*(180/M_PI), phase_b*(180/M_PI)); printf(" u , w : %.6lf , %.6lf \n" , u, w); printf(" Nl , Nr : %.6lf , %.6lf \n" , Nl , Nr); printf(" MSL/R : %.6d , %.6d \n", *msl, *msr); printf(" --------------------- \n "); } } }
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; }
//////////////////////////////////////////// // 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 }
void getSensorValues(int *sensorTable) { unsigned int i; for(i = 0; i < NB_SENSORS; i++) { sensorTable[i] = wb_distance_sensor_get_value(sensors[i]); } }
// controller main loop static int run(int ms) { static double ds_value[NB_SENSORS]; int i; for (i = 0; i < NB_SENSORS; i++) // read sensor values // reads the handle in ps[i] and saves its value in ds_value[i] ds_value[i] = wb_distance_sensor_get_value(ps[i]); // range: 0 (far) to 4095 (0 distance (in theory)) // choose behavior double left_speed, right_speed; int duration; //printf("ds_value[0] = %f\n", ds_value[0]); int front_threshold = 1024; int side_threshold = 2048; if (ds_value[0] > front_threshold ) { // we detected an obstacle on the front right left_speed = -400; right_speed = +400; // turn around duration = 10 * TIME_STEP; // for 1280 milliseconds } else if ( ds_value[1] > front_threshold ) { left_speed = 200; right_speed = 400; duration = 10 * TIME_STEP; } else if ( ds_value[6] > front_threshold ) { // we detected an obstacle on the front right right_speed = -400; left_speed = +400; //turn around the other side duration = 10 * TIME_STEP; // for 1280 millisecondss } else if ( ds_value[7] > front_threshold) { right_speed = -400; left_speed = +400; //turn around the other side duration = 10 * TIME_STEP; // for 1280 millisecondss } else if ( ds_value[2] > side_threshold) { // obstacle from the right // move away left a bit left_speed = 100; right_speed = 200; duration = 5 * TIME_STEP; } else if ( ds_value[5] > side_threshold) { // obstacle from the left // move away right a bit left_speed = 200; right_speed = 100; duration = 5 * TIME_STEP; } else if ( ds_value[3] > side_threshold || ds_value[4] > side_threshold) { // detected on the back // run away from the obstacle left_speed = right_speed = 500; // go straight duration = TIME_STEP; // for 64 milliseconds } else { left_speed = right_speed = 500; // go straight duration = TIME_STEP; // for 64 milliseconds } // update fitness fitness_step(left_speed, right_speed, ds_value); // actuate wheel motors // sets the e-pucks wheel speeds to left_speed (left wheel) and right_speed (right wheel) // max speed is 1000 == 2 turns per second wb_differential_wheels_set_speed(left_speed, right_speed); // compute and display fitness every 1000 controller steps if (step_count % 1000 == 999) { double fitness = fitness_compute(); printf("fitness = %f\n", fitness); fitness_reset(); } return duration; }
// 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; }