ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpServolens.cpp
1 /****************************************************************************
2  *
3  * $Id: vpServolens.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface for the Servolens lens attached to the camera fixed on the
36  * Afma4 robot.
37  *
38  * Authors:
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
52 #if defined(UNIX)
53 
54 #include <unistd.h>
55 #include <termios.h>
56 #include <stdio.h>
57 #include <sys/types.h>
58 #include <sys/stat.h>
59 #include <fcntl.h>
60 #include <stdlib.h>
61 #include <string.h>
62 
63 #include <visp/vpServolens.h>
64 #include <visp/vpRobotException.h>
65 #include <visp/vpDebug.h>
66 #include <visp/vpTime.h>
67 
75 {
76  isinit = false;
77 }
78 
87 vpServolens::vpServolens(const char *port)
88 {
89  isinit = false;
90 
91  this->open(port);
92 }
93 
102 {
103  this->close();
104 }
105 
120 void
121 vpServolens::open(const char *port)
122 {
123  if (! isinit) {
124  struct termios info;
125 
126  printf("\nOpen the Servolens serial port \"%s\"\n", port);
127 
128  if ((this->remfd=::open(port, O_RDWR|O_NONBLOCK)) < 0) {
129  vpERROR_TRACE ("Cannot open Servolens serial port.");
131  "Cannot open Servolens serial port.");
132  }
133 
134  // Lecture des paramètres courants de la liaison série.
135  if (tcgetattr(this->remfd, &info) < 0) {
136  ::close(this->remfd);
137  vpERROR_TRACE ("Error using TCGETS in ioctl.");
139  "Error using TCGETS in ioctl");
140  }
141 
142  //
143  // Configuration de la liaison serie:
144  // 9600 bauds, 1 bit de stop, parite paire, 7 bits de donnee
145  //
146 
147  // Traitement sur les caractères recus
148  info.c_iflag = 0;
149  info.c_iflag |= INLCR;
150 
151  // Traitement sur les caractères envoyés sur la RS232.
152  info.c_oflag = 0; // idem
153 
154  // Traitement des lignes
155  info.c_lflag = 0;
156 
157  // Controle materiel de la liaison
158  info.c_cflag = 0;
159  info.c_cflag |= CREAD; // Validation reception
160  info.c_cflag |= B9600 | CS7 | PARENB; // 9600 baus, 7 data, parite paire
161 
162  // Caractères immédiatement disponibles.
163  // info.c_cc[VMIN] = 1;
164  // info.c_cc[VTIME] = 0;
165 
166  if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
167  ::close(this->remfd);
168  vpERROR_TRACE ("Error using TCGETS in ioctl.");
170  "Error using TCGETS in ioctl");
171  }
172 
173  // Supprime tous les caracteres recus mais non encore lus par read()
174  tcflush(this->remfd, TCIFLUSH);
175 
176  isinit = true;
177 
178  this->init();
179 
180  // Try to get the position of the zoom to check if the lens is really connected
181  unsigned int izoom;
182  if (this->getPosition(vpServolens::ZOOM, izoom) == false) {
183  vpERROR_TRACE ("Cannot dial with the servolens. Check if the serial link is connected.");
185  "Cannot dial with the servolens. Check if the serial link is connected.");
186 
187  }
188 
189  }
190 }
191 
196 void
198 {
199  if (isinit) {
200  printf("\nClose the serial connexion with Servolens\n");
201  ::close(this->remfd);
202  isinit = false;
203  }
204 }
205 
212 void
214 {
215  if (!isinit) {
216  vpERROR_TRACE ("Cannot dial with Servolens.");
218  "Cannot dial with Servolens.");
219  }
220  char commande[10];
221 
222  /* suppression de l'echo */
223  sprintf(commande, "SE1");
224  this->write(commande);
225 
226  /* initialisation de l'objectif, idem qu'a la mise sous tension */
227  sprintf(commande, "SR0");
228  this->write(commande);
229 
230  vpTime::wait(25000);
231 
232  this->wait();
233 
234  /* suppression de l'echo */
235  sprintf(commande, "SE0");
236  this->write(commande);
237 
238  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
239  sprintf(commande, "VW0");
240  this->write(commande);
241 }
250 void
251 vpServolens::init()
252 {
253  if (!isinit) {
254  vpERROR_TRACE ("Cannot dial with Servolens.");
256  "Cannot dial with Servolens.");
257  }
258 
259  char commande[10];
260 
261  /* suppression de l'echo */
262  sprintf(commande, "SE0");
263  this->write(commande);
264 
265  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
266  sprintf(commande, "VW0");
267  this->write(commande);
268 
269  /* L'experience montre qu'une petite tempo est utile. */
270  vpTime::wait(500);
271 }
272 
285 void
286 vpServolens::enableCmdComplete(vpServoType servo, bool active)
287 {
288  if (!isinit) {
289  vpERROR_TRACE ("Cannot dial with Servolens.");
291  "Cannot dial with Servolens.");
292  }
293  char commande[10];
294 
295  /* Envoie une commande pour qu'en fin de mouvement servolens renvoie
296  * une information de fin de mouvement (ex: ZF, FF, DF).
297  */
298  switch(servo) {
299  case ZOOM:
300  if (active)
301  sprintf(commande, "ZF1");
302  else
303  sprintf(commande, "ZF0");
304  break;
305  case FOCUS:
306  if (active)
307  sprintf(commande, "FF1");
308  else
309  sprintf(commande, "FF0");
310  break;
311  case IRIS:
312  if (active)
313  sprintf(commande, "DF1");
314  else
315  sprintf(commande, "DF0");
316  break;
317  }
318 
319  /* envoie de la commande */
320  this->write(commande); /* a la fin du mouvement envoie de ZF, FF, DF */
321 }
322 
333 void
335 {
336  if (!isinit) {
337  vpERROR_TRACE ("Cannot dial with Servolens.");
339  "Cannot dial with Servolens.");
340  }
341  char commande[10];
342 
343  /* suppression de l'echo */
344  if (active == true)
345  sprintf(commande, "SE1");
346  else
347  sprintf(commande, "SE0");
348 
349  this->write(commande);
350 }
351 
360 void
362 {
363  if (!isinit) {
364  vpERROR_TRACE ("Cannot dial with Servolens.");
366  "Cannot dial with Servolens.");
367  }
368  char commande[10];
369 
370  switch(controller) {
371  case AUTO:
372  /* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
373  sprintf(commande, "VW1");
374  this->write(commande);
375  break;
376  case CONTROLLED:
377  /* nettoyage : mot d'etat vide 0000 */
378  sprintf(commande,"SX0842");
379  this->write(commande);
380  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
381  sprintf(commande, "VW0");
382  this->write(commande);
383  break;
384  case RELEASED:
385  sprintf(commande,"SX1084");
386  this->write(commande);
387  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
388  sprintf(commande, "VW0");
389  this->write(commande);
390  break;
391  }
392 
393 }
394 
401 void
403 {
404  if (!isinit) {
405  vpERROR_TRACE ("Cannot dial with Servolens.");
407  "Cannot dial with Servolens.");
408  }
409  char commande[10];
410 
411  if (enable)
412  sprintf(commande, "DA1");
413  else
414  sprintf(commande, "DA0");
415 
416  this->write(commande);
417 }
418 
429 void
430 vpServolens::setPosition(vpServoType servo, unsigned int position)
431 {
432  if (!isinit) {
433  vpERROR_TRACE ("Cannot dial with Servolens.");
435  "Cannot dial with Servolens.");
436  }
437  char commande[10];
438 
439  /* attente du prompt pour envoyer une commande */
440  /*
441  printf("attente prompt\n");
442  this->wait();
443  */
444 
445 #if FINSERVO
446  /* envoie des commandes pour qu'en fin de mouvement servolens renvoie */
447  /* une commande de fin de mouvement (ex: ZF, FF, DF). */
448  this->enableCommandComplete();
449 #endif /* FINSERVO */
450 
451  // 08/08/00 Fabien S. - Correction de la consigne demandee
452  // pour prendre en compte l'erreur entre la consigne demandée
453  // et la consigne mesurée.
454  // A la consigne du zoom on retranche 1.
455  // A la consigne du focus on ajoute 1.
456  // A la consigne du iris on ajoute 1.
457  switch (servo) {
458  case ZOOM:
459  //printf("zoom demande: %d ", position);
460  position --;
461  if (position < ZOOM_MIN) position = ZOOM_MIN;
462  //printf("zoom corrige: %d \n", position);
463  break;
464  case FOCUS:
465  //printf("focus demande: %d ", position);
466  position ++;
467  if (position > FOCUS_MAX) position = FOCUS_MAX;
468  //printf("focus corrige: %d \n", position);
469  break;
470  case IRIS:
471  // printf("iris demande: %d ", position);
472  position ++;
473  if (position > IRIS_MAX) position = IRIS_MAX;
474  //printf("iris corrige: %s \n", position);
475  break;
476  }
477 
478  /* commande a envoyer aux servomoteurs */
479  switch(servo) {
480  case ZOOM:
481  sprintf(commande, "ZD%d", position);
482  break;
483  case FOCUS:
484  sprintf(commande, "FD%d", position);
485  break;
486  case IRIS:
487  sprintf(commande, "DD%d", position);
488  break;
489  }
490  /* envoie de la commande */
491 #if PRINT
492  printf("\ncommande: %s", commande);
493 #endif
494 
495  this->write(commande);
496 
497 #if FINSERVO
498  /* on attend la fin du mouvement des objectifs */
499  this->wait(servo); /* on attend les codes ZF, FF, DF */
500 #endif
501 }
502 
513 bool
514 vpServolens::getPosition(vpServoType servo, unsigned int &position)
515 {
516  if (!isinit) {
517  vpERROR_TRACE ("Cannot dial with Servolens.");
519  "Cannot dial with Servolens.");
520  }
521  char commande[10];
522  char posit[10], *pt_posit;
523  char c;
524  short fin_lect_posit; /* indique si on a lu la position du servo-moteur */
525  short posit_car; /* donne la position du caractere lu */
526  short lecture_posit_en_cours; /* indique si on lit la position courante */
527 
528  /* attente du prompt pour envoyer une commande */
529  /*
530  this->wait();
531  */
532  pt_posit = posit;
533 
534  /* envoie des commandes pour obtenir la position des servo-moteurs. */
535  switch (servo) {
536  case ZOOM:
537  sprintf(commande, "ZD?");
538  break;
539  case FOCUS:
540  sprintf(commande, "FD?");
541  break;
542  case IRIS:
543  sprintf(commande, "DD?");
544  break;
545  default:
546  break;
547  }
548  /* envoie de la commande */
549  // printf("\ncommande: %s", commande);
550 
551  this->write(commande);
552 
553  /* on cherche a lire la position du servo-moteur */
554  /* Servolens renvoie une chaine de caractere du type ZD00400 ou FD00234 */
555  fin_lect_posit = 0;
556  posit_car = 0;
557  lecture_posit_en_cours = 0;
558  do {
559  if (this->read(&c, 1) == true) {
560 
561  // printf("caractere lu: %c\n", c);
562  switch (posit_car){
563  /* on lit le 1er caractere; (soit Z, soit F, soit D) */
564  case 0:
565  /* sauvegarde du pointeur */
566  pt_posit = posit;
567 
568  switch (servo) {
569  case ZOOM:
570  if( c == 'Z') posit_car = 1;
571  break;
572  case FOCUS:
573  if( c == 'F') posit_car = 1;
574  break;
575  case IRIS:
576  if( c == 'D') posit_car = 1;
577  break;
578  }
579  break;
580 
581  /* si le 1er caractere est correct, on lit le 2eme caractere */
582  /* (toujours D) */
583  case 1:
584  if( c == 'D') posit_car = 2;
585  else posit_car = 0; /* le 2eme caractere n'est pas correct */
586  break;
587 
588  /* si on a lu les 2 premiers caracteres, on peut lire la */
589  /* position du servo-moteur */
590  case 2:
591  if (c >= '0' && c <= '9')
592  {
593  *pt_posit++ = c; /* sauvegarde de la position */
594  lecture_posit_en_cours = 1;
595  }
596  else if (lecture_posit_en_cours)
597  {
598  fin_lect_posit = 1;
599  *pt_posit = '\0';
600  }
601  else posit_car = 0;
602  break;
603  }
604 
605  }
606  else {
607  // Timout sur la lecture, on retoure FALSE
608  return false;
609  }
610  }
611  while ( !fin_lect_posit );
612 
613  // printf("\nChaine lue: posit: %s", posit);
614 
615  /* toilettage de la position courantes lue */
616  this->clean(posit, posit);
617 
618  // printf("\nChaine toilettee: posit: %s", posit);
619  position = (unsigned int)atoi(posit);
620 
621  return(true);
622 }
623 
653 {
654  if (!isinit) {
655  vpERROR_TRACE ("Cannot dial with Servolens.");
657  "Cannot dial with Servolens.");
658  }
659  vpCameraParameters cam;
660  double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
661  double px=1000, py=1000, u0=320, v0=240;
662  // Determine if the image is subsampled.
663  // Dragonfly2 native images are 640 by 480
664  double subsample_factor = 1.;
665  double width = I.getWidth();
666  double height= I.getHeight();
667 
668 
669  if (width > 300 && width < 340 && height > 220 && height < 260)
670  subsample_factor = 2;
671  else if (width > 140 && width < 1800 && height > 100 && height < 140)
672  subsample_factor = 4;
673 
674  unsigned zoom;
676  //std::cout << "Actual zoom value: " << zoom << std::endl;
677 
678  // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale correspondent
679  // aux parametres de calibration de la camera (donnees constructeur) pour des
680  // tailles d'images CCIR (768x576), donc avec scale = 1.
681  double focale = zoom * 1.0e-5; // Transformation en metres
682  px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
683  py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
684  u0 = I.getWidth() / 2.;
685  v0 = I.getHeight() / 2.;
686  cam.initPersProjWithoutDistortion(px, py, u0, v0);
687 
688  return cam;
689 }
690 
699 char
700 vpServolens::wait()
701 {
702  if (!isinit) {
703  vpERROR_TRACE ("Cannot dial with Servolens.");
705  "Cannot dial with Servolens.");
706  }
707 
708  ssize_t r;
709  r = ::write(this->remfd, "\r\n", strlen("\r\n"));
710  if (r != (ssize_t)(strlen("\r\n"))) {
712  "Cannot write on Servolens.");
713  }
714  char c;
715  do {
716  r = ::read(this->remfd, &c, 1);
717  c &= 0x7f;
718  }
719  while (c != '>');
720  return c;
721 
722 }
723 
734 void
735 vpServolens::wait(vpServoType servo)
736 {
737  if (!isinit) {
738  vpERROR_TRACE ("Cannot dial with Servolens.");
740  "Cannot dial with Servolens.");
741  }
742 
743  char c;
744  char fin_mvt[3];
745  bool sortie = false;
746 
747  switch (servo) {
748  case ZOOM:
749  sprintf(fin_mvt, "ZF");
750  break;
751  case FOCUS:
752  sprintf(fin_mvt, "FF");
753  break;
754  case IRIS:
755  default:
756  sprintf(fin_mvt, "DF");
757  break;
758 
759  }
760 
761  /* lecture des caracteres recus */
762  do {
763  /* lecture des caracteres */
764  if (::read(this->remfd,&c,1) != 1) {
766  "Cannot read on Servolens.");
767  }
768  c &= 0x7f;
769 
770  /* tests si fin de mouvement */
771  if (c == fin_mvt[0]) {
772  /* lecture du caractere suivant */
773  if (::read(this->remfd,&c,1) != 1) {
775  "Cannot read on Servolens.");
776  }
777 
778  c &= 0x7f;
779  if (c == fin_mvt[1]) {
780  sortie = true;
781  }
782  }
783  }
784  while ( !sortie);
785 
786  /* printf("\nmouvement fini: chaine lue = %s", chaine); */
787 }
788 
800 bool
801 vpServolens::read(char *c, long timeout_s)
802 {
803  if (!isinit) {
804  vpERROR_TRACE ("Cannot dial with Servolens.");
806  "Cannot dial with Servolens.");
807  }
808 
809  int n;
810  fd_set readfds; /* list of fds for select to listen to */
811  struct timeval timeout = {timeout_s, 0}; // seconde, micro-sec
812 
813  FD_ZERO(&readfds);
814  FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
815 
816  if (select(FD_SETSIZE, &readfds, (fd_set *)NULL,
817  (fd_set *)NULL, &timeout) > 0) {
818  n = ::read(this->remfd, c, 1); /* read one character at a time */
819  if (n != 1)
820  return false;
821  *c &= 0x7f;
822  //printf("lecture 1 car: %c\n", *c);
823  return(true);
824  }
825 
826  return (false);
827 }
828 
837 void
838 vpServolens::write(const char *s)
839 {
840  if (!isinit) {
841  vpERROR_TRACE ("Cannot dial with Servolens.");
843  "Cannot dial with Servolens.");
844  }
845  ssize_t r = 0;
846  r = ::write(this->remfd,"\r", strlen("\r"));
847  r += ::write(this->remfd, s, strlen(s));
848  r += ::write(this->remfd,"\r", strlen("\r"));
849  if (r != (ssize_t)(2*strlen("\r") + strlen(s))) {
851  "Cannot write on Servolens.");
852  }
853 
854 
855  /*
856  * Une petite tempo pour laisser le temps a la liaison serie de
857  * digerer la commande envoyee. En fait, la liaison serie fonctionne
858  * a 9600 bauds soit une transmission d'environ 9600 bits pas seconde.
859  * Les plus longues commandes envoyees sur la liaison serie sont du type:
860  * SX0842 soit 6 caracteres codes sur 8 bits chacuns = 48 bits pour
861  * envoyer la commande SX0842.
862  * Ainsi, le temps necessaire pour envoyer SX0842 est d'environ
863  * 48 / 9600 = 0,0050 secondes = 5 milli secondes.
864  * Ici on rajoute une marge pour amener la tempo a 20 ms.
865  */
866  vpTime::wait(20);
867 }
868 
876 bool
877 vpServolens::clean(const char *in, char *out)
878 {
879  short nb_car, i=0;
880  bool error = false;
881 
882  nb_car = strlen(in);
883 
884  /* on se positionne sur le 1er caractere different de zero */
885  while( *(in) == '0' && i++ < nb_car ) {
886  in++;
887  if (i == nb_car)
888  {
889  error = true; /* la chaine ne contient pas une position */
890  *(out++) = '0'; /* mise a zero de la position */
891  }
892  }
893 
894  /* copie de la position epuree des zeros de gauche */
895  while( i++ <= nb_car ) { /* on a mis le = pour copier le caractere de fin */
896  /* de chaine \0 */
897  *(out++) = *(in++);
898  }
899  return (error);
900 }
901 
902 #endif