WURDE: /home/fwph/code/wurde/modules/ptu/ptuServer.cpp Source File

00001 #include <CommsManager.H>
00002 #include <PTUnit.H>
00003 #include <Time.H>
00004 
00005 #include "ptu.h"
00006 #include "ptu_types.h"
00007 
00008 using namespace WURDE;
00009 
00010 #define ANG_CONVERSION (3.086 * 2.0 * M_PI / 21600.0)
00011 #define PTU_DEV  "/dev/ttyR2"
00012 #define PAN_TOL .001
00013 #define TILT_TOL .001
00014 
00015 portstream_fd _fd;
00016 float pan_goal;
00017 float tilt_goal;
00018 bool pan_has_goal=false;
00019 bool tilt_has_goal=false;
00020 
00021 void initialize (void);
00022 void cancel_all(void);
00023 void pan_to (double rad);
00024 void pan (double rad);
00025 void tilt_to (double rad);
00026 void tilt (double rad);
00027 void set_pan_vel (double radps);
00028 void set_pan_accel (double radpss);
00029 void set_pan_upper (double rad);
00030 void set_tilt_vel (double radps);
00031 void set_tilt_accel (double radpss);
00032 void set_tilt_upper (double rad);
00033 
00034 int main(int argc, char *argv[]){
00035      
00036      CommsManager myManager("PTUServer");
00037      myManager.setRealName("PTUServer");
00038      myManager.setHelpString("This module provides access to Directed Perception Pan-Tilt units.\n");
00039      myManager.parseOptions(argc,argv);
00040 
00041      PTUnit myPTUnit("TomPTU");
00042      Logger myLogger("PTUServer");
00043      bool running=true;
00044 
00045 
00046      if(myManager.getState()!=STATE_INFO){
00047             initialize();
00048             g_debug("Pan tilt unit initialized");
00049      }else{
00050             g_debug("PTU Server in registration mode");
00051             running=false;
00052      }
00053 
00054      myManager.registerSupplier(&myPTUnit);    
00055      myManager.setSleep(0.05);
00056 
00057      if(myManager.getState()!=STATE_INFO){
00058             myPTUnit.info.pan.setValue(get_current(PAN,POSITION)*ANG_CONVERSION);
00059             myPTUnit.info.tilt.setValue(get_current(TILT,POSITION)*ANG_CONVERSION);
00060 
00061             myPTUnit.info.pan_vel.setValueAndLock(get_desired(PAN,SPEED)*ANG_CONVERSION);
00062             myPTUnit.info.tilt_vel.setValueAndLock(get_desired(TILT,SPEED)*ANG_CONVERSION);
00063 
00064             myPTUnit.info.pan_accel.setValueAndLock(get_desired(PAN,ACCELERATION)*ANG_CONVERSION);
00065             myPTUnit.info.tilt_accel.setValueAndLock(get_desired(TILT,ACCELERATION)*ANG_CONVERSION);
00066 
00067             myPTUnit.info.pan_upper.setValueAndLock(get_desired(PAN,UPPER_SPEED_LIMIT)*ANG_CONVERSION);
00068             myPTUnit.info.tilt_upper.setValueAndLock(get_desired(TILT,UPPER_SPEED_LIMIT)*ANG_CONVERSION);
00069      }
00070 
00071 
00072      while(myManager.runUpdate()==STATE_RUN){
00073             /* we're not in queue mode, so we will only get the last command. this is fine. */
00074             while(myPTUnit.newRequest()){
00075                    myPTUnit.getNextRequest();
00076                    g_debug("Just received a request.");
00077                    //              cout << "Received request to go to : " << myPTUnit.requests.pan.getValue() << " , " << myPTUnit.requests.tilt.getValue() << endl;
00078                    pan_to(myPTUnit.requests.pan.getValue());
00079                    tilt_to(myPTUnit.requests.tilt.getValue());
00080                    /*
00081                      if(myPTUnit.requests.pan_vel.getStatus()==STAT_REQUEST){
00082                      cout << "Made a pan vel request.\n";
00083                      set_pan_vel(myPTUnit.requests.pan_vel.getValue());
00084                      myPTUnit.requests.pan_vel.setStatus(STAT_OKAY);
00085                      }
00086                    
00087                      if(myPTUnit.requests.tilt_vel.getStatus()==STAT_REQUEST){
00088                      cout << "Made a tilt vel request.\n";
00089                      set_tilt_vel(myPTUnit.requests.tilt_vel.getValue());
00090                      myPTUnit.requests.tilt_vel.setStatus(STAT_OKAY);
00091                      }
00092                      
00093                      if(myPTUnit.requests.pan_accel.getStatus()==STAT_REQUEST){
00094                      cout << "Made a pan accel request.\n";
00095                      set_pan_accel(myPTUnit.requests.pan_accel.getValue());
00096                      myPTUnit.requests.pan_accel.setStatus(STAT_OKAY);
00097                      }
00098                      
00099                      if(myPTUnit.requests.tilt_accel.getStatus()==STAT_REQUEST){
00100                      cout << "Made a tilt accel request.\n";
00101                      set_tilt_accel(myPTUnit.requests.tilt_accel.getValue());
00102                      myPTUnit.requests.tilt_accel.setStatus(STAT_OKAY);
00103                      }
00104                    */
00105                    /*
00106                      set_desired(PAN,SPEED,myPTUnit.requests.pan_vel.getValue()/ANG_CONVERSION,ABSOLUTE);
00107                      set_desired(TILT,SPEED,myPTUnit.requests.tilt_vel.getValue()/ANG_CONVERSION,ABSOLUTE);
00108                      
00109                      set_desired(PAN,ACCELERATION,myPTUnit.requests.pan_accel.getValue()/ANG_CONVERSION,ABSOLUTE);
00110                      set_desired(TILT,ACCELERATION,myPTUnit.requests.tilt_accel.getValue()/ANG_CONVERSION,ABSOLUTE);
00111                      */
00112 
00113             }
00114           
00115             /* move the unit*/
00116 
00117             myPTUnit.info.pan.setValue(get_current(PAN,POSITION)*ANG_CONVERSION);       
00118             if (pan_has_goal && fabs(get_current(PAN,POSITION)*ANG_CONVERSION-pan_goal)<PAN_TOL){
00119                    pan_has_goal=false;
00120                    myPTUnit.info.pan.setStatus(STAT_OKAY);
00121             }
00122 
00123             myPTUnit.info.tilt.setValue(get_current(TILT,POSITION)*ANG_CONVERSION);
00124             if (tilt_has_goal && fabs(get_current(TILT,POSITION)*ANG_CONVERSION-tilt_goal)<TILT_TOL){
00125                    tilt_has_goal=false;      
00126                    myPTUnit.info.tilt.setStatus(STAT_OKAY);
00127             }
00128 
00129 
00130             //      cout << "Pan_vel is set to: " << get_desired(PAN,SPEED)*ANG_CONVERSION << endl;
00131             myPTUnit.info.pan_vel.setValue(get_desired(PAN,SPEED)*ANG_CONVERSION);
00132             myPTUnit.info.tilt_vel.setValue(get_desired(TILT,SPEED)*ANG_CONVERSION);
00133 
00134             myPTUnit.info.pan_accel.setValue(get_desired(PAN,ACCELERATION)*ANG_CONVERSION);
00135             myPTUnit.info.tilt_accel.setValue(get_desired(TILT,ACCELERATION)*ANG_CONVERSION);
00136 
00137             myPTUnit.info.pan_upper.setValue(get_desired(PAN,UPPER_SPEED_LIMIT)*ANG_CONVERSION);
00138             myPTUnit.info.tilt_upper.setValue(get_desired(TILT,UPPER_SPEED_LIMIT)*ANG_CONVERSION);
00139 
00140             myPTUnit.publishInfo();
00141      }
00142      
00143      if(running){
00144             cancel_all();
00145      }
00146 
00147      myManager.cleanUp();
00148 
00149      return 0;
00150 }
00151 
00152 void initialize (void){
00153      int res;
00154      _fd = open_host_port(PTU_DEV);
00155      if(_fd == PORT_NOT_OPENED){
00156           g_fatal("Error openening serial port.\n");
00157           exit(1);
00158      }
00159      
00160      //     set_mode(DEFAULTS, RESTORE_FACTORY_SETTINGS);
00161      
00162      res=reset_PTU_parser(1000);
00163      
00164      if(res!=PTU_OK){
00165           g_fatal("Error resetting ptu parser\n");
00166           exit(1);
00167      }
00168      
00169      reset_ptu();
00170      pan_has_goal=false;
00171      tilt_has_goal=false;
00172 }
00173 
00174 
00175 void cancel_all(void){
00176 
00177      pan_has_goal=false;
00178      tilt_has_goal=false;
00179      //pan_to(get_pan());
00180      pan_to(0.0);
00181      pan_has_goal=false;
00182      //tilt_to(get_tilt());
00183      tilt_to(0.0);
00184      tilt_has_goal=false;
00185 }
00186         
00187 
00189 void pan_to (double rad){
00190      PTU_PARM_PTR value = (PTU_PARM_PTR) ((rad) / ANG_CONVERSION);
00191      set_desired(PAN, POSITION, &value, ABSOLUTE);
00192 
00193      tilt_has_goal=false;
00194      pan_has_goal=true;
00195      pan_goal=rad;
00196 }
00197 
00199 void pan (double rad){
00200      pan_to(get_current(PAN,POSITION)*ANG_CONVERSION + rad);
00201 }
00202 
00204 void tilt_to (double rad){
00205      PTU_PARM_PTR value = (PTU_PARM_PTR)((rad) / ANG_CONVERSION);
00206      set_desired(TILT, POSITION, &value, ABSOLUTE);
00207 
00208      pan_has_goal=false;
00209      tilt_has_goal=true;
00210      tilt_goal=rad;
00211 }
00212 
00214 void tilt (double rad){
00215      tilt_to(rad+get_current(TILT,POSITION)*ANG_CONVERSION);
00216 }
00217 
00219 void set_pan_vel (double radps){
00220      PTU_PARM_PTR value = (PTU_PARM_PTR)(radps / ANG_CONVERSION);
00221      //cout << "setting pan vel to " << radps << endl;
00222      set_desired(PAN, SPEED, &value, ABSOLUTE);
00223      //     cout << "it got set to " << get_desired(PAN, BASE) * ANG_CONVERSION;
00224      //set_desired(PAN, BASE, &value, ABSOLUTE);
00225 }
00226 
00228 void set_pan_accel (double radpss){
00229      PTU_PARM_PTR value = (PTU_PARM_PTR)(radpss / ANG_CONVERSION);
00230      set_desired(PAN, ACCELERATION, &value, ABSOLUTE);
00231 }
00232 
00234 void set_pan_upper (double rad){
00235      PTU_PARM_PTR value = (PTU_PARM_PTR)((rad) / ANG_CONVERSION);
00236      set_desired(PAN, UPPER_SPEED_LIMIT, &value, ABSOLUTE);
00237 }
00238 
00240 void set_tilt_vel (double radps){
00241      PTU_PARM_PTR value =  (PTU_PARM_PTR)(radps / ANG_CONVERSION);
00242      set_desired(TILT, SPEED, &value, ABSOLUTE);
00243 }
00244 
00246 void set_tilt_accel (double radpss){
00247      PTU_PARM_PTR value = (PTU_PARM_PTR)(radpss / ANG_CONVERSION);
00248      set_desired(TILT, ACCELERATION, &value, ABSOLUTE);
00249 }
00250 
00252 void set_tilt_upper (double rad){
00253      PTU_PARM_PTR value = (PTU_PARM_PTR)((rad) / ANG_CONVERSION);
00254      set_desired(TILT, UPPER_SPEED_LIMIT, &value, ABSOLUTE);
00255 }
00256