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;
}
/*
* 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)
}
}