Main Page | Modules | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members | Related Pages

phWander.cpp

Go to the documentation of this file.
00001 #include <phWander.h>
00002 #include <phPlayerUtil.h>
00003 #include <phbase.h>
00004 
00005 /* ------------------------------------------------------------------------ */
00006 phWander::phWander( phPlayerPosition  *position,
00007                     phPlayerSonar     *sonar        )
00008 {
00009     phFUNCTION("phWander::phWander")
00010     int locked = 0;
00011     
00012     phTHIS_LOOSE_LOCK(locked);
00013 
00014     this->setName("phWander");
00015     this->m_sonar = sonar;
00016     this->m_position = position;
00017     
00018     phTHIS_LOOSE_UNLOCK(locked);
00019 }
00020 
00021 /* ------------------------------------------------------------------------ */
00022 int phWander::wander( phSonarData *sonar_data )
00023 {
00024     phFUNCTION("phWander::wander")
00025 
00026     double  translate   = MAX_TRANSLATE;
00027     double  rotate      = 0.0;
00028     int     retrc       = 1;
00029     
00030     /* Pioneer mapping:
00031      * left front           : 0
00032      * front left           : 1, 2
00033      * front front left     : 3
00034      * front front right    : 4
00035      * front right          : 5, 6
00036      * right front          : 7
00037      
00038      * left rear           : 15
00039      * rear left           : 13,14
00040      * rear rear left      : 12
00041      * rear rear right     : 11
00042      * rear right          : 10, 9
00043      * right rear          : 8
00044      */
00045     int     i               = 0;
00046     
00047     rc = this->m_position->trylock();
00048     if (rc == phSUCCESS)
00049     {
00050         //phPRINT("%0.2f:%0.2f:%d\n",translate,rotate);
00051         this->m_position->move(translate,rotate);
00052         
00053         rc = this->m_position->unlock();
00054         phPRINT_RC(rc,NULL,"m_position->unlock()");
00055     }
00056     else
00057     {
00058         retrc = 1;
00059         phPRINT_RC(rc,NULL,"m_position->trylock()");
00060     }
00061     return retrc;
00062 }
00063 
00064 /* ------------------------------------------------------------------------ */
00065 int phWander::run()
00066 {
00067     phFUNCTION("phWander::run")
00068     phPositionData  position_data;
00069     phSonarData     sonar_data;
00070     int             wrc             = phSUCCESS;
00071 
00072     rc = position_data.connect(this->m_position->getSource());
00073     phCHECK_RC(rc,NULL,"position_data.connect(this->m_position->getSource())");
00074     
00075     rc = sonar_data.connect(this->m_sonar->getSource());
00076     phCHECK_RC(rc,NULL,"sonar_data.connect(this->m_sonar->getSource())");
00077     
00078     rc = this->signal_running();
00079     phCHECK_RC(rc,NULL,"this->signal_running");
00080 
00081     while (this->isRunning())
00082     {
00083         /* phPROGRESS("\n"); */
00084         /* Position data update */
00085         rc = position_data.update(phLiveObjectNOWAIT);
00086         phPRINT_RC(rc,NULL,"position_data.update(NOWAIT)");
00087 
00088         if (rc == phSUCCESS)
00089         {
00090             //position_data.print_data();
00091         } 
00092 
00093         /* Sonar data update */
00094         rc = sonar_data.update((wrc == 1)? phLiveObjectNOWAIT : phLiveObjectWAIT);
00095         phPRINT_RC(rc,NULL,"sonar_data.update(NOWAIT)");
00096         if (rc == phSUCCESS)
00097         {
00098             //sonar_data.print_data();
00099         }
00100         /*
00101         wrc = this->wander(&sonar_data);
00102         phPRINT_RC(wrc,NULL,"this->wander()");
00103         */
00104         phYield();
00105     }
00106 
00107     rc = sonar_data.disconnect();
00108     phCHECK_RC(rc,NULL,"sonar_data.disconnect()");
00109     
00110     rc = position_data.disconnect();
00111     phCHECK_RC(rc,NULL,"position_data.disconnect()");
00112     
00113     return phSUCCESS;
00114 error:
00115     rc = this->signal_error();
00116     phPRINT_RC(rc,NULL,"this->signal_error");
00117 
00118     return phFAIL;
00119 }




Copyright (C) 2002 - 2007 Philip D.S. Thoren ( pthoren@users.sourceforge.net )
University Of Massachusetts at Lowell
Robotics Lab
SourceForge.net Logo

Generated on Sat Jun 16 02:44:07 2007 for phission by  doxygen 1.4.4