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 ) {
Error that can be emited by the vpRobot class and its derivates.
unsigned int getWidth() const
static int wait(double t0, double t)
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
void enablePrompt(bool active)