Visual Servoing Platform  version 3.0.0
vpServolens.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Interface for the Servolens lens attached to the camera fixed on the
32  * Afma4 robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
48 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
49 
50 #include <unistd.h>
51 #include <termios.h>
52 #include <stdio.h>
53 #include <sys/types.h>
54 #include <sys/stat.h>
55 #include <fcntl.h>
56 #include <stdlib.h>
57 #include <string.h>
58 
59 #include <visp3/robot/vpServolens.h>
60 #include <visp3/robot/vpRobotException.h>
61 #include <visp3/core/vpDebug.h>
62 #include <visp3/core/vpTime.h>
63 
70 vpServolens::vpServolens() : remfd(0), isinit(false)
71 {
72 }
73 
82 vpServolens::vpServolens(const char *port) : remfd(0), isinit(false)
83 {
84  this->open(port);
85 }
86 
95 {
96  this->close();
97 }
98 
113 void
114 vpServolens::open(const char *port)
115 {
116  if (! isinit) {
117  struct termios info;
118 
119  printf("\nOpen the Servolens serial port \"%s\"\n", port);
120 
121  if ((this->remfd=::open(port, O_RDWR|O_NONBLOCK)) < 0) {
122  vpERROR_TRACE ("Cannot open Servolens serial port.");
124  "Cannot open Servolens serial port.");
125  }
126 
127  // Lecture des parametres courants de la liaison serie.
128  if (tcgetattr(this->remfd, &info) < 0) {
129  ::close(this->remfd);
130  vpERROR_TRACE ("Error using TCGETS in ioctl.");
132  "Error using TCGETS in ioctl");
133  }
134 
135  //
136  // Configuration de la liaison serie:
137  // 9600 bauds, 1 bit de stop, parite paire, 7 bits de donnee
138  //
139 
140  // Traitement sur les caracteres recus
141  info.c_iflag = 0;
142  info.c_iflag |= INLCR;
143 
144  // Traitement sur les caracteres envoyes sur la RS232.
145  info.c_oflag = 0; // idem
146 
147  // Traitement des lignes
148  info.c_lflag = 0;
149 
150  // Controle materiel de la liaison
151  info.c_cflag = 0;
152  info.c_cflag |= CREAD; // Validation reception
153  info.c_cflag |= B9600 | CS7 | PARENB; // 9600 baus, 7 data, parite paire
154 
155  // Caracteres immediatement disponibles.
156  // info.c_cc[VMIN] = 1;
157  // info.c_cc[VTIME] = 0;
158 
159  if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
160  ::close(this->remfd);
161  vpERROR_TRACE ("Error using TCGETS in ioctl.");
163  "Error using TCGETS in ioctl");
164  }
165 
166  // Supprime tous les caracteres recus mais non encore lus par read()
167  tcflush(this->remfd, TCIFLUSH);
168 
169  isinit = true;
170 
171  this->init();
172 
173  // Try to get the position of the zoom to check if the lens is really connected
174  unsigned int izoom;
175  if (this->getPosition(vpServolens::ZOOM, izoom) == false) {
176  vpERROR_TRACE ("Cannot dial with the servolens. Check if the serial link is connected.");
178  "Cannot dial with the servolens. Check if the serial link is connected.");
179 
180  }
181 
182  }
183 }
184 
189 void
191 {
192  if (isinit) {
193  printf("\nClose the serial connexion with Servolens\n");
194  ::close(this->remfd);
195  isinit = false;
196  }
197 }
198 
205 void
207 {
208  if (!isinit) {
209  vpERROR_TRACE ("Cannot dial with Servolens.");
211  "Cannot dial with Servolens.");
212  }
213  char commande[10];
214 
215  /* suppression de l'echo */
216  sprintf(commande, "SE1");
217  this->write(commande);
218 
219  /* initialisation de l'objectif, idem qu'a la mise sous tension */
220  sprintf(commande, "SR0");
221  this->write(commande);
222 
223  vpTime::wait(25000);
224 
225  this->wait();
226 
227  /* suppression de l'echo */
228  sprintf(commande, "SE0");
229  this->write(commande);
230 
231  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
232  sprintf(commande, "VW0");
233  this->write(commande);
234 }
243 void
244 vpServolens::init() const
245 {
246  if (!isinit) {
247  vpERROR_TRACE ("Cannot dial with Servolens.");
249  "Cannot dial with Servolens.");
250  }
251 
252  char commande[10];
253 
254  /* suppression de l'echo */
255  sprintf(commande, "SE0");
256  this->write(commande);
257 
258  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
259  sprintf(commande, "VW0");
260  this->write(commande);
261 
262  /* L'experience montre qu'une petite tempo est utile. */
263  vpTime::wait(500);
264 }
265 
278 void
279 vpServolens::enableCmdComplete(vpServoType servo, bool active) const
280 {
281  if (!isinit) {
282  vpERROR_TRACE ("Cannot dial with Servolens.");
284  "Cannot dial with Servolens.");
285  }
286  char commande[10];
287 
288  /* Envoie une commande pour qu'en fin de mouvement servolens renvoie
289  * une information de fin de mouvement (ex: ZF, FF, DF).
290  */
291  switch(servo) {
292  case ZOOM:
293  if (active)
294  sprintf(commande, "ZF1");
295  else
296  sprintf(commande, "ZF0");
297  break;
298  case FOCUS:
299  if (active)
300  sprintf(commande, "FF1");
301  else
302  sprintf(commande, "FF0");
303  break;
304  case IRIS:
305  if (active)
306  sprintf(commande, "DF1");
307  else
308  sprintf(commande, "DF0");
309  break;
310  }
311 
312  /* envoie de la commande */
313  this->write(commande); /* a la fin du mouvement envoie de ZF, FF, DF */
314 }
315 
326 void
327 vpServolens::enablePrompt(bool active) const
328 {
329  if (!isinit) {
330  vpERROR_TRACE ("Cannot dial with Servolens.");
332  "Cannot dial with Servolens.");
333  }
334  char commande[10];
335 
336  /* suppression de l'echo */
337  if (active == true)
338  sprintf(commande, "SE1");
339  else
340  sprintf(commande, "SE0");
341 
342  this->write(commande);
343 }
344 
353 void
355 {
356  if (!isinit) {
357  vpERROR_TRACE ("Cannot dial with Servolens.");
359  "Cannot dial with Servolens.");
360  }
361  char commande[10];
362 
363  switch(controller) {
364  case AUTO:
365  /* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
366  sprintf(commande, "VW1");
367  this->write(commande);
368  break;
369  case CONTROLLED:
370  /* nettoyage : mot d'etat vide 0000 */
371  sprintf(commande,"SX0842");
372  this->write(commande);
373  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
374  sprintf(commande, "VW0");
375  this->write(commande);
376  break;
377  case RELEASED:
378  sprintf(commande,"SX1084");
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  }
385 
386 }
387 
394 void
395 vpServolens::setAutoIris(bool enable) const
396 {
397  if (!isinit) {
398  vpERROR_TRACE ("Cannot dial with Servolens.");
400  "Cannot dial with Servolens.");
401  }
402  char commande[10];
403 
404  if (enable)
405  sprintf(commande, "DA1");
406  else
407  sprintf(commande, "DA0");
408 
409  this->write(commande);
410 }
411 
422 void
423 vpServolens::setPosition(vpServoType servo, unsigned int position) const
424 {
425  if (!isinit) {
426  vpERROR_TRACE ("Cannot dial with Servolens.");
428  "Cannot dial with Servolens.");
429  }
430  char commande[10];
431 
432  /* attente du prompt pour envoyer une commande */
433  /*
434  printf("attente prompt\n");
435  this->wait();
436  */
437 
438 #ifdef FINSERVO
439  /* envoie des commandes pour qu'en fin de mouvement servolens renvoie */
440  /* une commande de fin de mouvement (ex: ZF, FF, DF). */
441  this->enableCommandComplete();
442 #endif /* FINSERVO */
443 
444  // 08/08/00 Fabien S. - Correction de la consigne demandee
445  // pour prendre en compte l'erreur entre la consigne demandee
446  // et la consigne mesuree.
447  // A la consigne du zoom on retranche 1.
448  // A la consigne du focus on ajoute 1.
449  // A la consigne du iris on ajoute 1.
450  switch (servo) {
451  case ZOOM:
452  //printf("zoom demande: %d ", position);
453  position --;
454  if (position < ZOOM_MIN) position = ZOOM_MIN;
455  //printf("zoom corrige: %d \n", position);
456  break;
457  case FOCUS:
458  //printf("focus demande: %d ", position);
459  position ++;
460  if (position > FOCUS_MAX) position = FOCUS_MAX;
461  //printf("focus corrige: %d \n", position);
462  break;
463  case IRIS:
464  // printf("iris demande: %d ", position);
465  position ++;
466  if (position > IRIS_MAX) position = IRIS_MAX;
467  //printf("iris corrige: %s \n", position);
468  break;
469  }
470 
471  /* commande a envoyer aux servomoteurs */
472  switch(servo) {
473  case ZOOM:
474  sprintf(commande, "ZD%d", position);
475  break;
476  case FOCUS:
477  sprintf(commande, "FD%d", position);
478  break;
479  case IRIS:
480  sprintf(commande, "DD%d", position);
481  break;
482  }
483  /* envoie de la commande */
484 #ifdef PRINT
485  printf("\ncommande: %s", commande);
486 #endif
487 
488  this->write(commande);
489 
490 #ifdef FINSERVO
491  /* on attend la fin du mouvement des objectifs */
492  this->wait(servo); /* on attend les codes ZF, FF, DF */
493 #endif
494 }
495 
506 bool
507 vpServolens::getPosition(vpServoType servo, unsigned int &position) const
508 {
509  if (!isinit) {
510  vpERROR_TRACE ("Cannot dial with Servolens.");
512  "Cannot dial with Servolens.");
513  }
514  char commande[10];
515  char posit[10], *pt_posit;
516  char c;
517  short fin_lect_posit; /* indique si on a lu la position du servo-moteur */
518  short posit_car; /* donne la position du caractere lu */
519  short lecture_posit_en_cours; /* indique si on lit la position courante */
520 
521  /* attente du prompt pour envoyer une commande */
522  /*
523  this->wait();
524  */
525  pt_posit = posit;
526 
527  /* envoie des commandes pour obtenir la position des servo-moteurs. */
528  switch (servo) {
529  case ZOOM:
530  sprintf(commande, "ZD?");
531  break;
532  case FOCUS:
533  sprintf(commande, "FD?");
534  break;
535  case IRIS:
536  sprintf(commande, "DD?");
537  break;
538  default:
539  break;
540  }
541  /* envoie de la commande */
542  // printf("\ncommande: %s", commande);
543 
544  this->write(commande);
545 
546  /* on cherche a lire la position du servo-moteur */
547  /* Servolens renvoie une chaine de caractere du type ZD00400 ou FD00234 */
548  fin_lect_posit = 0;
549  posit_car = 0;
550  lecture_posit_en_cours = 0;
551  do {
552  if (this->read(&c, 1) == true) {
553 
554  // printf("caractere lu: %c\n", c);
555  switch (posit_car){
556  /* on lit le 1er caractere; (soit Z, soit F, soit D) */
557  case 0:
558  /* sauvegarde du pointeur */
559  pt_posit = posit;
560 
561  switch (servo) {
562  case ZOOM:
563  if( c == 'Z') posit_car = 1;
564  break;
565  case FOCUS:
566  if( c == 'F') posit_car = 1;
567  break;
568  case IRIS:
569  if( c == 'D') posit_car = 1;
570  break;
571  }
572  break;
573 
574  /* si le 1er caractere est correct, on lit le 2eme caractere */
575  /* (toujours D) */
576  case 1:
577  if( c == 'D') posit_car = 2;
578  else posit_car = 0; /* le 2eme caractere n'est pas correct */
579  break;
580 
581  /* si on a lu les 2 premiers caracteres, on peut lire la */
582  /* position du servo-moteur */
583  case 2:
584  if (c >= '0' && c <= '9')
585  {
586  *pt_posit++ = c; /* sauvegarde de la position */
587  lecture_posit_en_cours = 1;
588  }
589  else if (lecture_posit_en_cours)
590  {
591  fin_lect_posit = 1;
592  *pt_posit = '\0';
593  }
594  else posit_car = 0;
595  break;
596  }
597 
598  }
599  else {
600  // Timout sur la lecture, on retoure FALSE
601  return false;
602  }
603  }
604  while ( !fin_lect_posit );
605 
606  // printf("\nChaine lue: posit: %s", posit);
607 
608  /* toilettage de la position courantes lue */
609  this->clean(posit, posit);
610 
611  // printf("\nChaine toilettee: posit: %s", posit);
612  position = (unsigned int)atoi(posit);
613 
614  return(true);
615 }
616 
646 {
647  if (!isinit) {
648  vpERROR_TRACE ("Cannot dial with Servolens.");
650  "Cannot dial with Servolens.");
651  }
652  vpCameraParameters cam;
653  double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
654  double px=1000, py=1000, u0=320, v0=240;
655  // Determine if the image is subsampled.
656  // Dragonfly2 native images are 640 by 480
657  double subsample_factor = 1.;
658  double width = I.getWidth();
659  double height= I.getHeight();
660 
661 
662  if (width > 300 && width < 340 && height > 220 && height < 260)
663  subsample_factor = 2;
664  else if (width > 140 && width < 1800 && height > 100 && height < 140)
665  subsample_factor = 4;
666 
667  unsigned zoom;
669  //std::cout << "Actual zoom value: " << zoom << std::endl;
670 
671  // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale correspondent
672  // aux parametres de calibration de la camera (donnees constructeur) pour des
673  // tailles d'images CCIR (768x576), donc avec scale = 1.
674  double focale = zoom * 1.0e-5; // Transformation en metres
675  px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
676  py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
677  u0 = I.getWidth() / 2.;
678  v0 = I.getHeight() / 2.;
679  cam.initPersProjWithoutDistortion(px, py, u0, v0);
680 
681  return cam;
682 }
683 
692 char
693 vpServolens::wait() const
694 {
695  if (!isinit) {
696  vpERROR_TRACE ("Cannot dial with Servolens.");
698  "Cannot dial with Servolens.");
699  }
700 
701  ssize_t r;
702  r = ::write(this->remfd, "\r\n", strlen("\r\n"));
703  if (r != (ssize_t)(strlen("\r\n"))) {
705  "Cannot write on Servolens.");
706  }
707  char c;
708  do {
709  r = ::read(this->remfd, &c, 1);
710  c &= 0x7f;
711  }
712  while (c != '>');
713  return c;
714 
715 }
716 
727 void
728 vpServolens::wait(vpServoType servo) const
729 {
730  if (!isinit) {
731  vpERROR_TRACE ("Cannot dial with Servolens.");
733  "Cannot dial with Servolens.");
734  }
735 
736  char c;
737  char fin_mvt[3];
738  bool sortie = false;
739 
740  switch (servo) {
741  case ZOOM:
742  sprintf(fin_mvt, "ZF");
743  break;
744  case FOCUS:
745  sprintf(fin_mvt, "FF");
746  break;
747  case IRIS:
748  default:
749  sprintf(fin_mvt, "DF");
750  break;
751 
752  }
753 
754  /* lecture des caracteres recus */
755  do {
756  /* lecture des caracteres */
757  if (::read(this->remfd,&c,1) != 1) {
759  "Cannot read on Servolens.");
760  }
761  c &= 0x7f;
762 
763  /* tests si fin de mouvement */
764  if (c == fin_mvt[0]) {
765  /* lecture du caractere suivant */
766  if (::read(this->remfd,&c,1) != 1) {
768  "Cannot read on Servolens.");
769  }
770 
771  c &= 0x7f;
772  if (c == fin_mvt[1]) {
773  sortie = true;
774  }
775  }
776  }
777  while ( !sortie);
778 
779  /* printf("\nmouvement fini: chaine lue = %s", chaine); */
780 }
781 
793 bool
794 vpServolens::read(char *c, long timeout_s) const
795 {
796  if (!isinit) {
797  vpERROR_TRACE ("Cannot dial with Servolens.");
799  "Cannot dial with Servolens.");
800  }
801 
802  int n;
803  fd_set readfds; /* list of fds for select to listen to */
804  struct timeval timeout = {timeout_s, 0}; // seconde, micro-sec
805 
806  FD_ZERO(&readfds);
807  FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
808 
809  if (select(FD_SETSIZE, &readfds, (fd_set *)NULL,
810  (fd_set *)NULL, &timeout) > 0) {
811  n = ::read(this->remfd, c, 1); /* read one character at a time */
812  if (n != 1)
813  return false;
814  *c &= 0x7f;
815  //printf("lecture 1 car: %c\n", *c);
816  return(true);
817  }
818 
819  return (false);
820 }
821 
830 void
831 vpServolens::write(const char *s) const
832 {
833  if (!isinit) {
834  vpERROR_TRACE ("Cannot dial with Servolens.");
836  "Cannot dial with Servolens.");
837  }
838  ssize_t r = 0;
839  r = ::write(this->remfd,"\r", strlen("\r"));
840  r += ::write(this->remfd, s, strlen(s));
841  r += ::write(this->remfd,"\r", strlen("\r"));
842  if (r != (ssize_t)(2*strlen("\r") + strlen(s))) {
844  "Cannot write on Servolens.");
845  }
846 
847 
848  /*
849  * Une petite tempo pour laisser le temps a la liaison serie de
850  * digerer la commande envoyee. En fait, la liaison serie fonctionne
851  * a 9600 bauds soit une transmission d'environ 9600 bits pas seconde.
852  * Les plus longues commandes envoyees sur la liaison serie sont du type:
853  * SX0842 soit 6 caracteres codes sur 8 bits chacuns = 48 bits pour
854  * envoyer la commande SX0842.
855  * Ainsi, le temps necessaire pour envoyer SX0842 est d'environ
856  * 48 / 9600 = 0,0050 secondes = 5 milli secondes.
857  * Ici on rajoute une marge pour amener la tempo a 20 ms.
858  */
859  vpTime::wait(20);
860 }
861 
869 bool
870 vpServolens::clean(const char *in, char *out) const
871 {
872  short nb_car, i=0;
873  bool error = false;
874 
875  nb_car = strlen(in);
876 
877  /* on se positionne sur le 1er caractere different de zero */
878  while( *(in) == '0' && i++ < nb_car ) {
879  in++;
880  if (i == nb_car)
881  {
882  error = true; /* la chaine ne contient pas une position */
883  *(out++) = '0'; /* mise a zero de la position */
884  }
885  }
886 
887  /* copie de la position epuree des zeros de gauche */
888  while( i++ <= nb_car ) { /* on a mis le = pour copier le caractere de fin */
889  /* de chaine \0 */
890  *(out++) = *(in++);
891  }
892  return (error);
893 }
894 
895 #endif
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
Error that can be emited by the vpRobot class and its derivates.
unsigned int getWidth() const
Definition: vpImage.h:161
#define vpERROR_TRACE
Definition: vpDebug.h:391
void setController(vpControllerType controller) const
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void enablePrompt(bool active) const
vpCameraParameters getCameraParameters(vpImage< unsigned char > &I) const
Generic class defining intrinsic camera parameters.
void open(const char *port="/dev/ttyS0")
void setAutoIris(bool enable) const
unsigned int getHeight() const
Definition: vpImage.h:152
void reset() const
bool getPosition(vpServoType servo, unsigned int &position) const
void setPosition(vpServoType servo, unsigned int position) const