int main( int argc, char* argv[] ) { bool stopped = false; const char *title = "SDLController"; // Initialize SDL and the window if( ! sdl_init( title ) ) { printf("Error: could not initialise!\n"); return 1; } yarp::os::Property command; command.fromCommand(argc, argv); if( ! yarp_init(command) ) { printf("Error: could not initialise YARP!\n"); return 1; } //Make the dot Dot aDot(0, "dota.bmp"); Dot bDot(2, "dotb.bmp"); sdl_add_object(&aDot); sdl_add_object(&bDot); // while we are still running while( !stopped ) { aDot.center(); bDot.center(); // while there's events to handle while( SDL_PollEvent( &event ) ) { //Handle events for the dot aDot.handle_input(event); bDot.handle_input(event); // the user has closed the window if( event.type == SDL_QUIT ) { // Stop the program stopped = true; } } //Move the dots aDot.move_yarp(); bDot.move_yarp(); sdl_update_screen(); SDL_Delay(MIN_CONTROLTIME); } //Clean up yarp_cleanup(); sdl_cleanup(); return 0; }
int main(int argc, char *argv[]) { // Buffer to store sound. // Make sure this is big enough for your case, or change it to be // allocated dynamically. unsigned char buf[16384*2*4]; // Buffer to store sound properties unsigned char properties[1000]; yarpAddress addr; yarpConnection con; int res; if (argc!=2) { printf("Call as:\n %s /port/to/read/from\n", argv[0]); exit(1); } yarp_init(); res = yarp_port_lookup(&addr,argv[1]); if (res<0) { printf("Failed to find port\n"); exit(1); } fprintf(stderr,"Connecting to %s:%d\n", addr.host, addr.port_number); con = yarp_prepare_to_read_binary(&addr); if (!yarp_is_valid(con)) { printf("Connection failed\n"); exit(1); } while (res>=0) { int total = yarp_receive_data_header(con); if (total>=0) { // YARP sound format is just like images (see yarpreadimage) // EXCEPT: // There is an extra header, declaring the image and a // small extra Bottle holding sound details like the // sampling frequency. unsigned char preheader[4*2]; // declares header and footer unsigned char header[4*15]; // image format unsigned char footer[4*3]; // bottle format res = yarp_receive_binary(con,(char*)preheader,8); if (res<0) { printf("Failed to read sound initial header\n"); exit(1); } // get image header, see YARPImagePortContentHeader // class in src/libYARP_sig/src/Image.cpp int i; res = yarp_receive_binary(con,(char*)header,sizeof(header)); if (res<0) { printf("Failed to read sound 'image' header\n"); exit(1); } char format[5] = {0,0,0,0,0}; for (i=0; i<4; i++) { format[i] = header[4*5+i]; } int depth = yarp_read_int(header+4*8,4); // header.depth int width = yarp_read_int(header+4*11,4); // header.width int height = yarp_read_int(header+4*12,4); // header.height int image_len = total-sizeof(preheader)-sizeof(header)- sizeof(footer); if (image_len>sizeof(buf)) { printf("Image too big to store, increase buffer size...\n"); exit(1); } res = yarp_receive_binary(con,(char*)buf,image_len); // process the buffer as you wish... res = yarp_receive_binary(con,(char*)footer,sizeof(footer)); if (res<0) { printf("Failed to read sound footer\n"); exit(1); } int freq = yarp_read_int(footer+4*2,4); printf("Received sound, size %dx%d, sample size %d, format %s, frequency %d\n", width, height, depth, format, freq); if (image_len!=width*height*depth) { printf("Sound may have padding, yarpreadsound.c needs to be updated to deal with that (or use \n"); exit(1); } } } yarp_disconnect(con); yarp_fini(); return 0; }
/* * Simulation initialization */ Loop_inputs* init_simulation() { // -- Variables declaration -- // int i; int model_state_size; double *ystart; MBSdataStruct *MBSdata = NULL; LocalDataStruct *lds = NULL; Loop_inputs *loop_inputs = NULL; #if defined(JNI) & defined (REAL_TIME) JNI_struct* jni_struct = NULL; #endif #ifdef WRITE_FILES Write_files *write_files; #endif #ifdef YARP void* RobotranYarp_interface = NULL; #endif struct timeval seed_time; // -- Seed for random -- // gettimeofday(&seed_time, NULL); srand(seed_time.tv_usec * seed_time.tv_sec); // -- Variables initialization -- // MBSdata = loadMBSdata_xml(MBS_FILE); if(MBSdata == NULL) { printf("error while loading MBSdata \n"); } #ifdef PRINT_REPORT else { printf("MBSdata successfully loaded \n"); } #endif // LocalDataStruct initialization #if !defined ACCELRED lds = initLocalDataStruct(MBSdata); #else lds = NULL; #endif if(lds == NULL) { printf("error while initializing LocalDataStruct\n"); } #ifdef PRINT_REPORT else { printf("LocalDataStruct successfully initialized\n"); } #endif // Yarp Initialization #ifdef YARP RobotranYarp_interface = yarp_init(); if(RobotranYarp_interface == NULL) { printf("******************\nSomething went wrong during YARP initialization... what to do here?\n******************\n"); } #endif // Model initialization if(user_initialization(MBSdata, lds)) { printf("error in user_initialization\n"); } #ifdef PRINT_REPORT else { printf("Model successfully initialized\n"); } #endif // Integrator parameters initialization model_state_size = 2*MBSdata->nqu + MBSdata->Nux; #ifdef PRINT_REPORT printf("model_state_size: %d\n", model_state_size); #endif ystart = dvector(1,model_state_size); #ifdef WRITE_FILES write_files = init_write_files(NB_SIMU_STEPS, MBSdata->njoint); #endif // Simulation state initialization for(i=1; i<=MBSdata->nqu; i++) { ystart[i] = MBSdata->q[MBSdata->qu[i]]; ystart[i+MBSdata->nqu] = MBSdata->qd[MBSdata->qu[i]]; } for(i=1; i<=MBSdata->Nux; i++) { ystart[i+2*MBSdata->nqu] = MBSdata->ux[i]; } // JNI visualization #if defined(JNI) & defined (REAL_TIME) jni_struct = init_jni(MBSdata); #endif // initialize the inputs of the simulation loop loop_inputs = (Loop_inputs*) malloc(sizeof(Loop_inputs)); // absolute initial time of the simulation time_get(&(loop_inputs->init_t_sec), &(loop_inputs->init_t_usec)); loop_inputs->nvar = model_state_size; loop_inputs->nstep = NB_SIMU_STEPS; loop_inputs->x1 = TSIM_INIT; loop_inputs->x2 = TSIM_END; loop_inputs->ystart = ystart; loop_inputs->lds = lds; loop_inputs->MBSdata = MBSdata; #ifdef WRITE_FILES loop_inputs->write_files = write_files; #endif // Real-time constraiints #ifdef REAL_TIME loop_inputs->real_time = init_real_time(loop_inputs->init_t_sec, loop_inputs->init_t_usec); #endif // SDL window #if defined(SDL) & defined (REAL_TIME) loop_inputs->screen_sdl = configure_screen_sdl(loop_inputs->init_t_sec, loop_inputs->init_t_usec); #endif #if defined(JNI) & defined (REAL_TIME) loop_inputs->jni_struct = jni_struct; update_jni(loop_inputs->jni_struct, MBSdata, loop_inputs->real_time, MBSdata->q+1); #endif #ifdef YARP loop_inputs->RobotranYarp_interface = RobotranYarp_interface; #endif // Running model integration #ifdef PRINT_REPORT printf("Running integration of the model...\n"); #endif return loop_inputs; }