int main() {
wb_robot_init();
printf("hello from NAO\n");
time_step = wb_robot_get_basic_time_step();
receiver = wb_robot_get_device("receiver");
wb_receiver_enable(receiver, time_step);
emitter = wb_robot_get_device("emitter2");
wb_receiver_enable(emitter, time_step);
find_and_enable_devices();
wb_motor_set_position (RShoulderPitch, 1.57079633);
wb_motor_set_position (LShoulderPitch, 1.57079633);
wb_motor_set_position (HeadYaw, 0.0);
//wb_motor_set_position (HeadPitch, 0.1); //10 degrees
// run until simulation is restarted
while (wb_robot_step(time_step) != -1) {
//check_for_new_genes();
//sense_compute_and_actuate();
//int samples = wb_camera_get_width(LaserHead);
//double field_of_view = wb_camera_get_fov(LaserHead);
//const float *values = wb_camera_get_range_image(LaserHead);
report_step_state_to_supervisor();
//printf("%f\n", field_of_view);
//printf("size: %d\n", sizeof(values));
//int i;
//for(i=0; i<samples;i++){
// printf("value %d is %f\n",i, values[i]);
//}
if(wb_robot_step(time_step) == 0){
//report_step_state_to_supervisor();
}
}
wb_robot_cleanup();
return 0;
}
/*
* 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)
}
}
/*
* 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)
}
}
//
// 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;
}
int main(int argc, char *argv[]) {
/* define variables */
WbDeviceTag turret_sensor;
double perf_data[3];
double clock = 0;
/* initialize Webots */
wb_robot_init();
// read robot id from the robot's name
char* robot_name;
robot_name=(char*) wb_robot_get_name();
sscanf(robot_name,"e-puck%d",&robot_id);
emitter = wb_robot_get_device("emitter");
wb_emitter_set_range(emitter,COM_RANGE);
receiver = wb_robot_get_device("receiver");
wb_receiver_set_channel(receiver,0);
wb_receiver_enable(receiver,TIME_STEP);
wb_robot_step(TIME_STEP);
turret_sensor = wb_robot_get_device("ts");
wb_light_sensor_enable(turret_sensor,TIME_STEP);
/* main loop */
while (wb_robot_step(TIME_STEP) != -1) {
clock += (double)TIME_STEP/1000;
/* get light sensor value */
sensor_value = wb_light_sensor_get_value(turret_sensor);
if(wb_receiver_get_queue_length(receiver) > 0) {
wb_emitter_set_channel(emitter,3);
perf_data[0] = (double)robot_id;
perf_data[1] = (double)(pkt_count-1);
perf_data[2] = 0;
wb_emitter_send(emitter,perf_data,3*sizeof(double));
break; // stop node
}
else {
send_data(clock); // send measurement data
}
}
wb_robot_cleanup();
return 0;
}
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;
}
/*
* The call to wb_robot_step is mandatory for the function
* supervisor_node_was_found to be able to work correctly.
*/
static void reset(void)
{
int i;
for(i=0; i<POP_SIZE; i++) fitness[i]=-1;
srand(time(0));
pF1= fopen("initPop.txt","w+");
//pF2= fopen("real2IntGenome.txt","w+");
//pF3= fopen("encodedPop.txt","w+");
// Initiate the emitter used to send genomes to the experiment
emitter = wb_robot_get_device("emittersupervisor");
// Initiate the receiver used to receive fitness
receiver = wb_robot_get_device("receiversupervisor");
wb_receiver_enable(receiver, TIME_STEP);
// Create a supervised node for the robot
robot = wb_supervisor_node_get_from_def("EPUCK");
assert(robot!=NULL);
pattern=wb_supervisor_node_get_from_def("FLOOR");
pattern_rotation = wb_supervisor_node_get_field(pattern,"rotation");
assert(pattern!=NULL);
assert(pattern_rotation!=NULL);
trans_field = wb_supervisor_node_get_field(robot,"translation");
rot_field = wb_supervisor_node_get_field(robot,"rotation");
ctrl_field= wb_supervisor_node_get_field(robot,"controller");
wb_robot_step(0);
wb_robot_step(0); //this is magic
// set the robot controller to nn
const char *controller_name = "drive2";
wb_supervisor_field_set_sf_string(ctrl_field,controller_name);
//check whether robot was found
if (wb_supervisor_node_get_type(robot) == WB_NODE_NO_NODE)
puts("Error: node EPUCK not found!!!");
if(EVOLVING) {
//Open log files
f1= fopen ("../../data/fitness.txt", "wt");
f2= fopen ("../../data/genomes.txt", "wt");
//pR= fopen("savePop.txt","w+");
//initial weights randomly
initializePopulation();
//rtoi(); //real to int conversion before encoding
popEncoder();
//puts("NEW EVOLUTION");
//puts("GENERATION 0");
// send genomes to experiment
resetRobotPosition();
wb_emitter_send(emitter, (void *)pop_bin[evaluated_inds], GENOME_LENGTH*sizeof(_Bool));
//instead save binary genomes to a file
//puts("Genes sent.");
}
else {
// testing best individual
// Read best genome from bestgenome.txt and initialize weights.
f3= fopen ("../../data/bestgenome.txt", "rt");
fscanf(f3,"%d %d", &generation, &evaluated_inds);
//TODO either read binary genome or make sure it is decoded prior to storage
for(i=0;i<GENOME_LENGTH;i++) fscanf(f3,"%d ",&pop_bin[0][i]);
//printf("TESTING INDIVIDUAL %d, GENERATION %d\n", evaluated_inds, generation);
// send genomes to experiment
resetRobotPosition();
// wb_emitter_send(emitter, (void *)pop[0], NB_GENES*sizeof(double));
}
return;
}
int main() {
printf("hello from supervisor\n");
const char *robot_name[ROBOTS] = {"NAO"};
WbNodeRef node;
WbFieldRef robot_translation_field[ROBOTS],robot_rotation_field[ROBOTS],ball_translation_field;
//WbDeviceTag emitter, receiver;
int i,j;
int score[2] = { 0, 0 };
double time = 10 * 60; /* a match lasts for 10 minutes */
double ball_reset_timer = 0;
double ball_initial_translation[3] = { -2.5, 0.0324568, 0 };
double robot_initial_translation[ROBOTS][3] = {
{-4.49515, 0.234045, -0.0112415},
{0.000574037, 0.332859, -0.00000133636}};
double robot_initial_rotation[ROBOTS][4] = {
{0.0604202, 0.996035, -0.0652942, 1.55047},
{0.000568956, 0.70711, 0.707104, 3.14045}};
double packet[ROBOTS * 3 + 2];
char time_string[64];
const double *robot_translation[ROBOTS], *robot_rotation[ROBOTS], *ball_translation;
wb_robot_init();
time_step = wb_robot_get_basic_time_step();
emitter = wb_robot_get_device("emitter");
wb_receiver_enable(emitter, time_step);
receiver = wb_robot_get_device("receiver");
wb_receiver_enable(receiver, time_step);
for (i = 0; i < ROBOTS; i++) {
node = wb_supervisor_node_get_from_def(robot_name[i]);
robot_translation_field[i] = wb_supervisor_node_get_field(node,"translation");
robot_translation[i] = wb_supervisor_field_get_sf_vec3f(robot_translation_field[i]);
for(j=0;j<3;j++) robot_initial_translation[i][j]=robot_translation[i][j];
robot_rotation_field[i] = wb_supervisor_node_get_field(node,"rotation");
robot_rotation[i] = wb_supervisor_field_get_sf_rotation(robot_rotation_field[i]);
for(j=0;j<4;j++) robot_initial_rotation[i][j]=robot_rotation[i][j];
}
node = wb_supervisor_node_get_from_def("BALL");
ball_translation_field = wb_supervisor_node_get_field(node,"translation");
ball_translation = wb_supervisor_field_get_sf_vec3f(ball_translation_field);
for(j=0;j<3;j++) ball_initial_translation[j]=ball_translation[j];
/* printf("ball initial translation = %g %g %g\n",ball_translation[0],ball_translation[1],ball_translation[2]); */
set_scores(0, 0);
while(wb_robot_step(TIME_STEP)!=-1) {
//printf("supervisor commands START!\n");
check_for_slaves_data();
ball_translation = wb_supervisor_field_get_sf_vec3f(ball_translation_field);
for (i = 0; i < ROBOTS; i++) {
robot_translation[i]=wb_supervisor_field_get_sf_vec3f(robot_translation_field[i]);
/* printf("coords for robot %d: %g %g %g\n",i,robot_translation[i][0],robot_translation[i][1],robot_translation[i][2]); */
packet[3 * i] = robot_translation[i][0]; /* robot i: X */
packet[3 * i + 1] = robot_translation[i][2]; /* robot i: Z */
if (robot_rotation[i][1] > 0) { /* robot i: rotation Ry axis */
packet[3 * i + 2] = robot_rotation[i][3]; /* robot i: alpha */
} else { /* Ry axis was inverted */
packet[3 * i + 2] = -robot_rotation[i][3];
}
}
packet[3 * ROBOTS] = ball_translation[0]; /* ball X */
packet[3 * ROBOTS + 1] = ball_translation[2]; /* ball Z */
wb_emitter_send(emitter, packet, sizeof(packet));
/* Adds TIME_STEP ms to the time */
time -= (double) TIME_STEP / 1000;
if (time < 0) {
time = 10 * 60; /* restart */
}
sprintf(time_string, "%02d:%02d", (int) (time / 60), (int) time % 60);
wb_supervisor_set_label(2, time_string, 0.45, 0.01, 0.1, 0x000000, 0.0); /* black */
if (ball_reset_timer == 0) {
if (ball_translation[0] > GOAL_X_LIMIT) { /* ball in the blue goal */
set_scores(++score[0], score[1]);
ball_reset_timer = 3; /* wait for 3 seconds before reseting the ball */
} else if (ball_translation[0] < -GOAL_X_LIMIT) { /* ball in the yellow goal */
set_scores(score[0], ++score[1]);
ball_reset_timer = 3; /* wait for 3 seconds before reseting the ball */
}
} else {
ball_reset_timer -= (double) TIME_STEP / 1000.0;
if (ball_reset_timer <= 0) {
ball_reset_timer = 0;
wb_supervisor_field_set_sf_vec3f(ball_translation_field, ball_initial_translation);
for (i = 0; i < ROBOTS; i++) {
wb_supervisor_field_set_sf_vec3f(robot_translation_field[i], robot_initial_translation[i]);
wb_supervisor_field_set_sf_rotation(robot_rotation_field[i], robot_initial_rotation[i]);
}
}
}
}
wb_robot_cleanup();
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;
}
