Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

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 "Sound/SoundManager.h"
00006 #include "Shared/Config.h"
00007 #include "Events/EventRouter.h"
00008 #include "Shared/ERS210Info.h"
00009 #include "Shared/ERS220Info.h"
00010 #include "Shared/ERS7Info.h"
00011 #include "Wireless/Wireless.h"
00012 
00013 EmergencyStopMC::EmergencyStopMC()
00014   : PostureMC(), paused(false), stilldown(false), active(true), period(2000),
00015     timeoflastbtn(0), timeofthisbtn(0), timeoflastfreeze(0), timeoflastrelease(0), duration(600),
00016     pidcutoff(0.2), ledengine()
00017 {
00018   for(unsigned int i=0; i<NumPIDJoints; i++)
00019     piddutyavgs[i]=0;
00020 #ifdef TGT_HAS_LEDS
00021   if(RobotName == ERS210Info::TargetName) {
00022     int red=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlRedLEDOffset]) - LEDOffset;
00023     int blue=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlBluLEDOffset]) - LEDOffset;
00024     ledengine.cycle((1<<red),period,1,0,period/2);
00025     ledengine.cycle((1<<blue),period,1);
00026   } else if(RobotName == ERS220Info::TargetName) {
00027     int tl=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailLeftLEDOffset]) - LEDOffset;
00028     int tc=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailCenterLEDOffset]) - LEDOffset;
00029     int tr=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailRightLEDOffset]) - LEDOffset;
00030     int bl1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft1LEDOffset]) - LEDOffset;
00031     int bl2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft2LEDOffset]) - LEDOffset;
00032     int bl3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft3LEDOffset]) - LEDOffset;
00033     int br1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight1LEDOffset]) - LEDOffset;
00034     int br2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight2LEDOffset]) - LEDOffset;
00035     int br3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight3LEDOffset]) - LEDOffset;
00036     ledengine.cycle((1<<tc), period, 2.0f, -.5f, (int)(period * 0/5.5));
00037     ledengine.cycle((1<<tl)|(1<<tr),   period, 2.0f, -.5f, (int)(period * 1/5.5));
00038     ledengine.cycle((1<<bl3)|(1<<br1), period, 2.0f, -.5f, (int)(period * 2/5.5));
00039     ledengine.cycle((1<<bl2)|(1<<br2), period, 2.0f, -.5f, (int)(period * 3/5.5));
00040     ledengine.cycle((1<<bl1)|(1<<br3), period, 2.0f, -.5f, (int)(period * 4/5.5));
00041   } else if(RobotName == ERS7Info::TargetName) {
00042     int mb=capabilities.getOutputOffset(ERS7Info::outputNames[ERS7Info::MdBackColorLEDOffset]) - LEDOffset;
00043     ledengine.cycle((1<<mb),2*period/3,.15,.15/2-.5,0);
00044   } else {
00045     serr->printf("Emergency Stop: unknown model, no LED special effects\n");
00046     ledengine.cycle(1<<(NumLEDs-1),period,1,0,period/2);
00047     ledengine.cycle(1<<(NumLEDs-2),period,1);
00048   }
00049 #endif
00050   defaultMaxSpeed(.15);
00051   takeSnapshot();
00052 }
00053 
00054 
00055 int EmergencyStopMC::updateOutputs() {
00056   if(trigger()) {
00057     if(!stilldown) {
00058       stilldown=true;
00059       timeoflastbtn=timeofthisbtn;
00060       timeofthisbtn=get_time();
00061       //      cout << "Press " << timeofthisbtn << ' ' << timeoflastbtn << endl;
00062     }
00063     //    cout << "Down" << endl;
00064   } else if(stilldown) {
00065     //    cout << "Release " << get_time() << endl;
00066     stilldown=false;
00067     if((get_time()-timeoflastbtn)<duration)
00068       setStopped(!paused);
00069   }
00070   unsigned int curt=get_time();
00071   dirty=dirty || (curt<timeoflastrelease);
00072   if(!paused) {
00073     if(!dirty)
00074       return 0;
00075     if(curt>=timeoflastrelease) {
00076 #ifdef TGT_HAS_LEDS
00077       for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00078         motman->setOutput(this,i,0.f); //blank out LEDs to avoid residual background display
00079 #endif
00080       dirty=false;
00081       return 0;
00082     }
00083     float w = (curt>=timeoflastrelease) ? 0 : (static_cast<float>(timeoflastrelease-curt)/FADE_OUT_TIME);
00084     for(unsigned int i=0; i<NumOutputs; i++)
00085       cmds[i].weight=w;
00086   } else {
00087     //immediately following a pause, just hold current position at first to prevent twitching if we were in motion
00088     if(curt-timeoflastfreeze>FrameTime*NumFrames*5) { 
00089       //once joints have come to rest, respond to outside forces
00090       for(unsigned int i=0; i<NumPIDJoints; i++) {
00091         //exponential average of duty cycles to filter out noise
00092         piddutyavgs[i]=piddutyavgs[i]*.9f+state->pidduties[i]*.1f;
00093         //reset if there's something significantly out of place (perhaps we're being overridden)
00094         if(fabsf(state->outputs[PIDJointOffset+i]-cmds[PIDJointOffset+i].value)>.15f) {
00095           //if(PIDJointOffset+i==LFrLegOffset+RotatorOffset)
00096           //cout << "resetting from " << cmds[PIDJointOffset+i].value << " to " << state->outputs[PIDJointOffset+i] << endl;
00097           curPositions[PIDJointOffset+i]=cmds[PIDJointOffset+i].value=state->outputs[PIDJointOffset+i];
00098           dirty=true;
00099           targetReached=false;
00100         }
00101         //give if there's a force...
00102         if(fabsf(piddutyavgs[i])>pidcutoff) {
00103           cmds[PIDJointOffset+i].value-=piddutyavgs[PIDJointOffset+i]; //move in the direction of the force
00104           dirty=true;
00105           targetReached=false;
00106         }
00107       }
00108     }
00109   }
00110 #ifdef TGT_HAS_LEDS
00111   ledengine.updateLEDs(&cmds[LEDOffset]);
00112 #endif
00113   if(RobotName == ERS7Info::TargetName) {
00114     //a special Battlestar Galactica inspired effect for the ERS-7
00115     static float acts[5];
00116     static bool wasPaused=false;
00117     if(!wasPaused && paused) {
00118       for(int i=0; i<5; i++)
00119         acts[i]=0;
00120       wasPaused=paused;
00121     }
00122     float t=curt;
00123     t/=period;
00124     t=(((int)t)&1)?(int)t+1-t:(t-(int)t);
00125     t*=8;
00126     const float invsigma=-6;
00127     const float gamma=.83;
00128     const float amp=.5;
00129     float imp[5];
00130     // w is used to fade out LEDs when releasing estop
00131     float w = (paused || curt>=timeoflastrelease) ? 1 : (static_cast<float>(timeoflastrelease-curt)/FADE_OUT_TIME);
00132     for(int i=0; i<5; i++) {
00133       float p=invsigma*(t-i-2)*(t-i-2);
00134       if(p>-10) { // only bother with impulse if big enough
00135         // (in particular, saw exp returning -inf instead of 0 for <-85... bug in libm?)
00136         imp[i]=expf(p)*w;
00137         acts[i]+=amp*imp[i];
00138       } else {
00139         imp[i]=0;
00140       }
00141       acts[i]*=gamma*w;
00142     }
00143     cmds[ERS7Info::FaceLEDPanelOffset+ 6]=acts[0]/2+imp[0];
00144     cmds[ERS7Info::FaceLEDPanelOffset+ 8]=acts[1]/2+imp[1];
00145     cmds[ERS7Info::FaceLEDPanelOffset+10]=acts[2]/2+imp[2];
00146     cmds[ERS7Info::FaceLEDPanelOffset+ 9]=acts[3]/2+imp[3];
00147     cmds[ERS7Info::FaceLEDPanelOffset+ 7]=acts[4]/2+imp[4];
00148   }
00149   int changed=PostureMC::updateOutputs();
00150   dirty=(curt<timeoflastrelease);
00151   return changed;
00152 }
00153 
00154 void EmergencyStopMC::setActive(bool a) {
00155   if(paused) {
00156     if(!a && active)
00157       releaseJoints();
00158     else if(a && !active)
00159       freezeJoints();
00160   }
00161   active=a;
00162 }
00163 
00164 
00165 void EmergencyStopMC::setStopped(bool p, bool sound) {
00166   if(p!=paused) {
00167     paused=p;
00168     if(active) {
00169       if(paused) {
00170         freezeJoints();
00171         if(sound)
00172           sndman->playFile(config->motion.estop_on_snd);
00173         std::cout << "*** PAUSED ***" << std::endl;
00174       } else {
00175         releaseJoints();
00176         if(sound)
00177           sndman->playFile(config->motion.estop_off_snd);
00178         std::cout << "*** UNPAUSED ***" << std::endl;
00179       }
00180     }
00181   }
00182 }
00183 
00184 void EmergencyStopMC::freezeJoints() {
00185   dirty=true;
00186   targetReached=false;
00187   for(unsigned int i=0; i<NumOutputs; i++) {
00188     OutputCmd c=motman->getOutputCmd(i);
00189     curPositions[i]=cmds[i].value = (c.weight==0) ? state->outputs[i] : c.value;
00190   }
00191   for(unsigned int i=0; i<NumPIDJoints; i++)
00192     piddutyavgs[i]=0; //or: state->pidduties[i];
00193 #ifdef TGT_HAS_WHEELS
00194   //Wheels need to be set to 0 in e-stop mode
00195   for(unsigned int i=WheelOffset; i<WheelOffset+NumWheels; i++) {
00196     cmds[i].value = 0.0;
00197     curPositions[i] = 0;
00198   }
00199 #endif
00200 #ifndef TGT_HAS_LEDS
00201   // no LEDs, just go all the way through in one pass...
00202   for(unsigned int i=0; i<NumOutputs; i++)
00203     cmds[i].weight=1;
00204 #else
00205   for(unsigned int i=0; i<LEDOffset; i++)
00206     cmds[i].weight=1;
00207   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00208     cmds[i].unset(); // let other commands' LEDs "show through"
00209   for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00210     cmds[i].weight=1;
00211   if(RobotName == ERS210Info::TargetName) {
00212     int red=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlRedLEDOffset]);
00213     int blue=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlBluLEDOffset]);
00214     cmds[red].set(0,.5);
00215     cmds[blue].set(0,.5);
00216   } else if(RobotName == ERS220Info::TargetName) {
00217     int tl=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailLeftLEDOffset]);
00218     int tc=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailCenterLEDOffset]);
00219     int tr=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailRightLEDOffset]);
00220     int bl1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft1LEDOffset]);
00221     int bl2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft2LEDOffset]);
00222     int bl3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft3LEDOffset]);
00223     int br1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight1LEDOffset]);
00224     int br2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight2LEDOffset]);
00225     int br3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight3LEDOffset]);
00226     cmds[tl].set(0, .5); cmds[tc].set(0, .5); cmds[tr].set(0, .5);
00227     cmds[bl1].set(0, .5); cmds[bl2].set(0, .5); cmds[bl3].set(0, .5);
00228     cmds[br1].set(0, .5); cmds[br2].set(0, .5); cmds[br3].set(0, .5);
00229   } else if(RobotName == ERS7Info::TargetName) {
00230     cmds[ERS7Info::MdBackColorLEDOffset].set(0,.5);
00231     for(int i=6; i<6+5; i++)
00232       cmds[ERS7Info::FaceLEDPanelOffset+i].set(0,0.5);
00233   } else {
00234     cmds[LEDOffset+NumLEDs-1].set(0,.5);
00235     cmds[LEDOffset+NumLEDs-2].set(0,.5);
00236   }
00237 #endif
00238   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::activateETID,0));
00239   timeoflastfreeze=get_time();
00240 }
00241 
00242 void EmergencyStopMC::releaseJoints() {
00243   dirty=true;
00244   targetReached=false;
00245   unsigned int curt=get_time();
00246   timeoflastrelease=curt+FADE_OUT_TIME;
00247   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::deactivateETID,curt-timeoflastfreeze));
00248 }
00249 
00250 bool EmergencyStopMC::trigger() {
00251   WorldState * st=WorldState::getCurrent(); // this is need because trigger is a static, so it doesn't have access to the MC 'state' instance
00252   if(RobotName == ERS210Info::TargetName)
00253     return st->button_times[capabilities.getButtonOffset(ERS210Info::buttonNames[ERS210Info::BackButOffset])];
00254   if(RobotName == ERS220Info::TargetName)
00255     return st->button_times[capabilities.getButtonOffset(ERS220Info::buttonNames[ERS220Info::BackButOffset])];
00256   if(RobotName == ERS7Info::TargetName)
00257     return st->button_times[ERS7Info::FrontBackButOffset]+st->button_times[ERS7Info::MiddleBackButOffset]+st->button_times[ERS7Info::RearBackButOffset];
00258 #ifdef TGT_HAS_BUTTONS
00259   // depending on platform, RobotName may be either char* or string
00260   std::string tmpName = RobotName; // convert to consistent form so we can printf it...
00261   serr->printf("EmergencyStopMC: %s unsupported model!\n",tmpName.c_str());
00262 #endif
00263   return false;
00264 }
00265 
00266 /*! @file
00267  * @brief Implements EmergencyStopMC, overrides all joints, allows modelling, blinks tail
00268  * @author ejt (Creator)
00269  *
00270  * $Author: ejt $
00271  * $Name: tekkotsu-4_0 $
00272  * $Revision: 1.40 $
00273  * $State: Exp $
00274  * $Date: 2007/11/18 06:47:03 $
00275  */
00276 

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