00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "ArExport.h"
00028 #include "ariaOSDef.h"
00029 #include "ArSyncLoop.h"
00030 #include "ArLog.h"
00031 #include "ariaUtil.h"
00032 #include "ArRobot.h"
00033
00034
00035 ArSyncLoop::ArSyncLoop() :
00036 ArASyncTask(),
00037 myStopRunIfNotConnected(false),
00038 myRobot(0)
00039 {
00040 setThreadName("ArSyncLoop");
00041 }
00042
00043 ArSyncLoop::~ArSyncLoop()
00044 {
00045 }
00046
00047 void ArSyncLoop::setRobot(ArRobot *robot)
00048 {
00049 myRobot=robot;
00050 }
00051
00052 void ArSyncLoop::stopRunIfNotConnected(bool stopRun)
00053 {
00054 myStopRunIfNotConnected = stopRun;
00055 }
00056
00057 void * ArSyncLoop::runThread(void *arg)
00058 {
00059 threadStarted();
00060
00061 long timeToSleep;
00062 ArTime loopEndTime;
00063 std::list<ArFunctor *> *runList;
00064 std::list<ArFunctor *>::iterator iter;
00065 ArTime lastLoop;
00066 bool firstLoop = true;
00067 bool warned = false;
00068
00069 if (!myRobot)
00070 {
00071 ArLog::log(ArLog::Terse, "ArSyncLoop::runThread: Trying to run the synchronous loop without a robot.");
00072 return(0);
00073 }
00074
00075 if (!myRobot->getSyncTaskRoot())
00076 {
00077 ArLog::log(ArLog::Terse, "ArSyncLoop::runThread: Can not run the synchronous loop without a task tree");
00078 return(0);
00079 }
00080
00081 while (myRunning)
00082 {
00083
00084 myRobot->lock();
00085 if (!firstLoop && !warned && !myRobot->getNoTimeWarningThisCycle() &&
00086 myRobot->getCycleWarningTime() != 0 &&
00087 lastLoop.mSecSince() > (signed int) myRobot->getCycleWarningTime())
00088 {
00089 ArLog::log(ArLog::Normal,
00090 "Warning: ArRobot cycle took too long because the loop was waiting for lock.");
00091 ArLog::log(ArLog::Normal,
00092 "\tThe cycle took %u ms, (%u ms normal %u ms warning)",
00093 lastLoop.mSecSince(), myRobot->getCycleTime(),
00094 myRobot->getCycleWarningTime());
00095 }
00096 myRobot->setNoTimeWarningThisCycle(false);
00097 firstLoop = false;
00098 warned = false;
00099 lastLoop.setToNow();
00100
00101 loopEndTime.setToNow();
00102 loopEndTime.addMSec(myRobot->getCycleTime());
00103 myRobot->incCounter();
00104 myRobot->unlock();
00105
00106
00107
00108 myRobot->getSyncTaskRoot()->run();
00109 if (myStopRunIfNotConnected && !myRobot->isConnected())
00110 {
00111 if (myRunning)
00112 ArLog::log(ArLog::Normal, "Exiting robot run because of lost connection.");
00113 break;
00114 }
00115 timeToSleep = loopEndTime.mSecTo();
00116
00117
00118 if (myRobot->isCycleChained() && myRobot->isConnected())
00119 timeToSleep = 0;
00120
00121 if (!myRobot->getNoTimeWarningThisCycle() &&
00122 myRobot->getCycleWarningTime() != 0 &&
00123 lastLoop.mSecSince() > (signed int) myRobot->getCycleWarningTime())
00124 {
00125 ArLog::log(ArLog::Normal,
00126 "Warning: ArRobot sync tasks too long at %u ms, (%u ms normal %u ms warning)",
00127 lastLoop.mSecSince(), myRobot->getCycleTime(),
00128 myRobot->getCycleWarningTime());
00129 warned = true;
00130 }
00131
00132
00133 if (timeToSleep > 0)
00134 ArUtil::sleep(timeToSleep);
00135 }
00136 myRobot->lock();
00137 myRobot->wakeAllRunExitWaitingThreads();
00138 myRobot->unlock();
00139
00140 myRobot->lock();
00141 runList=myRobot->getRunExitListCopy();
00142 myRobot->unlock();
00143 for (iter=runList->begin();
00144 iter != runList->end(); ++iter)
00145 (*iter)->invoke();
00146 delete runList;
00147
00148 return(0);
00149 }