/** Coordinator/Simulator/Scheduler Main loop of top most process */ void coordinator( void ) { initialize_timer_pipes(); // create the timer for controller quantum management int result = create_timer_controller(); if( result != 0 ) { //throw std::runtime_error( "Failed to open timercontroller_to_coordinator pipe." ) ; printf( "ERROR: Failed to create controller process timer: code %d\n", result ); return; // Note: Returning here leaves the child processes running! } char input_buffer; char output_buffer; // Suspend both to begin kill( controller_pid, SIGSTOP ); //kill( dynamics_pid, SIGSTOP ); // 0 read channel, 1 write channel // coordinator uses only the read from controller channel on this pipe close( fd_controller_to_coordinator[1] ); // coordinator uses only the write to controller channel on this pipe close( fd_coordinator_to_controller[0] ); bool simulating = false; bool controlling = true; int delay; while( 1 ) { // block waiting for controller to complete computation //read( fd_timercontroller_to_coordinator[0], &input_buffer, 1 ); struct timeval tv_timenow; struct timespec ts; gettimeofday( &tv_timenow, NULL ); clock_gettime( controller_clock, &ts ); printf( "Controller Quantum has expired: %d\n", ts.tv_sec ); /* if ( read( fd_timercontroller_to_coordinator[0], &input_buffer, 1 ) > 0 ) { // must determine the time differential at this point. If controller // ahead of dynamics then suspend controller if controller behind // dynamics, controller needs to catch up if( _DEBUG_TO_STDOUT ) { printf( "Controller Quantum has expired\n" ); } // for now brute force suspend //suspend_controller(); // and suspend the timer // if x the resume controller where x is whether dynamics and controller are synch'd enough } */ /* if (read(fd_sensor_to_coordinator[0], &input_buffer, 1) > 0) { // Sensor has published a state if( _DEBUG_TO_STDOUT ) { printf("Sensor has published a message\n"); } } */ run_dynamics( DEFAULT_NSEC_QUANTUM_DYNAMICS ); /* if (read(fd_controller_to_coordinator[0], &input_buffer, 1) > 0) { // Controller has published either (1) a state request or (2) a command if( _DEBUG_TO_STDOUT ) { printf("Controller has published a message\n"); } // This is just to make it send messages through the whole cycle //write(fd_coordinator_to_dynamics[1], &output_buffer, 1); } */ // These are prototypes. Don't really need this code but used to validate process control delay = 1; if( controlling ) { // Continue the controller kill( controller_pid, SIGCONT ); // DO STUFF - ARBITRARY for( int i = 1; i < 1000; i++ ) delay *= i; // Suspend the controller kill( controller_pid, SIGSTOP ); controlling = false; simulating = true; } /* delay = 1; if( simulating ) { // Continue the dynamics kill( dynamics_pid, SIGCONT ); // DO STUFF - ARBITRARY for( int i = 1; i < 1000; i++ ) delay *= i; // Suspend the dynamics kill( dynamics_pid, SIGSTOP ); controlling = true; simulating = false; } */ } // cleanup - unreachable but noted for now close( fd_controller_to_coordinator[0] ); close( fd_coordinator_to_controller[1] ); }
int main(int argc, char **argv) { if (argc < 2) { printf("%s: Invalid parms\n", argv[0]); printf("usage: \n"); printf(" %s Tracker0@host\n", argv[0]); return -1; } char *server = argv[1]; pos *phan_position = new pos; pos *phan_offset = new pos; force *phan_force = new force; state *atom_state = new state; int ff_enabled, ff_active; double kspr = 500.0; // Units??? vrpn_Tracker_Remote *tkr; vrpn_Button_Remote *btn; vrpn_ForceDevice_Remote *fdv; printf("Opening: %s ...\n", server); tkr = new vrpn_Tracker_Remote(server); tkr->register_change_handler(phan_position, handle_tracker); btn = new vrpn_Button_Remote(server); btn->register_change_handler(&ff_enabled, handle_button); fdv = new vrpn_ForceDevice_Remote(server); fdv->register_force_change_handler(phan_force, handle_force); void * myglwin = glwin_create(700, 700); if (myglwin == NULL) { printf("Failed to open OpenGL window!!\n"); return -1; } atom_state->x = 0.0; atom_state->y = 0.0; atom_state->z = 0.0; atom_state->vx = 0.0; atom_state->vy = 0.0; atom_state->vz = 0.0; atom_state->ax = 0.0; atom_state->ay = 0.0; atom_state->az = 0.0; atom_state->mass = .001; ff_active = 0; init_graphics(myglwin); GLUquadricObj *qobj = gluNewQuadric(); GLUquadricObj *qatom = gluNewQuadric(); /* * main interactive loop */ while (1) { // Let the tracker do its thing tkr->mainloop(); btn->mainloop(); fdv->mainloop(); run_dynamics(atom_state, phan_force); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); if (ff_enabled ) { if (!ff_active) { // Set up force field so initially the force is zero phan_offset->x = phan_position->x - .2*atom_state->x; phan_offset->y = phan_position->y - .2*atom_state->y; phan_offset->z = phan_position->z - .2*atom_state->z; } // Now turn the force field on // scene -> haptic: rotate 180 about the y axis fdv->setConstraintMode(vrpn_ForceDevice::POINT_CONSTRAINT); float cpos[3]; cpos[0] = -(.2*atom_state->x + phan_offset->x); cpos[1] = (.2*atom_state->y + phan_offset->y); cpos[2] = -(.2*atom_state->z + phan_offset->z); fdv->setConstraintPoint(cpos); fdv->setConstraintKSpring(kspr); fdv->enableConstraint(1); // enable force field ff_active = 1; } else if (ff_active) { fdv->enableConstraint(0); // disable force field ff_active = 0; } draw_axes(); draw_tracker_and_atom(phan_force, qobj, atom_state, qatom); glwin_swap_buffers(myglwin); } } /* main */