/**
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 */
