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

phSonarHandler.cpp

Go to the documentation of this file.
00001 #include <phSonarHandler.h>
00002 #include <phbase.h>
00003 
00004 /* ------------------------------------------------------------------------ */
00005 phSonarHandler::phSonarHandler( phPlayerPosition  *position,
00006                                 phPlayerSonar     *sonar        )
00007 {
00008     phFUNCTION("phSonarHandler::phSonarHandler")
00009     int locked = 0;
00010     
00011     phTHIS_LOOSE_LOCK(locked);
00012 
00013     this->setName("phSonarHandler");
00014     this->m_sonar = sonar;
00015     this->m_position = position;
00016     
00017     phTHIS_LOOSE_UNLOCK(locked);
00018 }
00019 
00020 /* ------------------------------------------------------------------------ */
00021 int phSonarHandler::run()
00022 {
00023     phFUNCTION("phSonarHandler::run")
00024     phPositionData  position_data;
00025     phSonarData     sonar_data;
00026 
00027     rc = position_data.connect(this->m_position->getSource());
00028     phCHECK_RC(rc,NULL,"position_data.connect(this->m_position->getSource())");
00029     
00030     rc = sonar_data.connect(this->m_sonar->getSource());
00031     phCHECK_RC(rc,NULL,"sonar_data.connect(this->m_sonar->getSource())");
00032     
00033     rc = this->signal_running();
00034     phCHECK_RC(rc,NULL,"this->signal_running");
00035 
00036     while (this->isRunning())
00037     {
00038         /* Position data update */
00039         rc = position_data.update(phLiveObjectNOWAIT);
00040         phPRINT_RC(rc,NULL,"position_data.update(NOWAIT)");
00041 
00042         if (rc == phSUCCESS)
00043         {
00044             position_data.print_data();
00045         } 
00046 
00047         /* Sonar data update */
00048         rc = sonar_data.update(phLiveObjectNOWAIT);
00049         phPRINT_RC(rc,NULL,"sonar_data.update(NOWAIT)");
00050 
00051         if (rc == phSUCCESS)
00052         {
00053             sonar_data.print_data();
00054 
00055             if ((sonar_data.getScan(3) < 1.0) || 
00056                 (sonar_data.getScan(4) < 1.0))
00057             {
00058                 this->m_position->stop();
00059             }
00060         }
00061         
00062         phYield();
00063     }
00064 
00065     rc = sonar_data.disconnect();
00066     phCHECK_RC(rc,NULL,"sonar_data.disconnect()");
00067     
00068     rc = position_data.disconnect();
00069     phCHECK_RC(rc,NULL,"position_data.disconnect()");
00070     
00071     return phSUCCESS;
00072 error:
00073     rc = this->signal_error();
00074     phPRINT_RC(rc,NULL,"this->signal_error");
00075 
00076     return phFAIL;
00077 }




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