evid100p.cpp

00001 
00002 /***************************************************************************
00003  *  evid100p.cpp - Sony EviD100P Visca wrapper
00004  *
00005  *  Created: Sun Jun 21 13:10:51 2009
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 "evid100p.h"
00025 
00026 #include <core/exceptions/software.h>
00027 #include <utils/math/angle.h>
00028 
00029 using namespace fawkes;
00030 
00031 /** @class SonyEviD100PVisca "evid100p.h"
00032  * Sony EviD100P Visca controller.
00033  * This sub-class using the Visca protocol contains some constants specific
00034  * for the Sony EviD100P camera.
00035  * @author Tim Niemueller
00036  */
00037 
00038 /** Maximum pan. */
00039 const int   SonyEviD100PVisca::MAX_PAN       =  1440;
00040 /** Minimum pan. */
00041 const int   SonyEviD100PVisca::MIN_PAN       = -1439;
00042 /** Max Tilt. */
00043 const int   SonyEviD100PVisca::MAX_TILT      =   360;
00044 /** Min tilt .*/
00045 const int   SonyEviD100PVisca::MIN_TILT      = - 359;
00046 
00047 /** Max pan in degrees. */
00048 const float SonyEviD100PVisca::MAX_PAN_DEG   =  100.f;
00049 /** Min pan in degrees. */
00050 const float SonyEviD100PVisca::MIN_PAN_DEG   = -100.f;
00051 /** Max tilt in degrees. */
00052 const float SonyEviD100PVisca::MAX_TILT_DEG  =   25.f;
00053 /** Min tilt in degrees. */
00054 const float SonyEviD100PVisca::MIN_TILT_DEG  = - 25.f;
00055 
00056 /** Max pan in rad. */
00057 const float SonyEviD100PVisca::MAX_PAN_RAD   = deg2rad(MAX_PAN_DEG);
00058 /** Min pan in rad. */
00059 const float SonyEviD100PVisca::MIN_PAN_RAD   = deg2rad(MIN_PAN_DEG);
00060 /** Max tilt in rad. */
00061 const float SonyEviD100PVisca::MAX_TILT_RAD  = deg2rad(MAX_TILT_DEG);
00062 /** Min tilt in rad. */
00063 const float SonyEviD100PVisca::MIN_TILT_RAD  = deg2rad(MIN_TILT_DEG);
00064 
00065 /** Pan steps per degree */
00066 const float SonyEviD100PVisca::PAN_STEPS_PER_DEG  = MAX_PAN  / MAX_PAN_DEG;
00067 /** Tilt steps per degree */
00068 const float SonyEviD100PVisca::TILT_STEPS_PER_DEG = MAX_TILT / MAX_TILT_DEG;
00069 
00070 /** Pan steps per rad */
00071 const float SonyEviD100PVisca::PAN_STEPS_PER_RAD  = MAX_PAN  / MAX_PAN_RAD;
00072 /** Tilt steps per rad */
00073 const float SonyEviD100PVisca::TILT_STEPS_PER_RAD = MAX_TILT / MAX_TILT_RAD;
00074 
00075 /** Pastel effect. */
00076 const unsigned int SonyEviD100PVisca::EFFECT_PASTEL   = 1;
00077 /** Negative effect. */
00078 const unsigned int SonyEviD100PVisca::EFFECT_NEGATIVE = 2;
00079 /** Sepia effect. */
00080 const unsigned int SonyEviD100PVisca::EFFECT_SEPIA    = 3;
00081 /** B/W effect. */
00082 const unsigned int SonyEviD100PVisca::EFFECT_BW       = 4;
00083 /** Solarize effect. */
00084 const unsigned int SonyEviD100PVisca::EFFECT_SOLARIZE = 5;
00085 /** Mosaic effect. */
00086 const unsigned int SonyEviD100PVisca::EFFECT_MOSAIC   = 6;
00087 /** Slim effect. */
00088 const unsigned int SonyEviD100PVisca::EFFECT_SLIM     = 7;
00089 /** Stretch effect. */
00090 const unsigned int SonyEviD100PVisca::EFFECT_STRETCH  = 8;
00091 
00092 
00093 /** Speed table for supported pan speed values in radians.
00094  * Has been created empirically.
00095  */
00096 const float SonyEviD100PVisca::SPEED_TABLE_PAN[] =
00097   {0.03548, 0.04138, 0.05319, 0.06497, 0.08262, 0.10608, 0.12951, 0.15865,
00098    0.19933, 0.24535, 0.30159, 0.35137, 0.43540, 0.53611, 0.67246, 0.81519,
00099    0.99870, 1.20673, 1.45304, 1.70703, 1.99278, 2.25729, 2.44293, 2.71852};
00100 
00101 /** Speed table for supported tilt speed values in radians.
00102  * Has been created empirically.
00103  */
00104 const float SonyEviD100PVisca::SPEED_TABLE_TILT[] =
00105   {0.03541, 0.04127, 0.05298, 0.06449, 0.08195, 0.10480, 0.12741, 0.15535,
00106    0.19356, 0.23685, 0.28438, 0.33367, 0.41066, 0.49517, 0.59622, 0.71474,
00107    0.83085, 0.97431, 1.08745, 1.20977};
00108 
00109 
00110 /** Constructor.
00111  * @param device_file serial device file (e.g. /dev/ttyUSB0)
00112  * @param def_timeout_ms default read timeout, used if no specific timeout
00113  * is passed
00114  * @param blocking true to make gathering pan/tilt information wait for
00115  * the reponse, false to be able to split the operation
00116  */
00117 SonyEviD100PVisca::SonyEviD100PVisca(const char *device_file,
00118                                      unsigned int def_timeout_ms,
00119                                      bool blocking)
00120   : Visca(device_file, def_timeout_ms, blocking)
00121 {
00122 }
00123 
00124 
00125 /** Destructor. */
00126 SonyEviD100PVisca::~SonyEviD100PVisca()
00127 {
00128 }
00129 
00130 
00131 /** Set pan/tilt in radians.
00132  * @param pan pan value in radians
00133  * @param tilt tilt value in radians
00134  */
00135 void
00136 SonyEviD100PVisca::set_pan_tilt_rad(float pan, float tilt)
00137 {
00138   if ( (pan < MIN_PAN_RAD) || (pan > MAX_PAN_RAD) ) {
00139     throw OutOfBoundsException("Illegal pan value", pan, MIN_PAN_RAD, MAX_PAN_RAD);
00140   }
00141   if ( (tilt < MIN_TILT_RAD) || (tilt > MAX_TILT_RAD) ) {
00142     throw OutOfBoundsException("Illegal tilt value", tilt, MIN_TILT_RAD, MAX_TILT_RAD);
00143   }
00144 
00145   int tpan = 0, ttilt = 0;
00146 
00147   tpan = (int)rint(  pan  * PAN_STEPS_PER_RAD  );
00148   ttilt = (int)rint( tilt * TILT_STEPS_PER_RAD );
00149 
00150   set_pan_tilt(tpan, ttilt);
00151 }
00152 
00153 
00154 /** Get pan/tilt in radians.
00155  * @param pan upon return contains the current pan value
00156  * @param tilt upone return contains the current tilt value
00157  */
00158 void
00159 SonyEviD100PVisca::get_pan_tilt_rad(float &pan, float &tilt)
00160 {
00161   int tpan = 0, ttilt = 0;
00162   get_pan_tilt(tpan, ttilt);
00163 
00164   pan  = tpan  / PAN_STEPS_PER_RAD;
00165   tilt = ttilt / PAN_STEPS_PER_RAD;
00166 }
00167 
00168 
00169 /** Set speed given in rad/sec.
00170  * Note that not the exact speed is taken, but rather the closes equivalent in
00171  * motor ticks is taken.
00172  * @param pan_speed desired pan speed in rad/sec
00173  * @param tilt_speed desired tilt speed in rad/sec
00174  * @exception OutOfBoundsException thrown if desired speed is out of range
00175  */
00176 void
00177 SonyEviD100PVisca::set_speed_radsec(float pan_speed, float tilt_speed)
00178 {
00179   if ( (pan_speed < 0) ||
00180        (pan_speed > SPEED_TABLE_PAN[SONY_EVID100P_NUM_PAN_SPEEDS - 1]) ) {
00181     throw OutOfBoundsException("Illegal pan speed", pan_speed, 0,
00182                                SPEED_TABLE_PAN[SONY_EVID100P_NUM_PAN_SPEEDS - 1]);
00183   }
00184   if ( (tilt_speed < 0) ||
00185        (tilt_speed > SPEED_TABLE_TILT[SONY_EVID100P_NUM_TILT_SPEEDS - 1]) ) {
00186     throw OutOfBoundsException("Illegal tilt speed", tilt_speed, 0,
00187                                SPEED_TABLE_TILT[SONY_EVID100P_NUM_TILT_SPEEDS - 1]);
00188   }
00189 
00190   unsigned int pan_ind = SONY_EVID100P_NUM_PAN_SPEEDS - 1;
00191   float min_pan_dist = SPEED_TABLE_PAN[pan_ind];
00192   float last_dist = min_pan_dist;;
00193   for (unsigned int i = 0; i < SONY_EVID100P_NUM_PAN_SPEEDS; ++i) {
00194     float dist = 0;
00195     if ( (dist = fabs(pan_speed - SPEED_TABLE_PAN[i])) < min_pan_dist ) {
00196       min_pan_dist = dist;
00197       pan_ind = i;
00198     } else if (dist > last_dist) {
00199       break; // times are growing now, found best
00200     }
00201     last_dist = dist;
00202   }
00203 
00204   unsigned int tilt_ind = SONY_EVID100P_NUM_TILT_SPEEDS - 1;
00205   float min_tilt_dist = SPEED_TABLE_TILT[tilt_ind];
00206   last_dist = min_tilt_dist;
00207   for (unsigned int i = 0; i < SONY_EVID100P_NUM_TILT_SPEEDS; ++i) {
00208     float dist = 0;
00209     if ( (dist = fabs(tilt_speed - SPEED_TABLE_TILT[i])) < min_tilt_dist ) {
00210       min_tilt_dist = dist;
00211       tilt_ind = i;
00212     } else if (dist > last_dist) {
00213       break; // times are growing now, found best
00214     }
00215     last_dist = dist;
00216   }
00217 
00218   set_pan_tilt_speed(pan_ind, tilt_ind);
00219 }
00220 
00221 
00222 /** Get current speed in rad/sec.
00223  * @param pan_speed upon return contains pan speed in rad/sec
00224  * @param tilt_speed upon return contains tilt speed in rad/sec
00225  */
00226 void
00227 SonyEviD100PVisca::get_speed_radsec(float &pan_speed, float &tilt_speed)
00228 {
00229   unsigned char ps, ts;
00230   get_pan_tilt_speed(ps, ts);
00231   pan_speed = SPEED_TABLE_PAN[ps - 1];
00232   tilt_speed = SPEED_TABLE_TILT[ps - 1];
00233 }
00234 
00235 
00236 /** Get speed limits.
00237  * @param pan_min minimum pan speed possible
00238  * @param pan_max maximum pan speed possible
00239  * @param tilt_min minimum tilt speed possible
00240  * @param tilt_max maximum tilt speed possible
00241  */
00242 void
00243 SonyEviD100PVisca::get_speed_limits(float &pan_min, float &pan_max,
00244                                     float &tilt_min, float &tilt_max)
00245 {
00246   pan_min = SPEED_TABLE_PAN[0];
00247   pan_max = SPEED_TABLE_PAN[SONY_EVID100P_NUM_PAN_SPEEDS - 1];
00248   tilt_min = SPEED_TABLE_TILT[0];
00249   tilt_max = SPEED_TABLE_TILT[SONY_EVID100P_NUM_TILT_SPEEDS - 1];
00250 }

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1