49 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX 56 #include <sys/types.h> 60 #include <visp3/core/vpDebug.h> 61 #include <visp3/core/vpTime.h> 62 #include <visp3/robot/vpRobotException.h> 63 #include <visp3/robot/vpServolens.h> 111 printf(
"\nOpen the Servolens serial port \"%s\"\n", port);
113 if ((this->remfd = ::
open(port, O_RDWR | O_NONBLOCK)) < 0) {
119 if (tcgetattr(this->remfd, &info) < 0) {
132 info.c_iflag |= INLCR;
142 info.c_cflag |= CREAD;
143 info.c_cflag |= B9600 | CS7 | PARENB;
149 if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
156 tcflush(this->remfd, TCIFLUSH);
166 vpERROR_TRACE(
"Cannot dial with the servolens. Check if the serial " 167 "link is connected.");
169 "serial link is connected.");
181 printf(
"\nClose the serial connection with Servolens\n");
202 sprintf(commande,
"SE1");
203 this->write(commande);
206 sprintf(commande,
"SR0");
207 this->write(commande);
214 sprintf(commande,
"SE0");
215 this->write(commande);
218 sprintf(commande,
"VW0");
219 this->write(commande);
229 void vpServolens::init()
const 239 sprintf(commande,
"SE0");
240 this->write(commande);
243 sprintf(commande,
"VW0");
244 this->write(commande);
270 sprintf(commande,
"SE1");
272 sprintf(commande,
"SE0");
274 this->write(commande);
293 switch (controller) {
296 sprintf(commande,
"VW1");
297 this->write(commande);
301 sprintf(commande,
"SX0842");
302 this->write(commande);
304 sprintf(commande,
"VW0");
305 this->write(commande);
308 sprintf(commande,
"SX1084");
309 this->write(commande);
311 sprintf(commande,
"VW0");
312 this->write(commande);
332 sprintf(commande,
"DA1");
334 sprintf(commande,
"DA0");
336 this->write(commande);
355 std::stringstream command;
366 this->enableCommandComplete();
402 command <<
"ZD" << position;
405 command <<
"FD" << position;
408 command <<
"DD" << position;
413 printf(
"\ncommand: %s", command.str());
416 this->write(command.str().c_str());
441 char posit[10], *pt_posit;
443 short fin_lect_posit;
445 short lecture_posit_en_cours;
456 sprintf(commande,
"ZD?");
459 sprintf(commande,
"FD?");
462 sprintf(commande,
"DD?");
470 this->write(commande);
476 lecture_posit_en_cours = 0;
478 if (this->read(&c, 1) ==
true) {
515 if (c >=
'0' && c <=
'9') {
517 lecture_posit_en_cours = 1;
518 }
else if (lecture_posit_en_cours) {
530 }
while (!fin_lect_posit);
535 this->clean(posit, posit);
538 position = (
unsigned int)atoi(posit);
576 double pix_size = 7.4e-6;
577 double px = 1000, py = 1000, u0 = 320, v0 = 240;
580 double subsample_factor = 1.;
584 if (width > 300 && width < 340 && height > 220 && height < 260)
585 subsample_factor = 2;
586 else if (width > 140 && width < 1800 && height > 100 && height < 140)
587 subsample_factor = 4;
597 double focale = zoom * 1.0e-5;
598 px = focale / (double)(subsample_factor * pix_size);
599 py = focale / (double)(subsample_factor * pix_size);
615 char vpServolens::wait()
const 623 r = ::write(this->remfd,
"\r\n", strlen(
"\r\n"));
624 if (r != (ssize_t)(strlen(
"\r\n"))) {
629 r = ::read(this->remfd, &c, 1);
661 sprintf(fin_mvt,
"ZF");
664 sprintf(fin_mvt,
"FF");
668 sprintf(fin_mvt,
"DF");
675 if (::read(this->remfd, &c, 1) != 1) {
681 if (c == fin_mvt[0]) {
683 if (::read(this->remfd, &c, 1) != 1) {
688 if (c == fin_mvt[1]) {
708 bool vpServolens::read(
char *c,
long timeout_s)
const 716 struct timeval timeout = {timeout_s, 0};
719 FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
721 if (select(FD_SETSIZE, &readfds, (fd_set *)NULL, (fd_set *)NULL, &timeout) > 0) {
722 ssize_t n = ::read(this->remfd, c, 1);
741 void vpServolens::write(
const char *s)
const 748 r = ::write(this->remfd,
"\r", strlen(
"\r"));
749 r += ::write(this->remfd, s, strlen(s));
750 r += ::write(this->remfd,
"\r", strlen(
"\r"));
751 if (r != (ssize_t)(2 * strlen(
"\r") + strlen(s))) {
776 bool vpServolens::clean(
const char *in,
char *out)
const 784 while (*(in) ==
'0' && i++ < nb_car) {
793 while (i++ <= nb_car) {
VISP_EXPORT int wait(double t0, double t)
Error that can be emited by the vpRobot class and its derivates.
unsigned int getWidth() const
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
bool getPosition(vpServoType servo, unsigned int &position) const
void setPosition(vpServoType servo, unsigned int position) const