Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Controller.cc

Go to the documentation of this file.
00001 #include "Controller.h"
00002 #include "Motion/EmergencyStopMC.h"
00003 #include "Motion/LedMC.h"
00004 #include "Motion/MMAccessor.h"
00005 #include "IPC/SharedObject.h"
00006 #include "Shared/WorldState.h"
00007 #include "Shared/get_time.h"
00008 #include "Sound/SoundManager.h"
00009 #include "Events/TextMsgEvent.h"
00010 #include "Shared/RobotInfo.h"
00011 #include "Shared/ERS210Info.h"
00012 #include "Shared/ERS220Info.h"
00013 #include "Shared/ERS2xxInfo.h"
00014 #include "Shared/ERS7Info.h"
00015 #include "Shared/string_util.h"
00016 #include "Shared/ProjectInterface.h"
00017 #include <sstream>
00018 
00019 Controller * Controller::theOneController=NULL;
00020 
00021 //these are given appropriate values in init once we know which model we're running on
00022 EventBase Controller::nextItem;
00023 EventBase Controller::prevItem;
00024 EventBase Controller::nextItemFast;
00025 EventBase Controller::prevItemFast;
00026 EventBase Controller::selectItem;
00027 EventBase Controller::cancel;
00028 
00029 using namespace string_util;
00030 using namespace std;
00031 
00032 void Controller::DoStart() {
00033   BehaviorBase::DoStart();
00034   sndman->loadFile(config->controller.select_snd);
00035   sndman->loadFile(config->controller.next_snd);
00036   sndman->loadFile(config->controller.prev_snd);
00037   sndman->loadFile(config->controller.read_snd);
00038   sndman->loadFile(config->controller.cancel_snd);
00039   erouter->addListener(this,EventBase::estopEGID);
00040   // Turn on wireless
00041   gui_comm=wireless->socket(Socket::SOCK_STREAM, 2048, 32000);
00042   wireless->setReceiver(gui_comm->sock, gui_comm_callback);
00043   wireless->setDaemon(gui_comm,true);
00044   wireless->listen(gui_comm->sock, config->controller.gui_port);
00045   theOneController=this;
00046   SharedObject<LedMC> leds;
00047   leds->setWeights(~FaceLEDMask,0);
00048   leds->setWeights(FaceLEDMask,.75);
00049   display=motman->addPersistentMotion(leds,isControlling?MotionManager::kEmergencyPriority:MotionManager::kIgnoredPriority);
00050   reset();
00051 }
00052 
00053 void Controller::DoStop() {
00054   sndman->releaseFile(config->controller.select_snd);
00055   sndman->releaseFile(config->controller.next_snd);
00056   sndman->releaseFile(config->controller.prev_snd);
00057   sndman->releaseFile(config->controller.read_snd);
00058   sndman->releaseFile(config->controller.cancel_snd);
00059   erouter->removeListener(this);
00060   reset();
00061   motman->removeMotion(display);
00062   display=MotionManager::invalid_MC_ID;
00063   //these two lines help prevent residual display in case that was the only MotionCommand using LEDs
00064 #ifdef TGT_HAS_LEDS
00065   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00066     motman->setOutput(NULL,i,0.f);
00067 #endif
00068   gui_comm->printf("goodbye\n");
00069   wireless->setDaemon(gui_comm,false);
00070   wireless->close(gui_comm);
00071   theOneController=NULL;
00072   BehaviorBase::DoStop();
00073 }
00074 
00075 bool Controller::trapEvent(const EventBase& e) {
00076   if(!chkCmdStack())
00077     return false;
00078   last_time=cur_time;
00079   cur_time=get_time();
00080   //this will prevent inadvertant controller commands when you pick up an ERS-7
00081   if(state->buttons[nextItem.getSourceID()] && state->buttons[prevItem.getSourceID()] && state->buttons[selectItem.getSourceID()])
00082     return true;
00083   
00084   if(nextItem.sameGenSource(e)) {
00085     nextEv_val=e.getMagnitude();
00086     nextEv_dur=e.getDuration();
00087     if(nextEv_val==0 && prevEv_val==0)
00088       alreadyGotBoth=false;
00089     if(nextEv_val>.75 && prevEv_val>.75 && nextEv_dur<666 && prevEv_dur<666)
00090       if(alreadyGotBoth)
00091         return true;
00092       else {
00093         alreadyGotBoth=true;
00094         return setNext(cmdstack.top()->doReadStdIn());
00095       }
00096     if(e.getTypeID()==nextItem.getTypeID() && e.getDuration()<666)
00097       return setNext(cmdstack.top()->doNextItem());
00098     if(e.getTypeID()==nextItemFast.getTypeID() && e.getDuration()>666 && calcPulse(cur_time,last_time,static_cast<unsigned int>(50/e.getMagnitude())))
00099       return setNext(cmdstack.top()->doNextItem());
00100   }
00101   if(prevItem.sameGenSource(e)) {
00102     prevEv_val=e.getMagnitude();
00103     prevEv_dur=e.getDuration();
00104     if(nextEv_val==0 && prevEv_val==0)
00105       alreadyGotBoth=false;
00106     if(nextEv_val>.75 && prevEv_val>.75 && nextEv_dur<666 && prevEv_dur<666)
00107       if(alreadyGotBoth)
00108         return true;
00109       else {
00110         alreadyGotBoth=true;
00111         return setNext(cmdstack.top()->doReadStdIn());
00112       }
00113     if(e.getTypeID()==prevItem.getTypeID() && e.getDuration()<666)
00114       return setNext(cmdstack.top()->doPrevItem());
00115     if(e.getTypeID()==prevItemFast.getTypeID() && e.getDuration()>666 && calcPulse(cur_time,last_time,static_cast<unsigned int>(50/e.getMagnitude())))
00116       return setNext(cmdstack.top()->doPrevItem());
00117   }
00118   if(e.getDuration()>250) {
00119     if(e==selectItem)
00120       return setNext(cmdstack.top()->doSelect());
00121     if(e==cancel)
00122       return setNext(cmdstack.top()->doCancel());
00123   }
00124   return true;
00125 }
00126 
00127 void Controller::processEvent(const EventBase& event) {
00128   if(event.getTypeID()==EventBase::activateETID) { //estop just turned on
00129     if(!isControlling)
00130       activate();
00131   } else { //estop just turned off
00132     if(isControlling)
00133       deactivate();
00134   }
00135 }
00136 
00137 void Controller::reset() {
00138   while(cmdstack.size()>1)
00139     pop();
00140   if(!cmdstack.empty()) {
00141     cmdstack.top()->deactivate();
00142     cmdstack.pop();
00143   }
00144   refresh();
00145 }
00146 
00147 void Controller::refresh() {
00148   if(!chkCmdStack())
00149     return;
00150   cmdstack.top()->refresh();
00151 }
00152 
00153 void Controller::push(ControlBase* c) {
00154   if(!chkCmdStack())
00155     return;
00156   cmdstack.top()->pause();
00157   cmdstack.push(c);
00158   theOneController->gui_comm->printf("push\n");
00159   setNext(cmdstack.top()->activate(display,gui_comm));
00160 }
00161 
00162 void Controller::pop() {
00163   cmdstack.top()->deactivate();
00164   cmdstack.pop();
00165   theOneController->gui_comm->printf("pop\n");
00166   refresh();
00167 }
00168 
00169 Controller& Controller::setRoot(ControlBase* r) {
00170   reset();
00171   root=r;
00172   refresh();
00173   return *this;
00174 }
00175 
00176 Controller& Controller::setEStopID(MotionManager::MC_ID estopid) {
00177   estop_id=estopid;
00178   if(static_cast<EmergencyStopMC*>(motman->peekMotion(estopid))->getStopped()) {
00179     if(!isControlling)
00180       activate();
00181   } else {
00182     if(isControlling)
00183       deactivate();
00184   }   
00185   return *this;
00186 }
00187 
00188 void Controller::loadGUI(const std::string& type, const std::string& name, unsigned int port, const std::vector<std::string>& args) {
00189   if(theOneController==NULL)
00190     return;
00191   std::stringstream ss;
00192   ss << "load\n" << type << '\n' << name << '\n' << port << '\n';
00193   for(unsigned int i=0; i<args.size(); i++) {
00194     ss << '"';
00195     for(unsigned int j=0; j<args[i].size(); j++) {
00196       if(args[i][j]=='\\' || args[i][j]=='"' || args[i][j]=='\n')
00197         ss << '\\';
00198       ss << args[i][j];
00199     }
00200     ss << "\" ";
00201   }
00202   ss << '\n';
00203   theOneController->gui_comm->write((const byte*)ss.str().c_str(),ss.str().size());
00204 }
00205 
00206 void Controller::closeGUI(const std::string& name) {
00207   if(theOneController==NULL)
00208     return;
00209   ASSERTRET(theOneController->gui_comm!=NULL,"null gui_comm");
00210 
00211   theOneController->gui_comm->printf("close\n%s\n",name.c_str());
00212 }
00213 
00214 int Controller::gui_comm_callback(char *buf, int bytes) {
00215   std::string s(buf,bytes);
00216   //  cout << "Controller Received: " << s << endl;
00217   if(theOneController==NULL)
00218     return 0;
00219 
00220   static std::string incomplete;
00221 
00222   //pass a line at a time to the controller
00223   while(s.size()>0) {
00224     std::string::size_type endline=s.find('\n');
00225     if(endline==std::string::npos) {
00226       incomplete+=s;
00227       return 0;
00228     }
00229     
00230     //strip a \r\n or a \n
00231     if(endline>0 && s[endline-1]=='\r')
00232       incomplete+=s.substr(0,endline-1);
00233     else
00234       incomplete+=s.substr(0,endline);
00235     
00236     //is now complete
00237     theOneController->takeLine(incomplete); 
00238     incomplete.erase();
00239     s=s.substr(endline+1);
00240   }
00241   
00242   return 0;
00243 }
00244 
00245 int Controller::console_callback(char *buf, int bytes) {
00246   std::string s(buf,bytes);
00247   //  cout << "Console Received: " << s << endl;
00248   if(theOneController==NULL)
00249     return 0;
00250 
00251   static std::string incomplete;
00252 
00253   //pass a line at a time to the controller
00254   while(s.size()>0) {
00255     std::string::size_type endline=s.find('\n');
00256     if(endline==std::string::npos) {
00257       incomplete+=s;
00258       return 0;
00259     }
00260 
00261     //strip a \r\n or a \n
00262     if(endline>0 && s[endline-1]=='\r')
00263       incomplete+=s.substr(0,endline-1);
00264     else
00265       incomplete+=s.substr(0,endline);
00266     
00267     //is now complete
00268     switch(config->main.consoleMode) {
00269       case Config::main_config::CONTROLLER:
00270         theOneController->takeLine(incomplete); break;
00271       case Config::main_config::TEXTMSG:
00272         erouter->postEvent(TextMsgEvent(incomplete,0)); break;
00273       case Config::main_config::AUTO:
00274         if(wireless->isConnected(theOneController->gui_comm->sock))    
00275           erouter->postEvent(TextMsgEvent(incomplete,0));    
00276         else
00277           theOneController->takeLine(incomplete); 
00278         break;
00279     }
00280     incomplete.erase();
00281     s=s.substr(endline+1);
00282   }
00283   
00284   return 0;
00285 }
00286 
00287 /*! Select which model is running and call initButtons with the appropriate button offsets
00288  *  This could be somewhat simplified by using capabilities.getButtonOffset(), (wouldn't need
00289  *  the ERS2xx case with essentially duplicated ERS210 and ERS220 cases), but this
00290  *  style has the advantage that the symbols are checked by the compiler so there's no
00291  *  chance of a typo in a button name going unnoticed. */
00292 void Controller::init() {
00293   if(TargetName == ERS2xxInfo::TargetName) {
00294     // compatability mode, see which of the targets is actually running
00295     // Note using ERS2xxInfo namespace to get appropriate offsets!
00296     // could remove duplication with "direct" 210/220 cases below by using something like:
00297     //   capabilities.getButtonOffset(ERS210Info::outputNames[ERS210Info::fooButOffset])
00298     if(RobotName == ERS210Info::TargetName) {
00299       initButtons(666,250,ERS2xxInfo::HeadFrButOffset,ERS2xxInfo::HeadBkButOffset,ERS2xxInfo::HeadFrButOffset,ERS2xxInfo::HeadBkButOffset,ERS2xxInfo::ChinButOffset,ERS2xxInfo::BackButOffset);
00300     } else if(RobotName == ERS220Info::TargetName) {
00301       //the 220 doesn't really support "fast" because it's using boolean buttons
00302       //i'm using a "hack" on the 210 because the pressure sensitivity causes status
00303       //events to continually be sent but since this is just on/off, it only gets the
00304       //activate/deactivate.  To fix this, override nextItemFast and prevItemFast with
00305       // timers and do timer management in processEvents()
00306       initButtons(666,50,ERS2xxInfo::TailLeftButOffset,ERS2xxInfo::TailRightButOffset,ERS2xxInfo::TailLeftButOffset,ERS2xxInfo::TailRightButOffset,ERS2xxInfo::TailCenterButOffset,ERS2xxInfo::BackButOffset);
00307     } else {
00308       cerr << "Controller: Unsupported 2xx model '" << RobotName << "'!  Appears to have buttons, but Controller doesn't know how to use them." << endl;
00309     }
00310   } else if(RobotName == ERS210Info::TargetName) {
00311     initButtons(666,250,ERS210Info::HeadFrButOffset,ERS210Info::HeadBkButOffset,ERS210Info::HeadFrButOffset,ERS210Info::HeadBkButOffset,ERS210Info::ChinButOffset,ERS210Info::BackButOffset);
00312   } else if(RobotName == ERS220Info::TargetName) {
00313     //the 220 doesn't really support "fast" because it's using boolean buttons
00314     //i'm using a "hack" on the 210 because the pressure sensitivity causes status
00315     //events to continually be sent but since this is just on/off, it only gets the
00316     //activate/deactivate.  To fix this, override nextItemFast and prevItemFast with
00317     // timers and do timer management in processEvents()
00318     initButtons(666,50,ERS220Info::TailLeftButOffset,ERS220Info::TailRightButOffset,ERS220Info::TailLeftButOffset,ERS220Info::TailRightButOffset,ERS220Info::TailCenterButOffset,ERS220Info::BackButOffset);
00319   } else if(RobotName == ERS7Info::TargetName) {
00320     initButtons(500,25,ERS7Info::FrontBackButOffset,ERS7Info::RearBackButOffset,ERS7Info::FrontBackButOffset,ERS7Info::RearBackButOffset,ERS7Info::MiddleBackButOffset,ERS7Info::HeadButOffset);
00321   } else {
00322 #ifdef TGT_HAS_BUTTONS
00323     cerr << "Controller: Unsupported model '" << RobotName << "'!  Appears to have buttons, but Controller doesn't know how to use them." << endl;
00324 #endif
00325   }
00326 }
00327 
00328 void Controller::initButtons(unsigned fastTime, unsigned downTime, unsigned nextB, unsigned prevB, unsigned nextFastB, unsigned prevFastB, unsigned selectB, unsigned cancelB) {
00329   nextItem=EventBase(EventBase::buttonEGID,nextB,EventBase::deactivateETID,0);
00330   prevItem=EventBase(EventBase::buttonEGID,prevB,EventBase::deactivateETID,0);
00331   nextItemFast=EventBase(EventBase::buttonEGID,nextFastB,EventBase::statusETID,fastTime);
00332   prevItemFast=EventBase(EventBase::buttonEGID,prevFastB,EventBase::statusETID,fastTime);
00333   selectItem=EventBase(EventBase::buttonEGID,selectB,EventBase::deactivateETID,downTime);
00334   cancel=EventBase(EventBase::buttonEGID,cancelB,EventBase::deactivateETID,downTime);
00335 }
00336 
00337 
00338 bool Controller::select(ControlBase* item, const std::string& name) {
00339   // Depth first
00340   const std::vector<ControlBase*>& slots = item->getSlots();
00341   for(unsigned int i=0; i<slots.size(); i++) {
00342     if (slots[i] != NULL) {
00343       if (slots[i]->getName() == name) { // sensitive to #Name
00344   char in[10];
00345   snprintf(in, 9, "%d", i); in[9]='\0';
00346   ControlBase * ret = item->takeInput(in);
00347   if(ret!=NULL) {
00348     setNext(ret);
00349     return true;
00350   }
00351       } else {
00352   if (select(slots[i], name)) 
00353     return true;
00354       }
00355     }
00356   }
00357   return false;
00358 }
00359 
00360 void Controller::takeLine(const std::string& s) {
00361   //  cout << "RECEIVED: " << s << endl;
00362   if(s.size()==0)
00363     return;
00364   // break s into a vector of arguments
00365   std::vector<std::string> args;
00366   std::vector<unsigned int> offsets;
00367   if(!string_util::parseArgs(s,args,offsets)) {
00368     serr->printf("Controller::takeLine(\"%s\") was malformed.\n",s.c_str());
00369     return;
00370   }
00371   if(args.size()==0 || offsets.size()==0)
00372     return;
00373   // now look through for a ';' (separates multiple commands)
00374   unsigned int last=offsets[0];
00375   for(unsigned int i=0; i<args.size(); i++) {
00376     if(args[i]==";") { // if we found a ';', recurse with substring
00377       takeLine(s.substr(last,offsets[i]-last));
00378       if(i+1==args.size()) // last arg is a ';'
00379         return;
00380       last=offsets[i+1];
00381     }
00382     if(args[i]=="\\;") // if we found a '\;', replace it with base ';'
00383       args[i]=";";
00384   }
00385   if(!chkCmdStack())
00386     return;
00387   if(args[0][0]!='!') {
00388     setNext(cmdstack.top()->takeInput(s));
00389   } else {
00390     if(last!=offsets[0]) { // only changes if we found a ';' - in that case, need to do last segment
00391       takeLine(s.substr(last));
00392     } else if(args[0]=="!refresh") {
00393       refresh();
00394     } else if(args[0]=="!reset") {
00395       reset();
00396     } else if(args[0]=="!cancel") {
00397       setNext(cmdstack.top()->doCancel());
00398     } else if(args[0]=="!select") {
00399       if (args.size() == 1)
00400         setNext(cmdstack.top()->doSelect());
00401       else {
00402         select(root, args[1].c_str());
00403         refresh();
00404       }
00405     } else if(args[0]=="!next") {
00406       setNext(cmdstack.top()->doNextItem());
00407     } else if(args[0]=="!prev") {
00408       setNext(cmdstack.top()->doPrevItem());
00409     } else if(args[0]=="!dump_stack") {
00410       theOneController->gui_comm->printf("stack_dump\n%lu\n",(unsigned long)cmdstack.size());
00411       //this is rather ugly - can't iterate a stack, have to unstack and restack it.  Oh well.
00412       std::stack< ControlBase* > tmpstack;
00413       while(!cmdstack.empty()) {
00414         tmpstack.push(cmdstack.top());
00415         cmdstack.pop();
00416       }
00417       while(!tmpstack.empty()) {
00418         theOneController->gui_comm->printf("%s\n",tmpstack.top()->getName().c_str());
00419         cmdstack.push(tmpstack.top());
00420         tmpstack.pop();
00421       }
00422     } else if(args[0]=="!post") {
00423       if(args.size()<4) {
00424         serr->printf("Bad post command, need at least 3 arguments: generator source type [duration]\n");
00425         return;
00426       }
00427       //parse generator id -- could be a generator name or a numeric value
00428       int egid=0;
00429       for(;egid<EventBase::numEGIDs && args[1]!=EventBase::EventGeneratorNames[egid];egid++) {}
00430       if(egid==EventBase::numEGIDs) {
00431         egid=atoi(args[1].c_str());
00432         if(egid==0 && args[1]!="0") {
00433           serr->printf("Bad event generator '%s'\n",args[1].c_str());
00434           return;
00435         }
00436       }
00437       //parse source id -- numeric value, unless egid is buttonEGID, in which case we can look up a button name
00438       //(if you want to add support for other symbolic source types, this is where to do it)
00439       unsigned int source;
00440       if(egid==EventBase::buttonEGID) {
00441         source=0;
00442         for(;source<NumButtons && args[2]!=buttonNames[source];source++) {}
00443         if(source==NumButtons) {
00444           source=atoi(args[2].c_str());
00445           if(source==0 && args[2]!="0") {
00446             serr->printf("Invalid button name or index '%s'\n",args[2].c_str());
00447             return;
00448           }
00449         }
00450       } else {
00451         source=atoi(args[2].c_str());
00452       }
00453       //parse type id -- numeric, name, or abbreviated name
00454       int etid=0;
00455       for(;etid<EventBase::numETIDs && args[3]!=EventBase::EventTypeNames[etid];etid++) {}
00456       if(etid==EventBase::numETIDs) {
00457         etid=0;
00458         for(;etid<EventBase::numETIDs && args[3]!=EventBase::EventTypeAbbr[etid];etid++) {}
00459         if(etid==EventBase::numETIDs) {
00460           etid=atoi(args[3].c_str());
00461           if(etid==0 && args[3]!="0") {
00462             serr->printf("Bad event type '%s'\n",args[3].c_str());
00463             return;
00464           }
00465         }
00466       }
00467       //duration field (optional, have to check args.size())
00468       int dur=0;
00469       if(args.size()>4)
00470         dur=atoi(args[4].c_str());
00471       //send event!
00472       if(egid==EventBase::buttonEGID && isControlling)
00473         erouter->removeTrapper(this);
00474       erouter->postEvent((EventBase::EventGeneratorID_t)egid,source,(EventBase::EventTypeID_t)etid,dur);
00475       if(egid==EventBase::buttonEGID && isControlling)
00476         erouter->addTrapper(this,EventBase::buttonEGID);
00477     } else if(args[0]=="!msg") {
00478       if(offsets.size()>1)
00479         erouter->postEvent(TextMsgEvent(s.substr(offsets[1]),0));
00480       else
00481         erouter->postEvent(TextMsgEvent("",0));
00482     } else if(args[0]=="!hello") {
00483       static unsigned int count=0;
00484       count++;
00485       theOneController->gui_comm->printf("hello\n%d\n",count);
00486     } else if(args[0]=="!root") {
00487       ControlBase * ret=root->takeInput(s.substr(offsets[1]));
00488       if(ret!=NULL)
00489         setNext(ret);
00490     } else if(args[0]=="!hilight") {
00491       std::vector<unsigned int> hilights;
00492       for(unsigned int i=1; i<args.size(); i++)
00493         hilights.push_back(atoi(args[i].c_str()));
00494       cmdstack.top()->setHilights(hilights);
00495     } else if(args[0]=="!input") {
00496       const std::vector<unsigned int>& hilights=cmdstack.top()->getHilights();
00497       const std::vector<ControlBase*>& slots=cmdstack.top()->getSlots();
00498       std::string in=s.substr(offsets[1]);
00499       for(unsigned int i=0; i<hilights.size(); i++)
00500         if(hilights[i]<slots.size() && slots[hilights[i]]!=NULL) {
00501           ControlBase * ret=slots[hilights[i]]->takeInput(in);
00502           if(ret!=NULL)
00503             setNext(ret);
00504         }
00505       refresh();
00506     } else if(args[0]=="!set") {
00507       setConfig(s.substr(offsets[1]).c_str());
00508     } else if(args[0]=="!sim") {
00509 #ifdef PLATFORM_APERIOS
00510       serr->printf("!sim command invalid -- not running in simulator!\n");
00511 #else
00512       ProjectInterface::sendCommand(s.substr(offsets[1]));
00513 #endif
00514     } else
00515       setNext(cmdstack.top()->takeInput(s));
00516   }
00517 }
00518 
00519 int Controller::setConfig(const std::string& str) {
00520   string::size_type eq=str.find('=');
00521   if(eq==string::npos)
00522     return -2;
00523   plist::ObjectBase* entry = config->resolveEntry(string_util::trim(str.substr(0,eq)));
00524   if(entry==NULL) {
00525     string::size_type p=str.find('.');
00526     string sec=string_util::trim(str.substr(0,p));
00527     string key=string_util::trim(str.substr(p+1,eq-p-1));
00528     string val=string_util::trim(str.substr(eq+1));
00529     if(config->setValue(sec,key,val)==NULL)
00530       return -2;
00531     return 0;
00532   }
00533   plist::PrimitiveBase* prim = dynamic_cast<plist::PrimitiveBase*>(entry);
00534   if(prim==NULL)
00535     return -2;
00536   prim->set(string_util::trim(str.substr(eq+1)));
00537   return 0;
00538 }
00539 
00540 bool Controller::setNext(ControlBase* next) {
00541   if(next==NULL)
00542     pop();
00543   else if(next!=cmdstack.top())
00544     push(next);
00545   return true;
00546 }
00547 
00548 void Controller::activate() {
00549   motman->setPriority(display,MotionManager::kEmergencyPriority);
00550   erouter->addTrapper(this,EventBase::buttonEGID);
00551   isControlling=true;
00552   if(!cmdstack.empty())
00553     cmdstack.top()->activate(display,gui_comm);
00554   else
00555     chkCmdStack();
00556 }
00557 
00558 void Controller::deactivate() {
00559   //these two lines help prevent residual display in case that was the only MotionCommand using LEDs
00560   motman->setPriority(display,MotionManager::kIgnoredPriority);
00561   isControlling=false;
00562 #ifdef TGT_HAS_LEDS
00563   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00564     motman->setOutput(NULL,i,0.f);
00565 #endif
00566   erouter->removeTrapper(this);
00567   cmdstack.top()->pause();
00568 }
00569 
00570 bool Controller::chkCmdStack() {
00571   if(cmdstack.empty()) {
00572     if(root==NULL)
00573       return false;
00574     cmdstack.push(root);
00575     ControlBase * next = cmdstack.top()->activate(display,gui_comm);
00576     if(next==NULL)
00577       cout << "*** WARNING Controller root returned NULL on activate!" << endl;
00578     else if(next!=root)
00579       push(next);
00580   }
00581   return true;
00582 }
00583 
00584 
00585 /*! @file
00586  * @brief Implements Controller class, a behavior that should be started whenever the emergency stop goes on to provide menus for robot control
00587  * @author ejt (Creator)
00588  *
00589  * $Author: ejt $
00590  * $Name: tekkotsu-4_0 $
00591  * $Revision: 1.64 $
00592  * $State: Exp $
00593  * $Date: 2007/11/18 06:47:00 $
00594  */

Tekkotsu v4.0
Generated Thu Nov 22 00:54:52 2007 by Doxygen 1.5.4