//run() is continuously called to check the two timers
//and act if necessary
void run()
{
if(wb_robot_get_time()>beacon_timer){
move_beacon_random();
beacon_timer=wb_robot_get_time()+BEACON_TIMER; //reset the beacon timer
}
if(wb_robot_get_time()>finaltime){
printf(" ----- run over -----\n");
wb_supervisor_simulation_revert();
}
}
void reset(void)
{
int i;
char stringaux[20];
//get e-pucks node names and location field node names
for(i=0;i<NUM_ROBOTS;i++){
sprintf(stringaux,"E_PUCK_%d",i+1);
epucks[i]=wb_supervisor_node_get_from_def(stringaux);
locfield[i]=wb_supervisor_node_get_field(epucks[i],"translation");
}
//get beacon node name and location field node names
beacon=wb_supervisor_node_get_from_def("BEACON1");
beacon_location=wb_supervisor_node_get_field(beacon,"translation");
srand(time(NULL));
//initialize random position for e-puck and beacon
change_robot_positions();
move_beacon_random();
//initialize finaltime and time for first beacon repositioning
finaltime=wb_robot_get_time()+EXP_TIME; // 1000 == 16min40sec
beacon_timer=BEACON_TIMER; //60 == 1min
}
void run(){
// Movement
move();
// Periodically check the current value of alpha
// TODO: make rate limiting consistent with the article
double t = wb_robot_get_time();
long long second = (long long)t;
if(second != previousSecond) {
broadcastMyId();
// Leave time for first broadcasts to arrive
if(second > 1)
alphaAlgorithm();
previousSecond = second;
// Communicate current state (for performance measures)
sendStateToSupervisor();
}
}
int main(int argc, char **argv)
{
wb_robot_init();
int i;
WbDeviceTag servos[NSERVOS];
WbDeviceTag head_led;
for (i=0; i<NSERVOS; i++)
servos[i] = wb_robot_get_device(servo_names[i]);
head_led = wb_robot_get_device("HeadLed");
wb_led_set(head_led, 0x40C040);
do {
double t = wb_robot_get_time();
for (i=0; i<6; i++)
wb_servo_set_position(servos[i], amplitudes[i]*sin(frequency*t) + offsets[i]);
} while (wb_robot_step(TIME_STEP) != -1);
wb_robot_cleanup();
return 0;
}