int main() { double buffer[255]; double fit; double *rbuffer; int i; wb_robot_init(); reset(); receiver_enable(rec,32); differential_wheels_enable_encoders(64); robot_step(64); while (1) { // Wait for data while (receiver_get_queue_length(rec) == 0) { robot_step(64); } rbuffer = (double *)wb_receiver_get_data(rec); // Check for pre-programmed avoidance behavior if (rbuffer[DATASIZE] == -1.0) { fitfunc(gwaypoints,100); // Otherwise, run provided controller } else { fit = fitfunc(rbuffer,rbuffer[DATASIZE]); buffer[0] = fit; wb_emitter_send(emitter,(void *)buffer,sizeof(double)); } wb_receiver_next_packet(rec); } return 0; }
static void reset(void) { int i; int channel; robot_name = robot_get_name(); char connector_name[10]; // Enable the Connectors // connectors[0] = robot_get_device("con1"); // connectors[1] = robot_get_device("con2"); // connector_enable_presence(connectors[0], TIME_STEP); // connector_enable_presence(connectors[1], TIME_STEP); int piece_type = robot_name[1] - '0' -1; max_connector_number = piece_type_nb_connectors[piece_type]; ////robot_console_printf("======= %d, %d\n", piece_type, nb_connectors); for (i = 0; i< max_connector_number; i++) { // The first connector is the one that attaches to the robot arm // Construct the names of connectors //sprintf(connector_name, "%s_c%d", robot_name, 2*i+1); sprintf(connector_name, "c%d", i+1); //robot_console_printf("connector %d: %s \n", i+1, connector_name); // Activate the connectors connectors[i] = robot_get_device(connector_name); connector_enable_presence(connectors[i], TIME_STEP); // Internal state connector_status[i] = UNLOCKED; connector_presence[i] = 0; count_badlock[i] = 0; } // Enable keyboard robot_keyboard_enable(TIME_STEP); // Enable the emitter/receiver ------------% commEmitter = robot_get_device("rs232_out"); commReceiver = robot_get_device("rs232_in"); /* If the channel is not the good one, we change it. */ channel = emitter_get_channel(commEmitter); if (channel != COMMUNICATION_CHANNEL) { emitter_set_channel(commEmitter, COMMUNICATION_CHANNEL); } receiver_enable(commReceiver, TIME_STEP); //robot_console_printf("The %s robot is reset, it uses %d connectors\n", robot_name, max_connector_number); return; }
static void reset(void) { int i,j; int channel; robot_name = robot_get_name(); robot_num = robot_name[1]; char connector_name[15]; float khepera3_matrix[9][2] = { {0, 0}, {-20000, 20000}, {-50000, 50000}, {-70001, 70000}, {70001, -70000}, {50000, -50000}, {20000, -20000}, {0, 0}, {0, 0} }; // { {-5000, -5000}, {-20000, 40000}, {-30000, 50000}, {-70000, 70000}, {70000, -60000}, // {50000, -40000}, {40000, -20000}, {-5000, -5000}, {-10000, -10000} }; char sensors_name[12]; // float (*temp_matrix)[2]; range = RANGE; sensor_number = 9; sprintf(sensors_name, "ds0"); range = 2000; // Enable the Distance sensors and Braitenberg weights for (i = 0; i < sensor_number; i++) { sensors[i] = robot_get_device(sensors_name); distance_sensor_enable(sensors[i], TIME_STEP); if ((i + 1) >= 10) { sensors_name[2] = '1'; sensors_name[3]++; if ((i + 1) == 10) { sensors_name[3] = '0'; sensors_name[4] = (char) '\0'; } } else { sensors_name[2]++; } for (j = 0; j < 2; j++) { matrix[i][j] = khepera3_matrix[i][j]; } } // Enable the Connectors for (i = 0; i< MAX_CONNECTOR_NUMBER; i++) { // Construct the names of connectors sprintf(connector_name, "con%d", i+1); //robot_console_printf("connector %d: %s \n", i, connector_name); // Activate the connectors connectors[i] = robot_get_device(connector_name); connector_enable_presence(connectors[i], TIME_STEP); // Internal state connector_status[i] = UNLOCKED; } // // connectors[0] = robot_get_device("con1"); // connectors[1] = robot_get_device("con2"); // connector_enable_presence(connectors[0], TIME_STEP); // connector_enable_presence(connectors[1], TIME_STEP); // // for (i = 0; i< MAX_CONNECTOR_NUMBER; i++) { // connector_status[i] = UNLOCKED; // } // Basic speed at 0 speed_bias[0] = INITIAL_SPEED; speed_bias[1] = INITIAL_SPEED; // Enable keyboard robot_keyboard_enable(TIME_STEP); // Enable the Arm servo arm_servo = robot_get_device("arm_servo"); servo_enable_position(arm_servo, TIME_STEP); //robot_console_printf("The %s robot is reset, it uses %d sensors\n",robot_name, sensor_number); // Enable the emitter/receiver, gps ------------% commEmitter = robot_get_device("rs232_out"); commReceiver = robot_get_device("rs232_in"); gps = robot_get_device("gps"); /* If the channel is not the good one, we change it. */ channel = emitter_get_channel(commEmitter); if (channel != COMMUNICATION_CHANNEL) { emitter_set_channel(commEmitter, COMMUNICATION_CHANNEL); } receiver_enable(commReceiver, TIME_STEP); gps_enable(gps, TIME_STEP); found_robot = 0; return; }