vpServolens.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Interface for the Servolens lens attached to the camera fixed on the 
 * Afma4 robot.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX

#include <unistd.h>
#include <termios.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include <string.h>

#include <visp3/robot/vpServolens.h>
#include <visp3/robot/vpRobotException.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpTime.h>

vpServolens::vpServolens() : remfd(0), isinit(false)
{
}

vpServolens::vpServolens(const char *port) : remfd(0), isinit(false)
{
  this->open(port);
}

vpServolens::~vpServolens()
{
  this->close();
}

void
vpServolens::open(const char *port)
{
  if (! isinit) {
    struct termios info;

    printf("\nOpen the Servolens serial port \"%s\"\n", port);

    if ((this->remfd=::open(port, O_RDWR|O_NONBLOCK)) < 0) {
      vpERROR_TRACE ("Cannot open Servolens serial port.");
      throw vpRobotException (vpRobotException::communicationError,
                  "Cannot open Servolens serial port.");
    }

    // Lecture des parametres courants de la liaison serie.
    if (tcgetattr(this->remfd, &info) < 0) {
      ::close(this->remfd);
      vpERROR_TRACE ("Error using TCGETS in ioctl.");
      throw vpRobotException (vpRobotException::communicationError,
                  "Error using TCGETS in ioctl");
    }

    //
    // Configuration de la liaison serie:
    // 9600 bauds, 1 bit de stop, parite paire, 7 bits de donnee
    //

    // Traitement sur les caracteres recus
    info.c_iflag = 0;
    info.c_iflag |= INLCR;

    // Traitement sur les caracteres envoyes sur la RS232.
    info.c_oflag = 0;  // idem

    // Traitement des lignes
    info.c_lflag = 0;

    // Controle materiel de la liaison
    info.c_cflag = 0;
    info.c_cflag |= CREAD;      // Validation reception
    info.c_cflag |= B9600 | CS7 | PARENB; // 9600 baus, 7 data, parite paire

    // Caracteres immediatement disponibles.
    //  info.c_cc[VMIN] = 1;
    //  info.c_cc[VTIME] = 0;

    if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
      ::close(this->remfd);
      vpERROR_TRACE ("Error using TCGETS in ioctl.");
      throw vpRobotException (vpRobotException::communicationError,
                  "Error using TCGETS in ioctl");
    }

    // Supprime tous les caracteres recus mais non encore lus par read()
    tcflush(this->remfd, TCIFLUSH);

    isinit = true;

    this->init();

    // Try to get the position of the zoom to check if the lens is really connected
    unsigned int izoom;
    if (this->getPosition(vpServolens::ZOOM, izoom) == false) {
      vpERROR_TRACE ("Cannot dial with the servolens. Check if the serial link is connected.");
      throw vpRobotException (vpRobotException::communicationError,
                  "Cannot dial with the servolens. Check if the serial link is connected.");

    }
   
  }
}

void
vpServolens::close()
{
  if (isinit) {
    printf("\nClose the serial connexion with Servolens\n");
    ::close(this->remfd);
    isinit = false;
  }
}

void
vpServolens::reset() const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
  char commande[10];

  /* suppression de l'echo */
  sprintf(commande, "SE1");
  this->write(commande);

  /* initialisation de l'objectif, idem qu'a la mise sous tension */
  sprintf(commande, "SR0");
  this->write(commande);

  vpTime::wait(25000);

  this->wait();

  /* suppression de l'echo */
  sprintf(commande, "SE0");
  this->write(commande);

  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
  sprintf(commande, "VW0");
  this->write(commande);
}
void
vpServolens::init() const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }

  char commande[10];

  /* suppression de l'echo */
  sprintf(commande, "SE0");
  this->write(commande);

  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
  sprintf(commande, "VW0");
  this->write(commande);

  /* L'experience montre qu'une petite tempo est utile.     */
  vpTime::wait(500);
}

void 
vpServolens::enableCmdComplete(vpServoType servo, bool active) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
  char commande[10];

  /* Envoie une commande pour qu'en fin de mouvement servolens renvoie
   * une information de fin de mouvement (ex: ZF, FF, DF).
   */
  switch(servo) {
  case ZOOM:
    if (active)
      sprintf(commande, "ZF1");
    else
      sprintf(commande, "ZF0");
    break;
  case FOCUS:
    if (active)
      sprintf(commande, "FF1");
    else
      sprintf(commande, "FF0");
    break;
  case IRIS:
    if (active)
      sprintf(commande, "DF1");
    else
      sprintf(commande, "DF0");
    break;
  }

  /* envoie de la commande */
  this->write(commande);   /* a la fin du mouvement envoie de ZF, FF, DF */
}

void 
vpServolens::enablePrompt(bool active) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
  char commande[10];

  /* suppression de l'echo */
  if (active == true)
    sprintf(commande, "SE1");
  else
    sprintf(commande, "SE0");
    
  this->write(commande);
}

void 
vpServolens::setController(vpControllerType controller) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
  char commande[10];

  switch(controller) {
  case AUTO:
    /* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
    sprintf(commande, "VW1");
    this->write(commande);
    break;
  case CONTROLLED:
    /* nettoyage : mot d'etat vide 0000 */
    sprintf(commande,"SX0842");
    this->write(commande);
    /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
    sprintf(commande, "VW0");
    this->write(commande);
    break;
  case RELEASED:
    sprintf(commande,"SX1084");
    this->write(commande);
    /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
    sprintf(commande, "VW0");
    this->write(commande);
    break;
  }

}

void 
vpServolens::setAutoIris(bool enable) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
  char commande[10];

  if (enable)
    sprintf(commande, "DA1");
  else
    sprintf(commande, "DA0");

  this->write(commande);
}

void
vpServolens::setPosition(vpServoType servo, unsigned int position) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
  char commande[10];

  /* attente du prompt pour envoyer une commande */
  /*
  printf("attente prompt\n");
  this->wait();
  */

#ifdef FINSERVO
  /* envoie des commandes pour qu'en fin de mouvement servolens renvoie */
  /* une commande de fin de mouvement (ex: ZF, FF, DF). */
  this->enableCommandComplete();
#endif  /* FINSERVO */

  // 08/08/00 Fabien S. - Correction de la consigne demandee
  // pour prendre en compte l'erreur entre la consigne demandee
  // et la consigne mesuree.
  // A la consigne du zoom on retranche 1.
  // A la consigne du focus on ajoute 1.
  // A la consigne du iris on ajoute 1.
  switch (servo) {
  case ZOOM:
    //printf("zoom demande: %d ", position);
    position --;
    if (position < ZOOM_MIN) position = ZOOM_MIN;
    //printf("zoom corrige: %d \n", position);
    break;
  case FOCUS:
    //printf("focus demande: %d ", position);
    position ++;
    if (position > FOCUS_MAX) position = FOCUS_MAX;
    //printf("focus corrige: %d \n", position);
    break;
  case IRIS:
    // printf("iris demande: %d ", position);
    position ++;
    if (position > IRIS_MAX) position = IRIS_MAX;
    //printf("iris corrige: %s \n", position);
    break;
  }

  /* commande a envoyer aux servomoteurs */
  switch(servo) {
    case ZOOM:
      sprintf(commande, "ZD%d", position);
      break;
    case FOCUS:
      sprintf(commande, "FD%d", position);
      break;
    case IRIS:
      sprintf(commande, "DD%d", position);
      break;
    }
  /* envoie de la commande */
#ifdef PRINT
  printf("\ncommande: %s", commande);
#endif

  this->write(commande);

#ifdef FINSERVO
  /* on attend la fin du mouvement des objectifs */
  this->wait(servo);  /* on attend les codes ZF, FF, DF */
#endif
}

bool
vpServolens::getPosition(vpServoType servo, unsigned int &position) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
  char commande[10];
  char posit[10], *pt_posit;
  char c;
  short fin_lect_posit; /* indique si on a lu la position du servo-moteur */
  short posit_car;      /* donne la position du caractere lu */
  short lecture_posit_en_cours; /* indique si on lit la position courante */

  /* attente du prompt pour envoyer une commande */
  /*
  this->wait();
  */
  pt_posit = posit;

  /* envoie des commandes pour obtenir la position des servo-moteurs. */
  switch (servo) {
  case ZOOM:
    sprintf(commande, "ZD?");
    break;
  case FOCUS:
    sprintf(commande, "FD?");
    break;
  case IRIS:
    sprintf(commande, "DD?");
    break;
  default:
    break;
  }
  /* envoie de la commande */
  //    printf("\ncommande: %s", commande);

  this->write(commande);

  /* on cherche a lire la position du servo-moteur */
  /* Servolens renvoie une chaine de caractere du type ZD00400 ou FD00234 */
  fin_lect_posit = 0;
  posit_car = 0;
  lecture_posit_en_cours = 0;
  do {
    if (this->read(&c, 1) == true) {

      //    printf("caractere lu: %c\n", c);
      switch (posit_car){
    /* on lit le 1er caractere; (soit Z, soit F, soit D) */
      case 0:
    /* sauvegarde du pointeur */
    pt_posit = posit;

    switch (servo) {
    case ZOOM:
      if( c == 'Z') posit_car = 1;
      break;
    case FOCUS:
      if( c == 'F') posit_car = 1;
      break;
    case IRIS:
      if( c == 'D') posit_car = 1;
      break;
    }
    break;

    /* si le 1er caractere est correct, on lit le 2eme caractere */
    /* (toujours D) */
      case 1:
    if( c == 'D') posit_car = 2;
    else posit_car = 0; /* le 2eme caractere n'est pas correct */
    break;

    /* si on a lu les 2 premiers caracteres, on peut lire la */
    /* position du servo-moteur */
      case 2:
    if (c >= '0' && c <= '9')
    {
      *pt_posit++ = c;        /* sauvegarde de la position */
      lecture_posit_en_cours = 1;
    }
    else if (lecture_posit_en_cours)
    {
      fin_lect_posit = 1;
      *pt_posit = '\0';
    }
    else posit_car = 0;
    break;
      }

    }
    else {
      // Timout sur la lecture, on retoure FALSE
      return false;
    }
  }
  while ( !fin_lect_posit );

  //    printf("\nChaine lue: posit: %s", posit);

  /* toilettage de la position courantes lue */
  this->clean(posit, posit);

  //    printf("\nChaine toilettee: posit: %s", posit);
  position = (unsigned int)atoi(posit);

  return(true);
}

vpCameraParameters 
vpServolens::getCameraParameters(vpImage<unsigned char> &I) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
  vpCameraParameters cam;
  double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
  double px=1000, py=1000, u0=320, v0=240;
  // Determine if the image is subsampled.
  // Dragonfly2 native images are 640 by 480
  double subsample_factor = 1.;
  double width = I.getWidth();
  double height= I.getHeight();


  if (width > 300 && width < 340 && height > 220 && height < 260)
    subsample_factor = 2;
  else if (width > 140 && width < 1800 && height > 100 && height < 140)
    subsample_factor = 4;

  unsigned zoom;
  getPosition(vpServolens::ZOOM, zoom);
  //std::cout << "Actual zoom value: " << zoom << std::endl;

  // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale correspondent
  // aux parametres de calibration de la camera (donnees constructeur) pour des
  // tailles d'images CCIR (768x576), donc avec scale = 1.
  double focale = zoom * 1.0e-5; // Transformation en metres
  px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
  py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
  u0 = I.getWidth() / 2.;
  v0 = I.getHeight() / 2.;
  cam.initPersProjWithoutDistortion(px, py, u0, v0);

  return cam;
}

char 
vpServolens::wait() const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
    
  ssize_t r;
  r = ::write(this->remfd, "\r\n", strlen("\r\n"));
  if (r != (ssize_t)(strlen("\r\n"))) {
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot write on Servolens.");
  }
  char c;
  do {
    r = ::read(this->remfd, &c, 1);
    c &= 0x7f;
  }
  while (c != '>');
  return c;

}

void
vpServolens::wait(vpServoType servo) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }

  char c;
  char fin_mvt[3];
  bool sortie = false;

  switch (servo) {
  case ZOOM:
    sprintf(fin_mvt, "ZF");
    break;
  case FOCUS:
    sprintf(fin_mvt, "FF");
    break;
  case IRIS:
  default:
    sprintf(fin_mvt, "DF");
    break;
    
  }

  /* lecture des caracteres recus */
  do {
    /* lecture des caracteres */
    if (::read(this->remfd,&c,1) != 1) {
      throw vpRobotException (vpRobotException::communicationError,
                  "Cannot read on Servolens.");
    }
    c &= 0x7f;

    /* tests si fin de mouvement */
    if (c == fin_mvt[0]) {
      /* lecture du caractere suivant */
      if (::read(this->remfd,&c,1) != 1) {
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot read on Servolens.");
      }

      c &= 0x7f;
      if (c == fin_mvt[1]) {
    sortie = true;
      }
    }
  }
  while ( !sortie);

  /*  printf("\nmouvement fini: chaine lue = %s", chaine); */
}

bool 
vpServolens::read(char *c, long timeout_s) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }

  int n;
  fd_set         readfds; /* list of fds for select to listen to */
  struct timeval timeout = {timeout_s, 0}; // seconde, micro-sec

  FD_ZERO(&readfds); 
  FD_SET(static_cast<unsigned int>(this->remfd), &readfds);

  if (select(FD_SETSIZE, &readfds, (fd_set *)NULL,
         (fd_set *)NULL, &timeout) > 0) {
    n = ::read(this->remfd, c, 1); /* read one character at a time */
    if (n != 1)
      return false;
    *c &= 0x7f;
    //printf("lecture 1 car: %c\n", *c);
    return(true);
  }

  return (false);
}

void 
vpServolens::write(const char *s) const
{
  if (!isinit) {
    vpERROR_TRACE ("Cannot dial with Servolens.");
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot dial with Servolens.");
  }
  ssize_t r = 0;
  r = ::write(this->remfd,"\r", strlen("\r"));
  r += ::write(this->remfd, s, strlen(s));
  r += ::write(this->remfd,"\r", strlen("\r"));
  if (r != (ssize_t)(2*strlen("\r") + strlen(s))) {
    throw vpRobotException (vpRobotException::communicationError,
                "Cannot write on Servolens.");
  }


  /*
   * Une petite tempo pour laisser le temps a la liaison serie de
   * digerer la commande envoyee. En fait, la liaison serie fonctionne
   * a 9600 bauds soit une transmission d'environ 9600 bits pas seconde.
   * Les plus longues commandes envoyees sur la liaison serie sont du type:
   * SX0842 soit 6 caracteres codes sur 8 bits chacuns = 48 bits pour
   * envoyer la commande SX0842.
   * Ainsi, le temps necessaire pour envoyer SX0842 est d'environ
   * 48 / 9600 = 0,0050 secondes = 5 milli secondes.
   * Ici on rajoute une marge pour amener la tempo a 20 ms.
   */
  vpTime::wait(20);
}

bool 
vpServolens::clean(const char *in, char *out) const
{
  short nb_car, i=0;
  bool error = false;

  nb_car = strlen(in);

  /* on se positionne sur le 1er caractere different de zero */
  while( *(in) == '0' && i++ < nb_car ) {
    in++;
    if (i == nb_car)
      {
    error = true; /* la chaine ne contient pas une position */
    *(out++) = '0'; /* mise a zero de la position */
      }
  }

  /* copie de la position epuree des zeros de gauche */
  while( i++ <= nb_car ) { /* on a mis le = pour copier le caractere de fin */
    /* de chaine \0 */
    *(out++) = *(in++);
  }
  return (error);
}

#endif