57 #include <sys/types.h>
63 #include <visp/vpServolens.h>
64 #include <visp/vpRobotException.h>
65 #include <visp/vpDebug.h>
66 #include <visp/vpTime.h>
126 printf(
"\nOpen the Servolens serial port \"%s\"\n", port);
128 if ((this->remfd=::
open(port, O_RDWR|O_NONBLOCK)) < 0) {
131 "Cannot open Servolens serial port.");
135 if (tcgetattr(this->remfd, &info) < 0) {
139 "Error using TCGETS in ioctl");
149 info.c_iflag |= INLCR;
159 info.c_cflag |= CREAD;
160 info.c_cflag |= B9600 | CS7 | PARENB;
166 if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
170 "Error using TCGETS in ioctl");
174 tcflush(this->remfd, TCIFLUSH);
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.");
200 printf(
"\nClose the serial connexion with Servolens\n");
218 "Cannot dial with Servolens.");
223 sprintf(commande,
"SE1");
224 this->write(commande);
227 sprintf(commande,
"SR0");
228 this->write(commande);
235 sprintf(commande,
"SE0");
236 this->write(commande);
239 sprintf(commande,
"VW0");
240 this->write(commande);
256 "Cannot dial with Servolens.");
262 sprintf(commande,
"SE0");
263 this->write(commande);
266 sprintf(commande,
"VW0");
267 this->write(commande);
286 vpServolens::enableCmdComplete(vpServoType servo,
bool active)
291 "Cannot dial with Servolens.");
301 sprintf(commande,
"ZF1");
303 sprintf(commande,
"ZF0");
307 sprintf(commande,
"FF1");
309 sprintf(commande,
"FF0");
313 sprintf(commande,
"DF1");
315 sprintf(commande,
"DF0");
320 this->write(commande);
339 "Cannot dial with Servolens.");
345 sprintf(commande,
"SE1");
347 sprintf(commande,
"SE0");
349 this->write(commande);
366 "Cannot dial with Servolens.");
373 sprintf(commande,
"VW1");
374 this->write(commande);
378 sprintf(commande,
"SX0842");
379 this->write(commande);
381 sprintf(commande,
"VW0");
382 this->write(commande);
385 sprintf(commande,
"SX1084");
386 this->write(commande);
388 sprintf(commande,
"VW0");
389 this->write(commande);
407 "Cannot dial with Servolens.");
412 sprintf(commande,
"DA1");
414 sprintf(commande,
"DA0");
416 this->write(commande);
435 "Cannot dial with Servolens.");
448 this->enableCommandComplete();
481 sprintf(commande,
"ZD%d", position);
484 sprintf(commande,
"FD%d", position);
487 sprintf(commande,
"DD%d", position);
492 printf(
"\ncommande: %s", commande);
495 this->write(commande);
519 "Cannot dial with Servolens.");
522 char posit[10], *pt_posit;
524 short fin_lect_posit;
526 short lecture_posit_en_cours;
537 sprintf(commande,
"ZD?");
540 sprintf(commande,
"FD?");
543 sprintf(commande,
"DD?");
551 this->write(commande);
557 lecture_posit_en_cours = 0;
559 if (this->read(&c, 1) ==
true) {
570 if( c ==
'Z') posit_car = 1;
573 if( c ==
'F') posit_car = 1;
576 if( c ==
'D') posit_car = 1;
584 if( c ==
'D') posit_car = 2;
591 if (c >=
'0' && c <=
'9')
594 lecture_posit_en_cours = 1;
596 else if (lecture_posit_en_cours)
611 while ( !fin_lect_posit );
616 this->clean(posit, posit);
619 position = (
unsigned int)atoi(posit);
657 "Cannot dial with Servolens.");
660 double pix_size = 7.4e-6;
661 double px=1000, py=1000, u0=320, v0=240;
664 double subsample_factor = 1.;
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;
681 double focale = zoom * 1.0e-5;
682 px = focale / (double)(subsample_factor * pix_size);
683 py = focale / (double)(subsample_factor * pix_size);
705 "Cannot dial with Servolens.");
709 r = ::write(this->remfd,
"\r\n", strlen(
"\r\n"));
710 if (r != (ssize_t)(strlen(
"\r\n"))) {
712 "Cannot write on Servolens.");
716 r = ::read(this->remfd, &c, 1);
735 vpServolens::wait(vpServoType servo)
740 "Cannot dial with Servolens.");
749 sprintf(fin_mvt,
"ZF");
752 sprintf(fin_mvt,
"FF");
756 sprintf(fin_mvt,
"DF");
764 if (::read(this->remfd,&c,1) != 1) {
766 "Cannot read on Servolens.");
771 if (c == fin_mvt[0]) {
773 if (::read(this->remfd,&c,1) != 1) {
775 "Cannot read on Servolens.");
779 if (c == fin_mvt[1]) {
801 vpServolens::read(
char *c,
long timeout_s)
806 "Cannot dial with Servolens.");
811 struct timeval timeout = {timeout_s, 0};
814 FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
816 if (select(FD_SETSIZE, &readfds, (fd_set *)NULL,
817 (fd_set *)NULL, &timeout) > 0) {
818 n = ::read(this->remfd, c, 1);
838 vpServolens::write(
const char *s)
843 "Cannot dial with Servolens.");
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.");
877 vpServolens::clean(
const char *in,
char *out)
885 while( *(in) ==
'0' && i++ < nb_car ) {
895 while( i++ <= nb_car ) {