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