Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

EmergencyStopMC.cc

Go to the documentation of this file.
00001 #include "EmergencyStopMC.h"
00002 #include "Shared/WorldState.h"
00003 #include "Shared/get_time.h"
00004 #include "Motion/MotionManager.h"
00005 #include "SoundPlay/SoundManager.h"
00006 #include "Shared/Config.h"
00007 #include "Events/EventRouter.h"
00008 #include "Shared/ERS210Info.h"
00009 #include "Shared/ERS220Info.h"
00010 
00011 EmergencyStopMC::EmergencyStopMC()
00012   : PostureMC(), paused(false), stilldown(false), active(true), period(2000),
00013     timeoflastbtn(0), timeofthisbtn(0), timeoflastfreeze(0), duration(600),
00014     pidcutoff(static_cast<unsigned char>(0.16*255)), ledengine()
00015 {
00016   setAutoPrune(false);
00017   for(unsigned int i=0; i<NumPIDJoints; i++)
00018     piddutyavgs[i]=0;
00019   if(state->robotDesign&WorldState::ERS210Mask) {
00020     ledengine.cycle(ERS210Info::TlRedLEDMask,period,1,0,period/2);
00021     ledengine.cycle(ERS210Info::TlBluLEDMask,period,1);
00022   } else if(state->robotDesign&WorldState::ERS220Mask) {
00023     ledengine.cycle(ERS220Info::TailCenterLEDMask, period, 2.0f, -.5f, (int)(period * 0/5.5));
00024     ledengine.cycle(ERS220Info::TailLeftLEDMask|ERS220Info::TailRightLEDMask,   period, 2.0f, -.5f, (int)(period * 1/5.5));
00025     ledengine.cycle(ERS220Info::BackLeft3LEDMask|ERS220Info::BackRight1LEDMask, period, 2.0f, -.5f, (int)(period * 2/5.5));
00026     ledengine.cycle(ERS220Info::BackLeft2LEDMask|ERS220Info::BackRight2LEDMask, period, 2.0f, -.5f, (int)(period * 3/5.5));
00027     ledengine.cycle(ERS220Info::BackLeft1LEDMask|ERS220Info::BackRight3LEDMask, period, 2.0f, -.5f, (int)(period * 4/5.5));
00028   }
00029 }
00030 
00031 
00032 int EmergencyStopMC::updateOutputs() {
00033   if(state->button_times[BackButOffset]) {
00034     if(!stilldown) {
00035       stilldown=true;
00036       timeoflastbtn=timeofthisbtn;
00037       timeofthisbtn=get_time();
00038       //      cout << "Press " << timeofthisbtn << ' ' << timeoflastbtn << endl;
00039     }
00040     //    cout << "Down" << endl;
00041   } else if(stilldown) {
00042     //    cout << "Release " << get_time() << endl;
00043     stilldown=false;
00044     if((get_time()-timeoflastbtn)<duration)
00045       setStopped(!paused);
00046   }
00047   if(paused) {
00048     for(unsigned int i=0; i<NumPIDJoints; i++) {
00049       piddutyavgs[i]=static_cast<unsigned char>(piddutyavgs[i]*.8+fabs(state->pidduties[i])*255*.2);
00050       if(piddutyavgs[i]>pidcutoff) {
00051         cmds[i].value-=state->pidduties[i]/10;
00052         //        cmds[i].value=state->outputs[i];
00053         //        cout << outputNames[i] << ' ' << state->pidduties[i] << ' ' << state->outputs[i] << endl;
00054       }
00055     }
00056   }
00057   ledengine.updateLEDs(&cmds[LEDOffset]);
00058   return PostureMC::updateOutputs();
00059 }
00060 
00061 void EmergencyStopMC::takeSnapshot() {
00062   for(unsigned int i=0; i<NumOutputs; i++)
00063     cmds[i].value=state->outputs[i];
00064   dirty=true;
00065 }
00066 
00067 void EmergencyStopMC::setActive(bool a) {
00068   if(paused) {
00069     if(!a && active)
00070       releaseJoints();
00071     else if(a && !active)
00072       freezeJoints();
00073   }
00074   active=a;
00075 }
00076 
00077 
00078 void EmergencyStopMC::setStopped(bool p, bool sound) {
00079   if(p!=paused) {
00080     paused=p;
00081     if(active) {
00082       dirty=true;
00083       if(paused) {
00084         freezeJoints();
00085         if(sound)
00086           sndman->PlayFile(config->motion.estop_on_snd);
00087         timeoflastfreeze=get_time();
00088         //      pprintf(TextOutputStream,"*** PAUSED ***\n");
00089         cout << "*** PAUSED ***" << endl;
00090       } else {
00091         releaseJoints();
00092         if(sound)
00093           sndman->PlayFile(config->motion.estop_off_snd);
00094         //      pprintf(TextOutputStream,"*** UNPAUSED ***\n");
00095         cout << "*** UNPAUSED ***" << endl;
00096       }
00097     }
00098   }
00099 }
00100 
00101 void EmergencyStopMC::freezeJoints() {
00102   takeSnapshot();
00103   for(unsigned int i=0; i<LEDOffset; i++)
00104     cmds[i].weight=1;
00105   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00106     cmds[i].unset(); // let other commands' LEDs "show through"
00107   for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00108     cmds[i].weight=1;
00109   if(state->robotDesign&WorldState::ERS210Mask) {
00110     cmds[ERS210Info::TlRedLEDOffset].set(0,.5);
00111     cmds[ERS210Info::TlBluLEDOffset].set(0,.5);
00112   } else if(state->robotDesign&WorldState::ERS220Mask) {
00113     for(unsigned int i = 0; i < NumLEDs; i++)
00114       if((ERS220Info::TailLEDMask|ERS220Info::BackLEDMask) & (1 << i))
00115         cmds[LEDOffset + i].set(0, .5);
00116   }
00117   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::activateETID,0));
00118 }
00119 
00120 void EmergencyStopMC::releaseJoints() {
00121   for(unsigned int i=0; i<NumOutputs; i++)
00122     cmds[i].unset();
00123   //these lines prevent residual display
00124   if(state->robotDesign&WorldState::ERS210Mask) {
00125     motman->setOutput(this,ERS210Info::TlRedLEDOffset,0.f);
00126     motman->setOutput(this,ERS210Info::TlBluLEDOffset,0.f);
00127   } else if(state->robotDesign&WorldState::ERS220Mask) {
00128     for(unsigned int i = 0; i < NumLEDs; i++)
00129       if((ERS220Info::TailLEDMask|ERS220Info::BackLEDMask) & (1 << i))
00130         motman->setOutput(this,LEDOffset + i,0.f);
00131   }
00132   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::deactivateETID,get_time()-timeoflastfreeze));
00133 }
00134 
00135 /*! @file
00136  * @brief Implements EmergencyStopMC, overrides all joints, allows modelling, blinks tail
00137  * @author ejt (Creator)
00138  *
00139  * $Author: ejt $
00140  * $Name: tekkotsu-1_4_1 $
00141  * $Revision: 1.13 $
00142  * $State: Exp $
00143  * $Date: 2003/06/10 18:30:00 $
00144  */
00145 

Tekkotsu v1.4
Generated Sat Jul 19 00:06:30 2003 by Doxygen 1.3.2