Fawkes API Fawkes Development Version
|
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 }