static int run(void) {
int i;
int *grey = (int *)malloc(sizeof(int)*width);
int speed[2]={150,150};
////default fixed speed, whther Epuck is on line or not
const unsigned char *image;
// 1. Get the sensors values
image = wb_camera_get_image(cam);
for (i = 0; i < width; i++) {
grey[i] = 255-wb_camera_image_get_grey(image, width, i, 0);
////0 = black, 255 = white, so 255-0=255 in grey[i] = black (value representation is now reversed
////capture just topmost row for the width of the camera vision
}
// 2. Behavior-based robotic:
// call the modules in the right order
lem(grey,width);
lfm(grey, width);
llm(grey, width);
//utm();
// 3. Send the values to actuators
wb_differential_wheels_set_speed(
speed[LEFT]+lfm_speed[LEFT],//+utm_speed[LEFT],
speed[RIGHT]+lfm_speed[RIGHT]//+utm_speed[RIGHT]
);
free(grey);
return TIME_STEP;
}
int main() {
double w_left, w_right;
wb_robot_init();
WbDeviceTag human02_wheel;
human02_wheel = wb_robot_get_device("human02_plate");
int time_step = wb_robot_get_basic_time_step();
wb_differential_wheels_enable_encoders(time_step);
for (;;)
{
w_left = 10.;
w_right = 10.;
wb_differential_wheels_set_speed(w_left, w_right);
//printf(" left_encoder=%7.5f\n",wb_differential_wheels_get_left_encoder());
wb_robot_step(32);
}
return 0;
}
/* ****************************** MOVEMENT ****************************** */
void setSpeed(int leftSpeed, int rightSpeed) {
if (leftSpeed < -MAXSPEED) {leftSpeed = -MAXSPEED;}
if (leftSpeed > MAXSPEED) {leftSpeed = MAXSPEED;}
if (rightSpeed < -MAXSPEED) {rightSpeed = -MAXSPEED;}
if (rightSpeed > MAXSPEED) {rightSpeed = MAXSPEED;}
wb_differential_wheels_set_speed(leftSpeed, rightSpeed);
}
//
// 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);
}
// the main function
int main(){
int msl, msr; // Wheel speeds
double *rbuffer;
double buffer[255];
int j;
for(;;) {
reset(); // Resetting the robot
while (wb_receiver_get_queue_length(receiver0) == 0)
wb_robot_step(64);
rbuffer = (double *)wb_receiver_get_data(receiver0);
COEF_KP = rbuffer[0];
COEF_B = rbuffer[1];
COEF_R = rbuffer[2];
COEF_S = rbuffer[3];
COEF_T = rbuffer[4];
msl = 0; msr = 0;
// Forever
for(j = 0; j < NB_STEPS; ++j) {
/* Send and get information */
send_ping(); // sending a ping to other robot, so they can measure their distance to this robot
process_received_ping_messages();
// 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]);
// Flocking behavior of the paper with the wheels speed
flocking_behavior(&msl, &msr);
// 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);
}
}
//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);
}
}
int main() {
//const char *name;
int left_speed, right_speed;
left_speed = 50;
right_speed = 50;
wb_robot_init();
//name = wb_robot_get_name();
wb_differential_wheels_set_speed(left_speed, right_speed);
wb_robot_cleanup();
return 0;
}
int main() {
srand ( time(NULL) );
wb_robot_init();
wb_robot_step(TIME_STEP);
wb_robot_step(TIME_STEP);
/* main loop */
while (wb_robot_step(TIME_STEP) != -1) {
wb_differential_wheels_set_speed(1000,1000);
}
wb_robot_cleanup();
return 0;
}
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]);
}
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
static int run(int ms)
{
int i;
if (mode!=wb_robot_get_mode())
{
mode = wb_robot_get_mode();
if (mode == SIMULATION) {
for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_SIMULATION[i];
printf("Switching to SIMULATION.\n\n");
}
else if (mode == REALITY) {
for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_REALITY[i];
printf("\nSwitching to REALITY.\n\n");
}
}
// if we're testing a new genome, receive weights and initialize trial
if (step == 0) {
int n = wb_receiver_get_queue_length(receiver);
printf("queue length %d\n",n);
//wait for new genome
if (n) {
const double *genes = (double *) wb_receiver_get_data(receiver);
//set neural network weights
for (i=0;i<NB_WEIGHTS;i++) weights[i]=genes[i];
printf("wt[%d]: %g\n",i,weights[i]);
wb_receiver_next_packet(receiver);
}
else {
// printf("time step %d\n",TIME_STEP);
return TIME_STEP;}
const double *gps_matrix = wb_gps_get_values(gps);
robot_initial_position[0] = gps_matrix[0];//gps_position_x(gps_matrix);
robot_initial_position[1] = gps_matrix[2];//gps_position_z(gps_matrix);
printf("rip[0]: %g, rip[1]: %g\n",robot_initial_position[0],robot_initial_position[1]);
fitness = 0;
}
step++;
printf("Step: %d\n", step);
if(step < TRIAL_DURATION/(double)TIME_STEP) {
//drive robot
fitness+=run_trial();
//send message with current fitness
double msg[2] = {fitness, 0.0};
wb_emitter_send(emitter, (void *)msg, 2*sizeof(double));
}
else {
//stop robot
wb_differential_wheels_set_speed(0, 0);
//send message to indicate end of trial
double msg[2] = {fitness, 1.0};
wb_emitter_send(emitter, (void *)msg, 2*sizeof(double));
//reinitialize counter
step = 0;
}
return TIME_STEP;
return TIME_STEP;
}
// Find the fitness for obstacle avoidance of the passed controller
double fitfunc(double waypoints[DATASIZE],int its) {
double fitness = 0.0; // Fitness of waypoint set
int j,i;
int left_encoder, right_encoder, old_left_encoder = 0, old_right_encoder = 0;
double theta = 0.0, old_theta = 0.0;
double x = 0.0, y = 0.0;
int k = 0;
int ds_l, ds_r, lspeed, rspeed;
double fit_ds, fit_speed, fit_totalds, fitness_distance, fitness_stuck, fitness_goal, fitness_waypoints;
fit_speed = 0.0;
fit_ds = 0.0;
fit_totalds = 0.0;
int count_stuck = 0;
int count_waypoints = 0;
int count_waypoints_actual = 0;
fitness_goal = 0.333;
double waypoints_reached[DATASIZE];
for (i=0; i<DATASIZE; i++) {
waypoints_reached[i] = 0.0;
}
wb_differential_wheels_set_encoders(0,0);
// Evaluate fitness repeatedly
for (j=0; j<its; j++) {
// if close enough to current waypoint select next waypoint
if ((x - waypoints[k])*(x - waypoints[k])+(y - waypoints[k+1])*(y - waypoints[k+1]) < 0.0001) {
//printf("WAYPOINT REACHED (%.2f,%.2f) YESSS \n",waypoints[k], waypoints[k+1]);
count_waypoints += 1;
count_waypoints_actual += 1;
waypoints_reached[k] = waypoints[k];
waypoints_reached[k+1] = waypoints[k+1];
k += 2;
loc_state = ALIGN;
if (k == DATASIZE) {
k = 0;
}
}
// compute wheel speeds
compute_wheel_speeds(x,y,theta,waypoints[k],waypoints[k+1],&lspeed,&rspeed);
wb_differential_wheels_set_speed(lspeed,rspeed);
robot_step(128); // run one step
// compute current position
left_encoder = wb_differential_wheels_get_left_encoder();
right_encoder = wb_differential_wheels_get_right_encoder();
ds_l = left_encoder - old_left_encoder;
ds_r = right_encoder - old_right_encoder;
odometry(ds_l, ds_r, x, y, old_theta, &x, &y, &theta, &fit_ds);
old_left_encoder = left_encoder;
old_right_encoder = right_encoder;
old_theta = theta;
fit_speed += (fabs(lspeed) + fabs(rspeed))/(2.0*MAX_SPEED);
fit_totalds += fit_ds;
if(fit_ds < 0.0001) {
count_stuck += 1;
}
if (y > 1.2) { // when this coordinate is reached the robot is considered escaped and the evaluation stops
printf("\nROBOT ESCAPED!! :)\n");
for (i=0; i<DATASIZE; i++) {
printf("%f ",waypoints_reached[i]);
}
printf("\n");
count_waypoints = 5; //this acts as a bonus for reaching the goal
fitness_goal = 1.0;
break;
}
}
fit_speed /= its;
fitness_distance = 1/(1+exp(fit_totalds-4));
fitness_stuck = (1 - (double)count_stuck/its);
fitness_goal = fitness_goal;
fitness_waypoints = pow((((double)count_waypoints/10)+0.5),2); //1.2 are assumed to be the length of sides of the world
fitness = pow(fitness_distance,0.75)*pow(fitness_stuck,1.5)*fitness_goal*fitness_waypoints;
/*
if (fitness > fitness_best) {
fitness_best = fitness;
printf("\nFITNESS: %f\nTotal Distance: %.2f\nStuck Ratio: %.2f\n\n", fitness, fit_totalds, fitness_stuck);
}
*/
printf("FITNESS: %f waypoints: %d ditance: %.2f stuck: %.2f\n", fitness, count_waypoints_actual, fit_totalds, fitness_stuck);
return fitness;
}
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);
}
}
// 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
}
//Turns right by setting speed
void turnRight()
{
wb_led_set(led4, 0);
currentState = TURNING;
wb_differential_wheels_set_speed(-MAX_SPEED, MAX_SPEED);
}
////////////////////////////////////////////
// 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;
}
int main(int argc, char **argv) {
wb_robot_init();
// check if roslaunch is properly installed
if (system("which roslaunch > /dev/null")!=0) {
fprintf(stderr,"Cannot find roslaunch in PATH. Please check that ROS is properly installed.\n");
wb_robot_cleanup();
return 0;
}
// launch the joy ROS node
int roslaunch=fork();
if (roslaunch==0) { // child process
execlp("roslaunch","roslaunch","joy.launch",NULL);
return 0;
}
// From this point we assume the ROS_ROOT, ROS_MASTER_URI and
// ROS_PACKAGE_PATH environment variables are set appropriately.
// See ROS manuals to set them properly.
ros::init(argc, argv, "joystick");
ros::NodeHandle nh;
ros::Subscriber sub=nh.subscribe("joy", 10, joy_callback);
// ros::Subscriber sub=nh.subscribe<int32>("joy", 10, joy_callback);
double left_speed = 0;
double right_speed = 0;
ROS_INFO("Joypad connected and running.");
ROS_INFO("Commands:");
ROS_INFO("arrow up: move forward, speed up");
ROS_INFO("arrow down: move backward, slow down");
ROS_INFO("arrow left: turn left");
ROS_INFO("arrow right: turn right");
// control loop: simulation steps of 32 ms
while(wb_robot_step(32) != -1) {
// get callback called
ros::spinOnce();
switch(cmd) {
case JOYPAD_UP:
left_speed=SPEED;
right_speed=SPEED;
break;
case JOYPAD_DOWN:
left_speed=-SPEED;
right_speed=-SPEED;
break;
case JOYPAD_LEFT:
left_speed=-SPEED;
right_speed=SPEED;
break;
case JOYPAD_RIGHT:
left_speed=SPEED;
right_speed=-SPEED;
break;
case JOYPAD_STOP:
left_speed = 0;
right_speed = 0;
break;
}
// set motor speeds
wb_differential_wheels_set_speed(left_speed, right_speed);
}
ros::shutdown();
kill(roslaunch,SIGINT); // terminate roslaunch for joy ROS node as with Ctrl-C
wb_robot_cleanup();
return 0;
}
// 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;
}
static int run(int ms)
{
int i;
if (mode!=wb_robot_get_mode())
{
mode = wb_robot_get_mode();
if (mode == SIMULATION) {
for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_SIMULATION[i]; //offset/baseline noise is ~35 in test world
puts("Switching to SIMULATION.\n");
}
else if (mode == REALITY) {
for(i=0;i<NB_DIST_SENS;i++) ps_offset[i]=PS_OFFSET_REALITY[i];
puts("\nSwitching to REALITY.\n");
}
}
// if we're testing a new genome, receive weights and initialize trial
if (step == 0) {
int n = wb_receiver_get_queue_length(receiver);
// printf("queue length %d\n",n);
reset_neural_network();
//wait for new genome
if (n) {
const _Bool *genes_bin = (_Bool *) wb_receiver_get_data(receiver);
//decode obtained boolean genes into real numbers and set the NN weights.
double *genes= popDecoder(genes_bin); //is const necessary here?
//set neural network weights
for (i=0;i<NB_WEIGHTS;i++) {
weights[i]=genes[i];
// printf("wt[%d]: %g\n",i,weights[i]);
}
wb_receiver_next_packet(receiver);
}
else {
return TIME_STEP;}
fitness = 0;
}
step++;
//printf("Step: %d\n", step);
//first trial has 360 steps available, subsequent trials have 180.
//At each epoch, first 360 steps (e.g. the first trial) have the number step_max=360 associated with them.
if(step < TRIAL_STEPS) {
//try the robot
fitness+= run_trial();
//printf("Fitness in step %d: %g\n",step,fitness);
double msg[2] = {fitness, 0.0};
wb_emitter_send(emitter, (void *)msg, 2*sizeof(double));
}
else {
//stop robot
wb_differential_wheels_set_speed(0, 0);
//send message to indicate end of trial
double msg[2] = {fitness, 1.0};
wb_emitter_send(emitter, (void *)msg, 2*sizeof(double));
//reinitialize counter
step = 0;
}
return TIME_STEP;
return TIME_STEP;
}
