Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpServolens.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the Servolens lens attached to the camera fixed on the
33 * Afma4 robot.
34 *
35*****************************************************************************/
36
46#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
47
48#include <fcntl.h>
49#include <stdio.h>
50#include <stdlib.h>
51#include <string.h>
52#include <sys/stat.h>
53#include <sys/types.h>
54#include <termios.h>
55#include <unistd.h>
56
57#include <visp3/core/vpDebug.h>
58#include <visp3/core/vpTime.h>
59#include <visp3/robot/vpRobotException.h>
60#include <visp3/robot/vpServolens.h>
61
68vpServolens::vpServolens() : remfd(0), isinit(false) {}
69
78vpServolens::vpServolens(const char *port) : remfd(0), isinit(false) { this->open(port); }
79
88
103void vpServolens::open(const char *port)
104{
105 if (!isinit) {
106 struct termios info;
107
108 printf("\nOpen the Servolens serial port \"%s\"\n", port);
109
110 if ((this->remfd = ::open(port, O_RDWR | O_NONBLOCK)) < 0) {
111 vpERROR_TRACE("Cannot open Servolens serial port.");
112 throw vpRobotException(vpRobotException::communicationError, "Cannot open Servolens serial port.");
113 }
114
115 // Lecture des parametres courants de la liaison serie.
116 if (tcgetattr(this->remfd, &info) < 0) {
117 ::close(this->remfd);
118 vpERROR_TRACE("Error using TCGETS in ioctl.");
119 throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl");
120 }
121
122 //
123 // Configuration de la liaison serie:
124 // 9600 bauds, 1 bit de stop, parite paire, 7 bits de donnee
125 //
126
127 // Traitement sur les caracteres recus
128 info.c_iflag = 0;
129 info.c_iflag |= INLCR;
130
131 // Traitement sur les caracteres envoyes sur la RS232.
132 info.c_oflag = 0; // idem
133
134 // Traitement des lignes
135 info.c_lflag = 0;
136
137 // Controle materiel de la liaison
138 info.c_cflag = 0;
139 info.c_cflag |= CREAD; // Validation reception
140 info.c_cflag |= B9600 | CS7 | PARENB; // 9600 baus, 7 data, parite paire
141
142 // Caracteres immediatement disponibles.
143 // info.c_cc[VMIN] = 1;
144 // info.c_cc[VTIME] = 0;
145
146 if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
147 ::close(this->remfd);
148 vpERROR_TRACE("Error using TCGETS in ioctl.");
149 throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl");
150 }
151
152 // Supprime tous les caracteres recus mais non encore lus par read()
153 tcflush(this->remfd, TCIFLUSH);
154
155 isinit = true;
156
157 this->init();
158
159 // Try to get the position of the zoom to check if the lens is really
160 // connected
161 unsigned int izoom;
162 if (this->getPosition(vpServolens::ZOOM, izoom) == false) {
163 vpERROR_TRACE("Cannot dial with the servolens. Check if the serial "
164 "link is connected.");
165 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with the servolens. Check if the "
166 "serial link is connected.");
167 }
168 }
169}
170
176{
177 if (isinit) {
178 printf("\nClose the serial connection with Servolens\n");
179 ::close(this->remfd);
180 isinit = false;
181 }
182}
183
191{
192 if (!isinit) {
193 vpERROR_TRACE("Cannot dial with Servolens.");
194 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
195 }
196 std::string cmd;
197
198 /* suppression de l'echo */
199 cmd = "SE1";
200 this->write(cmd.c_str());
201
202 /* initialisation de l'objectif, idem qu'a la mise sous tension */
203 cmd = "SR0";
204 this->write(cmd.c_str());
205
206 vpTime::wait(25000);
207
208 this->wait();
209
210 /* suppression de l'echo */
211 cmd = "SE0";
212 this->write(cmd.c_str());
213
214 /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
215 cmd = "VW0";
216 this->write(cmd.c_str());
217}
226void vpServolens::init() const
227{
228 if (!isinit) {
229 vpERROR_TRACE("Cannot dial with Servolens.");
230 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
231 }
232
233 std::string cmd;
234
235 /* suppression de l'echo */
236 cmd = "SE0";
237 this->write(cmd.c_str());
238
239 /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
240 cmd = "VW0";
241 this->write(cmd.c_str());
242
243 /* L'experience montre qu'une petite tempo est utile. */
244 vpTime::wait(500);
245}
246
257void vpServolens::enablePrompt(bool active) const
258{
259 if (!isinit) {
260 vpERROR_TRACE("Cannot dial with Servolens.");
261 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
262 }
263 std::string cmd;
264
265 /* suppression de l'echo */
266 if (active == true)
267 cmd = "SE1";
268 else
269 cmd = "SE0";
270
271 this->write(cmd.c_str());
272}
273
283{
284 if (!isinit) {
285 vpERROR_TRACE("Cannot dial with Servolens.");
286 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
287 }
288 std::string cmd;
289
290 switch (controller) {
291 case AUTO:
292 /* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
293 cmd = "VW1";
294 this->write(cmd.c_str());
295 break;
296 case CONTROLLED:
297 /* nettoyage : mot d'etat vide 0000 */
298 cmd = "SX0842";
299 this->write(cmd.c_str());
300 /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
301 cmd = "VW0";
302 this->write(cmd.c_str());
303 break;
304 case RELEASED:
305 cmd = "SX1084";
306 this->write(cmd.c_str());
307 /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
308 cmd = "VW0";
309 this->write(cmd.c_str());
310 break;
311 }
312}
313
320void vpServolens::setAutoIris(bool enable) const
321{
322 if (!isinit) {
323 vpERROR_TRACE("Cannot dial with Servolens.");
324 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
325 }
326 std::string cmd;
327
328 if (enable)
329 cmd = "DA1";
330 else
331 cmd = "DA0";
332
333 this->write(cmd.c_str());
334}
335
346void vpServolens::setPosition(vpServoType servo, unsigned int position) const
347{
348 if (!isinit) {
349 vpERROR_TRACE("Cannot dial with Servolens.");
350 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
351 }
352 std::stringstream command;
353
354 /* attente du prompt pour envoyer une commande */
355 /*
356 printf("attente prompt\n");
357 this->wait();
358 */
359
360#ifdef FINSERVO
361 /* envoie des commandes pour qu'en fin de mouvement servolens renvoie */
362 /* une commande de fin de mouvement (ex: ZF, FF, DF). */
363 this->enableCommandComplete();
364#endif /* FINSERVO */
365
366 // 08/08/00 Fabien S. - Correction de la consigne demandee
367 // pour prendre en compte l'erreur entre la consigne demandee
368 // et la consigne mesuree.
369 // A la consigne du zoom on retranche 1.
370 // A la consigne du focus on ajoute 1.
371 // A la consigne du iris on ajoute 1.
372 switch (servo) {
373 case ZOOM:
374 // printf("zoom demande: %d ", position);
375 position--;
376 if (position < ZOOM_MIN)
377 position = ZOOM_MIN;
378 // printf("zoom corrige: %d \n", position);
379 break;
380 case FOCUS:
381 // printf("focus demande: %d ", position);
382 position++;
383 if (position > FOCUS_MAX)
384 position = FOCUS_MAX;
385 // printf("focus corrige: %d \n", position);
386 break;
387 case IRIS:
388 // printf("iris demande: %d ", position);
389 position++;
390 if (position > IRIS_MAX)
391 position = IRIS_MAX;
392 // printf("iris corrige: %s \n", position);
393 break;
394 }
395
396 /* commande a envoyer aux servomoteurs */
397 switch (servo) {
398 case ZOOM:
399 command << "ZD" << position;
400 break;
401 case FOCUS:
402 command << "FD" << position;
403 break;
404 case IRIS:
405 command << "DD" << position;
406 break;
407 }
408/* envoie de la commande */
409#ifdef PRINT
410 printf("\ncommand: %s", command.str());
411#endif
412
413 this->write(command.str().c_str());
414
415#ifdef FINSERVO
416 /* on attend la fin du mouvement des objectifs */
417 this->wait(servo); /* on attend les codes ZF, FF, DF */
418#endif
419}
420
431bool vpServolens::getPosition(vpServoType servo, unsigned int &position) const
432{
433 if (!isinit) {
434 vpERROR_TRACE("Cannot dial with Servolens.");
435 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
436 }
437 std::string cmd;
438 char posit[10], *pt_posit;
439 char c;
440 short fin_lect_posit; /* indique si on a lu la position du servo-moteur */
441 short posit_car; /* donne la position du caractere lu */
442 short lecture_posit_en_cours; /* indique si on lit la position courante */
443
444 /* attente du prompt pour envoyer une commande */
445 /*
446 this->wait();
447 */
448 pt_posit = posit;
449
450 /* envoie des commandes pour obtenir la position des servo-moteurs. */
451 switch (servo) {
452 case ZOOM:
453 cmd = "ZD?";
454 break;
455 case FOCUS:
456 cmd = "FD?";
457 break;
458 case IRIS:
459 cmd = "DD?";
460 break;
461 default:
462 break;
463 }
464 /* envoie de la commande */
465 // printf("\ncommande: %s", commande);
466
467 this->write(cmd.c_str());
468
469 /* on cherche a lire la position du servo-moteur */
470 /* Servolens renvoie une chaine de caractere du type ZD00400 ou FD00234 */
471 fin_lect_posit = 0;
472 posit_car = 0;
473 lecture_posit_en_cours = 0;
474 do {
475 if (this->read(&c, 1) == true) {
476
477 // printf("caractere lu: %c\n", c);
478 switch (posit_car) {
479 /* on lit le 1er caractere; (soit Z, soit F, soit D) */
480 case 0:
481 /* sauvegarde du pointeur */
482 pt_posit = posit;
483
484 switch (servo) {
485 case ZOOM:
486 if (c == 'Z')
487 posit_car = 1;
488 break;
489 case FOCUS:
490 if (c == 'F')
491 posit_car = 1;
492 break;
493 case IRIS:
494 if (c == 'D')
495 posit_car = 1;
496 break;
497 }
498 break;
499
500 /* si le 1er caractere est correct, on lit le 2eme caractere */
501 /* (toujours D) */
502 case 1:
503 if (c == 'D')
504 posit_car = 2;
505 else
506 posit_car = 0; /* le 2eme caractere n'est pas correct */
507 break;
508
509 /* si on a lu les 2 premiers caracteres, on peut lire la */
510 /* position du servo-moteur */
511 case 2:
512 if (c >= '0' && c <= '9') {
513 *pt_posit++ = c; /* sauvegarde de la position */
514 lecture_posit_en_cours = 1;
515 } else if (lecture_posit_en_cours) {
516 fin_lect_posit = 1;
517 *pt_posit = '\0';
518 } else
519 posit_car = 0;
520 break;
521 }
522
523 } else {
524 // Timout sur la lecture, on retoure FALSE
525 return false;
526 }
527 } while (!fin_lect_posit);
528
529 // printf("\nChaine lue: posit: %s", posit);
530
531 /* toilettage de la position courantes lue */
532 this->clean(posit, posit);
533
534 // printf("\nChaine toilettee: posit: %s", posit);
535 position = (unsigned int)atoi(posit);
536
537 return (true);
538}
539
567{
568 if (!isinit) {
569 vpERROR_TRACE("Cannot dial with Servolens.");
570 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
571 }
573 double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
574 double px = 1000, py = 1000, u0 = 320, v0 = 240;
575 // Determine if the image is subsampled.
576 // Dragonfly2 native images are 640 by 480
577 double subsample_factor = 1.;
578 double width = I.getWidth();
579 double height = I.getHeight();
580
581 if (width > 300 && width < 340 && height > 220 && height < 260)
582 subsample_factor = 2;
583 else if (width > 140 && width < 1800 && height > 100 && height < 140)
584 subsample_factor = 4;
585
586 unsigned zoom;
588 // std::cout << "Actual zoom value: " << zoom << std::endl;
589
590 // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale
591 // correspondent aux parametres de calibration de la camera (donnees
592 // constructeur) pour des tailles d'images CCIR (768x576), donc avec scale
593 // = 1.
594 double focale = zoom * 1.0e-5; // Transformation en metres
595 px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
596 py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
597 u0 = I.getWidth() / 2.;
598 v0 = I.getHeight() / 2.;
599 cam.initPersProjWithoutDistortion(px, py, u0, v0);
600
601 return cam;
602}
603
612char vpServolens::wait() const
613{
614 if (!isinit) {
615 vpERROR_TRACE("Cannot dial with Servolens.");
616 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
617 }
618
619 ssize_t r;
620 r = ::write(this->remfd, "\r\n", strlen("\r\n"));
621 if (r != (ssize_t)(strlen("\r\n"))) {
622 throw vpRobotException(vpRobotException::communicationError, "Cannot write on Servolens.");
623 }
624 char c;
625 do {
626 r = ::read(this->remfd, &c, 1);
627 c &= 0x7f;
628 if (r != 1) {
629 throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
630 }
631 } while (c != '>');
632 return c;
633}
634
645void vpServolens::wait(vpServoType servo) const
646{
647 if (!isinit) {
648 vpERROR_TRACE("Cannot dial with Servolens.");
649 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
650 }
651
652 char c;
653 std::string fin_mvt;
654 bool sortie = false;
655
656 switch (servo) {
657 case ZOOM:
658 fin_mvt = "ZF";
659 break;
660 case FOCUS:
661 fin_mvt = "FF";
662 break;
663 case IRIS:
664 default:
665 fin_mvt = "DF";
666 break;
667 }
668
669 /* lecture des caracteres recus */
670 do {
671 /* lecture des caracteres */
672 if (::read(this->remfd, &c, 1) != 1) {
673 throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
674 }
675 c &= 0x7f;
676
677 /* tests si fin de mouvement */
678 if (c == fin_mvt.c_str()[0]) {
679 /* lecture du caractere suivant */
680 if (::read(this->remfd, &c, 1) != 1) {
681 throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
682 }
683
684 c &= 0x7f;
685 if (c == fin_mvt.c_str()[1]) {
686 sortie = true;
687 }
688 }
689 } while (!sortie);
690
691 /* printf("\nmouvement fini: chaine lue = %s", chaine); */
692}
693
705bool vpServolens::read(char *c, long timeout_s) const
706{
707 if (!isinit) {
708 vpERROR_TRACE("Cannot dial with Servolens.");
709 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
710 }
711
712 fd_set readfds; /* list of fds for select to listen to */
713 struct timeval timeout = {timeout_s, 0}; // seconde, micro-sec
714
715 FD_ZERO(&readfds);
716 FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
717
718 if (select(FD_SETSIZE, &readfds, (fd_set *)NULL, (fd_set *)NULL, &timeout) > 0) {
719 ssize_t n = ::read(this->remfd, c, 1); /* read one character at a time */
720 if (n != 1)
721 return false;
722 *c &= 0x7f;
723 // printf("lecture 1 car: %c\n", *c);
724 return (true);
725 }
726
727 return (false);
728}
729
738void vpServolens::write(const char *s) const
739{
740 if (!isinit) {
741 vpERROR_TRACE("Cannot dial with Servolens.");
742 throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
743 }
744 ssize_t r = 0;
745 r = ::write(this->remfd, "\r", strlen("\r"));
746 r += ::write(this->remfd, s, strlen(s));
747 r += ::write(this->remfd, "\r", strlen("\r"));
748 if (r != (ssize_t)(2 * strlen("\r") + strlen(s))) {
749 throw vpRobotException(vpRobotException::communicationError, "Cannot write on Servolens.");
750 }
751
752 /*
753 * Une petite tempo pour laisser le temps a la liaison serie de
754 * digerer la commande envoyee. En fait, la liaison serie fonctionne
755 * a 9600 bauds soit une transmission d'environ 9600 bits pas seconde.
756 * Les plus longues commandes envoyees sur la liaison serie sont du type:
757 * SX0842 soit 6 caracteres codes sur 8 bits chacuns = 48 bits pour
758 * envoyer la commande SX0842.
759 * Ainsi, le temps necessaire pour envoyer SX0842 est d'environ
760 * 48 / 9600 = 0,0050 secondes = 5 milli secondes.
761 * Ici on rajoute une marge pour amener la tempo a 20 ms.
762 */
763 vpTime::wait(20);
764}
765
773bool vpServolens::clean(const char *in, char *out) const
774{
775 short nb_car, i = 0;
776 bool error = false;
777
778 nb_car = strlen(in);
779
780 /* on se positionne sur le 1er caractere different de zero */
781 while (*(in) == '0' && i++ < nb_car) {
782 in++;
783 if (i == nb_car) {
784 error = true; /* la chaine ne contient pas une position */
785 *(out++) = '0'; /* mise a zero de la position */
786 }
787 }
788
789 /* copie de la position epuree des zeros de gauche */
790 while (i++ <= nb_car) { /* on a mis le = pour copier le caractere de fin */
791 /* de chaine \0 */
792 *(out++) = *(in++);
793 }
794 return (error);
795}
796
797#endif
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
Error that can be emitted by the vpRobot class and its derivatives.
@ communicationError
Unable to communicate.
void setPosition(vpServoType servo, unsigned int position) const
vpCameraParameters getCameraParameters(vpImage< unsigned char > &I) const
void setController(vpControllerType controller) const
void reset() const
bool getPosition(vpServoType servo, unsigned int &position) const
virtual ~vpServolens()
void setAutoIris(bool enable) const
void open(const char *port="/dev/ttyS0")
void enablePrompt(bool active) const
#define vpERROR_TRACE
Definition vpDebug.h:388
VISP_EXPORT int wait(double t0, double t)