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