dp_ptu.cpp

00001 
00002 /***************************************************************************
00003  *  dp_ptu.cpp - Controller for Directed Perception, Inc. Pan-Tilt Unit on B21
00004  *
00005  *  Created: Wed Nov 29 23:05:49 2006
00006  *  Copyright  2005-2009  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 #include "dp_ptu.h"
00025 
00026 #include <core/exceptions/system.h>
00027 #include <utils/math/angle.h>
00028 
00029 #include <sys/types.h>
00030 #include <sys/stat.h>
00031 #include <fcntl.h>
00032 #include <unistd.h>
00033 #include <sys/ioctl.h>
00034 #include <sys/time.h>
00035 #include <termios.h>
00036 #include <cstdio>
00037 #include <cstdlib>
00038 #include <cstring>
00039 #include <cerrno>
00040 
00041 using namespace std;
00042 using namespace fawkes;
00043 
00044 /** @class DirectedPerceptionPTU "dp_ptu.h"
00045  * DirectedPerception PTU implementation.
00046  * Control object to use the DirectedPerception PTU Pan/Tilt unit mounted
00047  * on carl.
00048  *
00049  * @author Tim Niemueller
00050  */
00051 
00052 const char * DirectedPerceptionPTU::DPPTU_PAN_ABSPOS             = "PP";
00053 const char * DirectedPerceptionPTU::DPPTU_TILT_ABSPOS            = "TP";
00054 const char * DirectedPerceptionPTU::DPPTU_PAN_RELPOS             = "PO";
00055 const char * DirectedPerceptionPTU::DPPTU_TILT_RELPOS            = "TO";
00056 const char * DirectedPerceptionPTU::DPPTU_PAN_RESOLUTION         = "PR";
00057 const char * DirectedPerceptionPTU::DPPTU_TILT_RESOLUTION        = "TR";
00058 const char * DirectedPerceptionPTU::DPPTU_PAN_MIN                = "PN";
00059 const char * DirectedPerceptionPTU::DPPTU_PAN_MAX                = "PX";
00060 const char * DirectedPerceptionPTU::DPPTU_TILT_MIN               = "TN";
00061 const char * DirectedPerceptionPTU::DPPTU_TILT_MAX               = "TX";
00062 const char * DirectedPerceptionPTU::DPPTU_LIMITENFORCE_QUERY     = "L";
00063 const char * DirectedPerceptionPTU::DPPTU_LIMITENFORCE_ENABLE    = "LE";
00064 const char * DirectedPerceptionPTU::DPPTU_LIMITENFORCE_DISABLE   = "LD";
00065 const char * DirectedPerceptionPTU::DPPTU_IMMEDIATE_EXECUTION    = "I";
00066 const char * DirectedPerceptionPTU::DPPTU_SLAVED_EXECUTION       = "S";
00067 const char * DirectedPerceptionPTU::DPPTU_AWAIT_COMPLETION       = "A";
00068 const char * DirectedPerceptionPTU::DPPTU_HALT_ALL               = "H";
00069 const char * DirectedPerceptionPTU::DPPTU_HALT_PAN               = "HP";
00070 const char * DirectedPerceptionPTU::DPPTU_HALT_TILT              = "HT";
00071 const char * DirectedPerceptionPTU::DPPTU_PAN_SPEED              = "PS";
00072 const char * DirectedPerceptionPTU::DPPTU_TILT_SPEED             = "TS";
00073 const char * DirectedPerceptionPTU::DPPTU_PAN_ACCEL              = "PA";
00074 const char * DirectedPerceptionPTU::DPPTU_TILT_ACCEL             = "TA";
00075 const char * DirectedPerceptionPTU::DPPTU_PAN_BASESPEED          = "PB";
00076 const char * DirectedPerceptionPTU::DPPTU_TILT_BASESPEED         = "TB";
00077 const char * DirectedPerceptionPTU::DPPTU_PAN_UPPER_SPEED_LIMIT  = "PU";
00078 const char * DirectedPerceptionPTU::DPPTU_PAN_LOWER_SPEED_LIMIT  = "PL";
00079 const char * DirectedPerceptionPTU::DPPTU_TILT_UPPER_SPEED_LIMIT = "TU";
00080 const char * DirectedPerceptionPTU::DPPTU_TILT_LOWER_SPEED_LIMIT = "TL";
00081 const char * DirectedPerceptionPTU::DPPTU_RESET                  = "R";
00082 const char * DirectedPerceptionPTU::DPPTU_STORE                  = "DS";
00083 const char * DirectedPerceptionPTU::DPPTU_RESTORE                = "DR";
00084 const char * DirectedPerceptionPTU::DPPTU_FACTORY_RESET          = "DF";
00085 const char * DirectedPerceptionPTU::DPPTU_ECHO_QUERY             = "E";
00086 const char * DirectedPerceptionPTU::DPPTU_ECHO_ENABLE            = "EE";
00087 const char * DirectedPerceptionPTU::DPPTU_ECHO_DISABLE           = "ED";
00088 const char * DirectedPerceptionPTU::DPPTU_ASCII_VERBOSE          = "FV";
00089 const char * DirectedPerceptionPTU::DPPTU_ASCII_TERSE            = "FT";
00090 const char * DirectedPerceptionPTU::DPPTU_ASCII_QUERY            = "F";
00091 const char * DirectedPerceptionPTU::DPPTU_VERSION                = "V";
00092 
00093 
00094 /** Constructor.
00095  * @param device_file serial device file (e.g. /dev/ttyS0)
00096  * @param timeout_ms timeout for read operations in miliseconds
00097  */
00098 DirectedPerceptionPTU::DirectedPerceptionPTU(const char *device_file,
00099                                              unsigned int timeout_ms)
00100 {
00101   __device_file = strdup(device_file);
00102   __opened      = false;
00103   __timeout_ms  = timeout_ms;
00104 
00105   open();
00106 }
00107 
00108 
00109 
00110 /** Destructor. */
00111 DirectedPerceptionPTU::~DirectedPerceptionPTU()
00112 {
00113   close();
00114   free(__device_file);
00115 }
00116 
00117 
00118 void
00119 DirectedPerceptionPTU::open()
00120 {
00121   if (__opened) return;
00122 
00123   __fd = ::open(__device_file, O_RDWR | O_NOCTTY | O_NONBLOCK);
00124   if ( ! __fd || ! isatty(__fd)) {
00125     throw Exception("Cannot open device or device is not a TTY");
00126   }
00127 
00128   struct termios param;
00129 
00130   if (tcgetattr(__fd, &param) != 0) {
00131     ::close(__fd);
00132     throw Exception("DP PTU: Cannot get parameters");;
00133   }
00134 
00135   if ( cfsetspeed( &param, B9600 ) == -1 ) {
00136     ::close( __fd );
00137     throw Exception("DP PTU: Cannot set speed");;
00138   }
00139 
00140   cfsetospeed(&param, B9600);
00141   cfsetispeed(&param, B9600);
00142 
00143   // set serial line options
00144   param.c_cflag |= ( CLOCAL | CREAD );  // set to local and enable the receiver
00145   param.c_cflag &= ~CSIZE;              // mask character size bits
00146   param.c_cflag |= CS8;                 // select 8 data bits
00147   param.c_cflag &= ~PARENB;             // no parity
00148   param.c_cflag &= ~CSTOPB;             // 1 stop bit
00149 
00150   // set input options
00151   param.c_iflag &= ~( INPCK | ISTRIP ); // no input parity checking
00152   param.c_iflag &= ~( IXON | IXOFF | IXANY ); // no software flow control
00153 
00154   param.c_lflag &= ~( ICANON | ECHO | ECHOE | ISIG );
00155 
00156   param.c_cc[ VTIME ] = 1;  // wait for a tenth of a second for data
00157   param.c_cc[ VMIN ] = 0;
00158 
00159   if (tcsetattr(__fd, TCSANOW, &param) != 0) {
00160     ::close(__fd);
00161     throw Exception("DP PTU: Cannot set parameters");;
00162   }
00163 
00164   // get initial values
00165   send(DPPTU_RESTORE);
00166   send(DPPTU_ECHO_DISABLE);
00167   send(DPPTU_ASCII_TERSE);
00168 
00169   send(DPPTU_RESET);
00170 
00171   __pan_resolution   = query_int(DPPTU_PAN_RESOLUTION);
00172   __tilt_resolution  = query_int(DPPTU_TILT_RESOLUTION);
00173 
00174   __pan_upper_limit  = query_int(DPPTU_PAN_MAX);
00175   __pan_lower_limit  = query_int(DPPTU_PAN_MIN);
00176   __tilt_upper_limit = query_int(DPPTU_TILT_MAX);
00177   __tilt_lower_limit = query_int(DPPTU_TILT_MIN);
00178 
00179   __opened = true;
00180 }
00181 
00182 
00183 void
00184 DirectedPerceptionPTU::close()
00185 {
00186   if (__opened) {
00187     ::close(__fd);
00188     __opened = false;
00189   }
00190 }
00191 
00192 
00193 /** Stop currently running motion. */
00194 void
00195 DirectedPerceptionPTU::stop_motion()
00196 {
00197   send(DPPTU_HALT_ALL);
00198 }
00199 
00200 
00201 /** Set pan in motor ticks.
00202  * @param pan pan position in ticks
00203  */
00204 void
00205 DirectedPerceptionPTU::set_pan(int pan)
00206 {
00207   send(DPPTU_PAN_ABSPOS, pan);
00208 }
00209 
00210 
00211 /** Set tilt in motor ticks.
00212  * @param tilt tilt position in ticks
00213  */
00214 void
00215 DirectedPerceptionPTU::set_tilt(int tilt)
00216 {
00217   send(DPPTU_TILT_ABSPOS, tilt);
00218 }
00219 
00220 
00221 /** Set pan and tilt in motor ticks.
00222  * @param pan pan position in ticks
00223  * @param tilt tilt position in ticks
00224  */
00225 void
00226 DirectedPerceptionPTU::set_pan_tilt(int pan, int tilt)
00227 {
00228   if ( pan  > __pan_upper_limit  )  pan  = __pan_upper_limit;
00229   if ( pan  < __pan_lower_limit  )  pan  = __pan_lower_limit;
00230   if ( tilt > __tilt_upper_limit )  tilt = __tilt_upper_limit;
00231   if ( tilt < __tilt_lower_limit )  tilt = __tilt_lower_limit;
00232 
00233   send(DPPTU_PAN_ABSPOS, pan);
00234   send(DPPTU_TILT_ABSPOS, tilt);
00235 }
00236 
00237 
00238 /** Set pan and tilt in radians.
00239  * @param pan pan position rad
00240  * @param tilt tilt position rad
00241  */
00242 void
00243 DirectedPerceptionPTU::set_pan_tilt_rad(float pan, float tilt)
00244 {
00245   set_pan_tilt(pan_rad2ticks(pan), tilt_rad2ticks(tilt));
00246 }
00247 
00248 
00249 /** Get current position in motor ticks.
00250  * @param pan upon return contains current pan position in motor ticks
00251  * @param tilt upon return contains current tilt position in motor ticks
00252  */
00253 void
00254 DirectedPerceptionPTU::get_pan_tilt(int &pan, int &tilt)
00255 {
00256   pan  = query_int(DPPTU_PAN_ABSPOS);
00257   tilt = query_int(DPPTU_TILT_ABSPOS);
00258 }
00259 
00260 
00261 /** Get pan/tilt in radians.
00262  * @param pan upon return contains current pan position in radians
00263  * @param tilt upon return contains current tilt position in radians
00264  */
00265 void
00266 DirectedPerceptionPTU::get_pan_tilt_rad(float &pan, float &tilt)
00267 {
00268   int tpan = 0, ttilt = 0;
00269 
00270   tpan  = query_int(DPPTU_PAN_ABSPOS);
00271   ttilt = query_int(DPPTU_TILT_ABSPOS);
00272 
00273   pan  = pan_ticks2rad(tpan);
00274   tilt = tilt_ticks2rad(ttilt);
00275 }
00276 
00277 
00278 /** Get current pan in motor ticks.
00279  * @return current pan in motor ticks
00280  */
00281 int
00282 DirectedPerceptionPTU::get_pan()
00283 {
00284   return query_int(DPPTU_PAN_ABSPOS);
00285 }
00286 
00287 
00288 /** Get current tilt in motor ticks.
00289  * @return current tilt in motor ticks
00290  */
00291 int
00292 DirectedPerceptionPTU::get_tilt()
00293 {
00294   return query_int(DPPTU_TILT_ABSPOS);
00295 }
00296 
00297 /** Get maximum pan in motor ticks.
00298  * @return maximum pan in motor ticks
00299  */
00300 int
00301 DirectedPerceptionPTU::max_pan()
00302 {
00303   return __pan_upper_limit;
00304 }
00305 
00306 
00307 /** Get minimum pan in motor ticks.
00308  * @return minimum pan in motor ticks
00309  */
00310 int
00311 DirectedPerceptionPTU::min_pan()
00312 {
00313   return __pan_lower_limit;
00314 
00315 }
00316 
00317 
00318 /** Get maximum tilt in motor ticks.
00319  * @return maximum tilt in motor ticks
00320  */
00321 int
00322 DirectedPerceptionPTU::max_tilt()
00323 {
00324   return __tilt_upper_limit;
00325 }
00326 
00327 
00328 /** Get minimum tilt in motor ticks.
00329  * @return minimum tilt in motor ticks
00330  */
00331 int
00332 DirectedPerceptionPTU::min_tilt()
00333 {
00334   return __tilt_lower_limit;
00335 }
00336 
00337 
00338 /** Get position limits in radians.
00339  * @param pan_min upon return contains minimum pan in radians
00340  * @param pan_max upon return contains maximum pan in radians
00341  * @param tilt_min upon return contains minimum tilt in radians
00342  * @param tilt_max upon return contains maximum tilt in radians
00343  */
00344 void
00345 DirectedPerceptionPTU::get_limits(float &pan_min, float &pan_max,
00346                                   float &tilt_min, float &tilt_max)
00347 {
00348   pan_min  = pan_ticks2rad(__pan_lower_limit);
00349   pan_max  = pan_ticks2rad(__tilt_upper_limit);
00350   tilt_min = tilt_ticks2rad(__tilt_lower_limit);
00351   tilt_max = tilt_ticks2rad(__tilt_upper_limit);
00352 }
00353 
00354 
00355 /** Reset the PTU. */
00356 void
00357 DirectedPerceptionPTU::reset()
00358 {
00359   send(DPPTU_RESET);
00360 }
00361 
00362 
00363 void
00364 DirectedPerceptionPTU::send(const char *command, int value)
00365 {
00366   snprintf(__obuffer, DPPTU_MAX_OBUFFER_SIZE, "%s%i ", command, value);
00367   write(__obuffer);
00368   if ( ! result_ok() ) {
00369     printf("Writing with value '%s' to PTU failed\n", __obuffer);
00370   }
00371 }
00372 
00373 
00374 void
00375 DirectedPerceptionPTU::send(const char *command)
00376 {
00377   snprintf(__obuffer, DPPTU_MAX_OBUFFER_SIZE, "%s ", command);
00378   write(__obuffer);
00379   if ( ! result_ok() ) {
00380     printf("Writing '%s' to PTU failed\n", __obuffer);
00381   }
00382 }
00383 
00384 
00385 void
00386 DirectedPerceptionPTU::write(const char *buffer)
00387 {
00388   printf("Writing '%s'\n", __obuffer);
00389 
00390   tcflush( __fd, TCIOFLUSH );
00391   unsigned int buffer_size = strlen(buffer);
00392   int written = ::write(__fd, buffer, buffer_size);
00393   tcdrain(__fd);
00394 
00395   if (written < 0) {
00396     printf("Writing '%s' failed: %s\n", buffer, strerror(errno));
00397   } else if ((unsigned int)written != buffer_size) {
00398     printf("Writing '%s' failed, only wrote %i of %u bytes\n", buffer, written, buffer_size);
00399   }
00400 }
00401 
00402 
00403 bool
00404 DirectedPerceptionPTU::read(char *buffer, unsigned int buffer_size)
00405 {
00406   // wait for message
00407   timeval start, now;
00408   unsigned int diff_msec = 0;
00409   gettimeofday(&start, NULL);
00410 
00411   int num_bytes = 0;
00412   ioctl(__fd, FIONREAD, &num_bytes);
00413   while ( ((__timeout_ms == 0) || (diff_msec < __timeout_ms)) && (num_bytes == 0)) {
00414     ioctl(__fd, FIONREAD, &num_bytes);
00415 
00416     gettimeofday(&now, NULL);
00417     diff_msec  = (now.tv_sec  - start.tv_sec) * 1000 + (now.tv_usec - start.tv_usec) / 1000;
00418     usleep(__timeout_ms * 100);
00419   }
00420   if (num_bytes == 0) {
00421     return false;
00422   }
00423   int bytes_read = ::read(__fd, buffer, buffer_size);
00424   if ( bytes_read < 0 ) {
00425     return false;
00426   } else {
00427     if ((unsigned int)bytes_read == buffer_size) {
00428       return true;
00429     } else {
00430       return false;
00431     }
00432   }
00433 }
00434 
00435 
00436 bool
00437 DirectedPerceptionPTU::result_ok()
00438 {
00439   if ( read(__ibuffer, 1) ) {
00440     if ( __ibuffer[0] == '*' ) {
00441       return true;
00442     }
00443   }
00444 
00445   return false;
00446 }
00447 
00448 
00449 bool
00450 DirectedPerceptionPTU::data_available()
00451 {
00452   int num_bytes = 0;
00453   ioctl(__fd, FIONREAD, &num_bytes);
00454   return (num_bytes > 0);
00455 }
00456 
00457 
00458 int
00459 DirectedPerceptionPTU::query_int(const char *query_command)
00460 {
00461   send(query_command);
00462   ssize_t read_bytes = read(__ibuffer, DPPTU_MAX_OBUFFER_SIZE);
00463   if ( read_bytes == -1 ) {
00464     throw FileReadException(__device_file, errno, "Querying integer from PTU failed");
00465   } else if (read_bytes == 0) {
00466     return 0;
00467   }
00468   int rv = 0;
00469   sscanf(__ibuffer, "* %i", &rv);
00470   return rv;
00471 }
00472 
00473 
00474 int
00475 DirectedPerceptionPTU::pan_rad2ticks(float r)
00476 {
00477   if ( __pan_resolution == 0 )  return 0;
00478   return (int)rint(rad2deg(r) * 3600 / __pan_resolution);
00479 }
00480 
00481 
00482 int
00483 DirectedPerceptionPTU::tilt_rad2ticks(float r)
00484 {
00485   if ( __tilt_resolution == 0 )  return 0;
00486   return (int)rint(rad2deg(r) * 3600 / __tilt_resolution);
00487 }
00488 
00489 
00490 float
00491 DirectedPerceptionPTU::pan_ticks2rad(int ticks)
00492 {
00493   if ( __pan_resolution == 0 )  return 0;
00494   return deg2rad(ticks * __pan_resolution / 3600);
00495 }
00496 
00497 
00498 float
00499 DirectedPerceptionPTU::tilt_ticks2rad(int ticks)
00500 {
00501   if ( __tilt_resolution == 0 )  return 0;
00502   return deg2rad(ticks * __tilt_resolution / 3600);
00503 }

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1