diff --git a/lib/fob/include/FobAlone.h b/lib/fob/include/FobAlone.h index 028fc89..14eb2c8 100644 --- a/lib/fob/include/FobAlone.h +++ b/lib/fob/include/FobAlone.h @@ -105,6 +105,14 @@ FLOCK */ void InitSlave(); +/* + Initialisation du ou des fob slave en mode FLOCK avec une connexion + RS232 seulement sur le master FLOCK (appeler InitMaster sur le + maître) +*/ +void InitSlaveUnSeulRS232(int numero_slave); + + /*InitMaster Initialisation du Fob maitre en mode FLOCK avec une connection RS232 par FLOCK diff --git a/lib/fob/lib/libfob.a b/lib/fob/lib/libfob.a index a253a86..272ef7d 100644 Binary files a/lib/fob/lib/libfob.a and b/lib/fob/lib/libfob.a differ diff --git a/lib/fob/src/FobAlone.C b/lib/fob/src/FobAlone.C index 20e035f..7c59476 100644 --- a/lib/fob/src/FobAlone.C +++ b/lib/fob/src/FobAlone.C @@ -59,12 +59,46 @@ FobAlone::InitSlave() } void +FobAlone::InitSlaveUnSeulRS232(int numero_slave) +{ + // On reprend le code de Init mais on précise qu'on parle à + // l'esclave à chaque fois + + // on se place dans le modele + // graphique de base x a droite y vers le haut z vers nous + // cerr<<"set position reference frame"<nb_flock = nb_flock; commande[0]=B_CHANGE_VALUE; commande[1]=BV_AUTOC; commande[2]=nb_flock; Put(commande,3); + sleep(1); } void diff --git a/lib/fob/src/FobCommand.h b/lib/fob/src/FobCommand.h index 1aae1ad..ca83b05 100644 --- a/lib/fob/src/FobCommand.h +++ b/lib/fob/src/FobCommand.h @@ -14,7 +14,8 @@ // de facon geometrique : elle repond juste a l'implementation bas niveau de la gestion du fob. // rem : commentaire a refaire -// actuellement le fob est fait pour marcher en standalone +// actuellement le fob est fait pour marcher en standalone et en mode +// master/slave avec seulement le master branché en RS232 // optimisation effectuer :lire la derniere valeur :fait plus haut // Distribution : @@ -23,16 +24,20 @@ // // // Todo : -// O Commentaire a refaire +// O Commentaire a refaire // O constante a modifier // O modifier la classe -// ou on rajouter pour permettre la connection de plusieurs fob. +// ou on rajouter pour permettre la connexion de plusieurs fob. // 0 ajouter timestamp // / // X // // History : // 1999/03/16 : Mr Grasset : Create of the file +// 2010/01/28 : Konubinix : Correction : SetAutoConfiguration +// needs at least 600ms of sleep before and after the command is +// sent via RS232. The attribute nb_flock was not updated when +// the method was called /////////////////////////////////////////////////////////////////////////////// #if defined FobCommand_CYCLE diff --git a/src/ApiArv/Fob.cpp b/src/ApiArv/Fob.cpp index 9f0c32d..ac6bcd9 100644 --- a/src/ApiArv/Fob.cpp +++ b/src/ApiArv/Fob.cpp @@ -17,66 +17,150 @@ #include #include #include +#include namespace arv { /******************************************************************************************/ -Fob_HData toFobHemi(Fob::Hemisphere hemi) { - switch (hemi) { - case Fob::HEMI_FORWARD: - return FORWARD; - break; - case Fob::HEMI_BACKWARD: - return AFT; - break; - case Fob::HEMI_UPPER: - return UPPER; - break; - case Fob::HEMI_LOWER: - return LOWER; - break; - case Fob::HEMI_LEFT : - return LEFT; - break; - case Fob::HEMI_RIGHT: - return RIGHT; - break; - default: - return FORWARD; - } - } + Fob_HData toFobHemi(Fob::Hemisphere hemi) { + switch (hemi) { + case Fob::HEMI_FORWARD: + return FORWARD; + break; + case Fob::HEMI_BACKWARD: + return AFT; + break; + case Fob::HEMI_UPPER: + return UPPER; + break; + case Fob::HEMI_LOWER: + return LOWER; + break; + case Fob::HEMI_LEFT : + return LEFT; + break; + case Fob::HEMI_RIGHT: + return RIGHT; + break; + default: + return FORWARD; + } + } /******************************************************************************************/ -Fob::Fob(Hemisphere hemisphere, SerialPort port) { - fob = new FobAlone(POSITION_QUATERNION, toFobHemi(hemisphere), getSystemName(port), CONTROL_SOFT, B9600); - } + Fob::Fob(Hemisphere hemisphere, SerialPort port) { + fob = new FobAlone(POSITION_QUATERNION, toFobHemi(hemisphere), getSystemName(port), CONTROL_SOFT, B9600); + } -void Fob::init() { - fob->InitAlone(); - } + void Fob::init(unsigned int number_of_birds) { + if ( number_of_birds == 0 ){ + fob->InitAlone(); + positions_.resize(1); + quaternions_.resize(1); + }else{ + // Initialisation of the master + fob->InitMaster(number_of_birds); + // Initialisation of the slaves + for ( unsigned int i = 2 ; i <= number_of_birds ; i++){ + fob->InitSlaveUnSeulRS232(i); + } + positions_.resize(number_of_birds); + quaternions_.resize(number_of_birds); + } + this->number_of_birds_ = number_of_birds; + } + + void Fob::close() { + } -void Fob::close() { - } + void Fob::update() { + // updates all the birds + if ( number_of_birds_ == 0 ){ + // StandAlone mode + update(0); + }else{ + // Master/Slave mode + for ( unsigned int i = 1 ; i <= number_of_birds_ ; i ++ ){ + update(i); + } + } + } + + void Fob::update(unsigned int bird_number){ + float quat[4]; + float pos[3]; + if ( number_of_birds_ > 0 ){ + // Def prog + assert((bird_number > 0) && (bird_number <= number_of_birds_)); + // Assume that GetPosQuaternion does only one + // RS232 request. If it does not, the ToFbb + // command is to launch before all the RS232 + // commands + fob->ToFbb(bird_number); + }else{ + // A bit tricky. When you are in StandAlone + // Mode, the master (the only bird) bird is at + // address 0, its number in this module is + // 0. When you are in Master/Slave mode, its + // address (and thus its number) is + // 1. Therefore, to get the same number for + // the master in the vectors of positions and + // quaternions, I put its number to 1. + bird_number++; + } + fob->GetPosQuaternion(pos, quat); + // Don't forget the birds range is from 1 to + // number_of_birds_ but Vector range is from 0 to + // number_of_birds_-1 + qglviewer::Vec& position = positions_[bird_number-1]; + qglviewer::Quaternion& quaternion = quaternions_[bird_number-1]; + for (int i=0; i<4; i++) quaternion[i] = quat[i]; // necessary because is a double[4] + for (int i=0; i<3; i++) position[i] = pos[i]; + } + + qglviewer::Vec Fob::position(unsigned int bird_number) const { + if ( number_of_birds_ > 0 ){ + assert((bird_number > 0) && ( bird_number <= number_of_birds_ )); + }else{ + assert(bird_number==0); + // Same trick as described in the + // update(unsigned int) method to have the + // range of birds into 0..number_of_birds_-1 + // whatever the situation is + bird_number++; + } + return positions_[bird_number-1]; + } -void Fob::update() { - float quat[4]; - fob->GetPosQuaternion(p, quat); - for (int i=0; i<4; i++) q[i] = quat[i]; // necessary because is a double[4] - } - -qglviewer::Vec Fob::position() const { - return p; - } - -qglviewer::Quaternion Fob::orientation() const { - return q; - } - -qglviewer::Frame Fob::transform() const { - return qglviewer::Frame(p, q); - } + qglviewer::Quaternion Fob::orientation(unsigned int bird_number) const { + if ( number_of_birds_ > 0 ){ + assert((bird_number > 0) && ( bird_number <= number_of_birds_ )); + }else{ + assert(bird_number==0); + // Same trick as described in the + // update(unsigned int) method to have the + // range of birds into 0..number_of_birds_-1 + // whatever the situation is + bird_number++; + } + return quaternions_[bird_number-1]; + } + qglviewer::Frame Fob::transform(unsigned int bird_number) const { + if ( number_of_birds_ > 0 ){ + assert((bird_number > 0) && ( bird_number <= number_of_birds_ )); + }else{ + assert(bird_number==0); + // Same trick as described in the + // update(unsigned int) method to have the + // range of birds into 0..number_of_birds_-1 + // whatever the situation is + bird_number++; + } + return qglviewer::Frame(positions_[bird_number-1], quaternions_[bird_number-1]); + } + } diff --git a/src/ApiArv/Fob.h b/src/ApiArv/Fob.h index ca99e1d..b336c72 100644 --- a/src/ApiArv/Fob.h +++ b/src/ApiArv/Fob.h @@ -19,63 +19,75 @@ #include #include #include +#include #include "SerialPort.h" class FobAlone; namespace arv { - /** Interface for the Flock of Birds tracker. - * The Flock of Birds (FOB) delivers 6DOF data from a tracker (their) with real-time rates. - * The bird needs a specific hemisphere to operate, and it needs to be positionned - * in this hemisphere at startup to function correctly. - */ - class Fob { + /** Interface for the Flock of Birds tracker. + * The Flock of Birds (FOB) delivers 6DOF data from a tracker (their) with real-time rates. + * The bird needs a specific hemisphere to operate, and it needs to be positionned + * in this hemisphere at startup to function correctly. + */ + class Fob { public: - /** This type represents hemispheres of the FOB magnetic transmitter. */ - enum Hemisphere { HEMI_FORWARD, HEMI_BACKWARD, HEMI_UPPER, HEMI_LOWER, HEMI_LEFT, HEMI_RIGHT }; + /** This type represents hemispheres of the FOB magnetic transmitter. */ + enum Hemisphere { HEMI_FORWARD, HEMI_BACKWARD, HEMI_UPPER, HEMI_LOWER, HEMI_LEFT, HEMI_RIGHT }; public: - Fob(Hemisphere hemisphere = HEMI_UPPER, SerialPort port = PORT0); + Fob(Hemisphere hemisphere = HEMI_UPPER, SerialPort port = PORT0); /**< Create a new instance. - @param hemisphere the hemisphere where the bird will fly. - @param port the serial port into which the FOB is plugged. - */ + @param hemisphere the hemisphere where the bird will fly. + @param port the serial port into which the FOB is plugged. + */ - void init(); + void init(unsigned int number_of_birds=0); /**< Initializes communication with the FOB. - */ + + \param number_of_birds: If 0, run in StandAlone + mode, else run in Master/Slaves mode with only + communication with the master via RS232 port (thus, + number_of_birds-1 slaves). + */ - void close(); + void close(); /**< Closes communication with the FOB. */ - void update(); - /**< Updates bird's state from its physical position and orientation. + void update(); + /**< Updates birds's state from their physical positions and orientations. */ - - qglviewer::Vec position() const; + + void update(unsigned int bird_number); + /**< Update the bird of number bird_number, starting + * from 1 to number_of_birds (= the number given in the init) + */ + + qglviewer::Vec position(unsigned int bird_number=0) const; /**< Gets the bird's position. - @return the position of the bird during last update. - */ + @return the position of the bird during last update. + */ - qglviewer::Quaternion orientation() const; + qglviewer::Quaternion orientation(unsigned int bird_number=0) const; /**< Gets the bird's orientation. - @return the orientation of the bird during last update. - */ + @return the orientation of the bird during last update. + */ - qglviewer::Frame transform() const; + qglviewer::Frame transform(unsigned int bird_number=0) const; /**< Gets the bird's transform. - @return the transfomation defined by position and orientation of the bird during last update. - */ + @return the transfomation defined by position and orientation of the bird during last update. + */ private: - FobAlone* fob; - qglviewer::Vec p; // the bird's position - qglviewer::Quaternion q; // the bird's orientation + FobAlone* fob; + std::vector positions_; // the bird's position + std::vector quaternions_; // the bird's orientation + unsigned int number_of_birds_; // The number of birds used }; - } +} #endif diff --git a/tests/fob/test3.cpp b/tests/fob/test3.cpp new file mode 100644 index 0000000..896e3b9 --- /dev/null +++ b/tests/fob/test3.cpp @@ -0,0 +1,128 @@ +/********************************************************************** + * \file test3.cpp + * + * test de deux fob en mode master slave + * + * \author Konubinix (konubinix@gmail.com) + * \date jeu 11:56:00 28/01/2010 + ***********************************************************************/ +#include +#include +#include +#include + +using namespace arv; +using namespace qglviewer; + +Fob *fob; + +void display(void) { + GLfloat mat_ambient[] = {0.5, 0.5, 0.5, 1.0}; + GLfloat mat_flash[] = {0.0, 0.0, 1.0, 1.0}; + GLfloat mat_flash_shiny[] = {50.0}; + GLfloat light_position[] = {100.0, -200.0, 200.0, 0.0}; + GLfloat ambi[] = {0.1, 0.1, 0.1, 0.1}; + GLfloat lightZeroColor[] = {0.9, 0.9, 0.9, 0.1}; + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + + /* Setup the view of the cube. */ + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + gluPerspective( /* field of view in degree */ 40.0, + /* aspect ratio */ 1.0, + /* Z near */ 1.0, + /* Z far */ 200.0); + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + gluLookAt(0.0, 0.0, 90.0, /* eye is at (0,0,5) */ + 0.0, 0.0, 0.0, /* center is at (0,0,0) */ + 0.0, 1.0, 0.); /* up is in positive Y direction */ + + + glEnable(GL_LIGHTING); + glEnable(GL_LIGHT0); + glLightfv(GL_LIGHT0, GL_POSITION, light_position); + glLightfv(GL_LIGHT0, GL_AMBIENT, ambi); + glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor); + glMaterialfv(GL_FRONT, GL_SPECULAR, mat_flash); + glMaterialfv(GL_FRONT, GL_SHININESS, mat_flash_shiny); + + glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient); + + // the fob base : + glutSolidCube(8.0); + + fob->update(); + + Vec p; + Quaternion q; + + glPushMatrix(); + p = fob->position(1); + q = fob->orientation(1); + glTranslatef(p.x, p.y, p.z); + glMultMatrixd(q.matrix()); + glScalef (2.0f, 1.0f, 4.0f); + glutSolidCube(2.0); + glPopMatrix(); + + glPushMatrix(); + p = fob->position(2); + q = fob->orientation(2); + glTranslatef(p.x, p.y, p.z); + glMultMatrixd(q.matrix()); + glScalef (2.0f, 1.0f, 4.0f); + glutSolidCube(2.0); + glPopMatrix(); + + // or : + // glMultMatrixd(fob->transform().matrix()); + + // the bird : + glutSwapBuffers(); +} + +void keyboard(unsigned char key, int /*x*/, int /*y*/) { + if ((key == 27) || (key == 'q') || (key == 'Q')) { + fprintf(stderr, "BYE\n"); + exit(0); + } +} + +int main(int argc, char* argv[]) { + std::cout << "Test de 2 fobs en mode master/slaves" << std::endl; + std::cout << "Le master doit être branché sur /dev/ttyS1, le slave doit être branché sur le master en FBB (RJ45?)" << std::endl; + std::cout << "Le master doit être d'adresse 1 (dipswitch à 011000010 (=UP DOWN DOWN UP UP UP UP DOWN UP))" << std::endl; + std::cout << "Le slave doit être d'adresse 2 (dipswitch à 011000100 (=UP DOWN DOWN UP UP UP DOWN UP UP))" << std::endl; + fprintf(stderr, "Options: \n"); + fprintf(stderr, " -f : for fullscreen.\n"); + fprintf(stderr, "Press Esc or q to quit.\n"); + + bool fullScreen = false; + for (int i = 1; i < argc; i++) { + if (strcmp("-f", argv[i]) == 0) + fullScreen = true; + else + fprintf(stderr, "%s is not a supported option of %s.\n", argv[i], argv[0]); + } + + glutInit(&argc, argv); + glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH); + glutInitWindowPosition(0, 0); + glutInitWindowSize(640, 480); + glutCreateWindow("ARV Fob GL Test"); + if (fullScreen) + glutFullScreen(); + glutDisplayFunc(display); + glutIdleFunc(display); + glutKeyboardFunc(keyboard); + + fob = new Fob(Fob::HEMI_UPPER); + fob->init(2); + + glutMainLoop(); + return 0; +} + diff --git a/tests/fob/test3.pro b/tests/fob/test3.pro new file mode 100644 index 0000000..e2d9900 --- /dev/null +++ b/tests/fob/test3.pro @@ -0,0 +1,8 @@ +TEMPLATE = app +TARGET = FobTest3 +CONFIG *= warn_on release arv glut + +ARV = fob +SOURCES = test3.cpp +OBJECTS_DIR = .obj +