ViSP  2.8.0
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
Error that can be emited by the vpRobot class and its derivates.
unsigned int getWidth() const
Definition: vpImage.h:159
#define vpERROR_TRACE
Definition: vpDebug.h:379
static int wait(double t0, double t)
Definition: vpTime.cpp:149
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void setController(vpControllerType controller)
void setPosition(vpServoType servo, unsigned int position)
Generic class defining intrinsic camera parameters.
vpCameraParameters getCameraParameters(vpImage< unsigned char > &I)
void setAutoIris(bool enable)
bool getPosition(vpServoType servo, unsigned int &position)
void open(const char *port="/dev/ttyS0")
unsigned int getHeight() const
Definition: vpImage.h:150
void enablePrompt(bool active)