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/RobotInfo.h"
00009 #include "Shared/ERS210Info.h"
00010 #include "Shared/ERS220Info.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.2f)
00017 #ifdef TGT_HAS_LEDS
00018     , ledengine()
00019 #endif
00020 {
00021   for(unsigned int i=0; i<NumPIDJoints; i++)
00022     piddutyavgs[i]=0;
00023 #if defined(TGT_ERS2xx) || defined(TGT_ERS210) || defined(TGT_ERS220)
00024   // because of the dual-boot ability, these models have to test dynamically
00025   // (TGT_ERS210 and TGT_ERS220 could make the assumption, but don't want to repeat the code...)
00026   if(RobotName == ERS210Info::TargetName) {
00027     int red=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlRedLEDOffset]) - LEDOffset;
00028     int blue=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlBluLEDOffset]) - LEDOffset;
00029     ledengine.cycle((1<<red),period,1,0,period/2);
00030     ledengine.cycle((1<<blue),period,1);
00031   } else if(RobotName == ERS220Info::TargetName) {
00032     int tl=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailLeftLEDOffset]) - LEDOffset;
00033     int tc=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailCenterLEDOffset]) - LEDOffset;
00034     int tr=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailRightLEDOffset]) - LEDOffset;
00035     int bl1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft1LEDOffset]) - LEDOffset;
00036     int bl2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft2LEDOffset]) - LEDOffset;
00037     int bl3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft3LEDOffset]) - LEDOffset;
00038     int br1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight1LEDOffset]) - LEDOffset;
00039     int br2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight2LEDOffset]) - LEDOffset;
00040     int br3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight3LEDOffset]) - LEDOffset;
00041     ledengine.cycle((1<<tc), period, 2.0f, -.5f, (int)(period * 0/5.5));
00042     ledengine.cycle((1<<tl)|(1<<tr),   period, 2.0f, -.5f, (int)(period * 1/5.5));
00043     ledengine.cycle((1<<bl3)|(1<<br1), period, 2.0f, -.5f, (int)(period * 2/5.5));
00044     ledengine.cycle((1<<bl2)|(1<<br2), period, 2.0f, -.5f, (int)(period * 3/5.5));
00045     ledengine.cycle((1<<bl1)|(1<<br3), period, 2.0f, -.5f, (int)(period * 4/5.5));
00046   }
00047 #elif defined(TGT_ERS7)
00048   ledengine.cycle(MdBackColorLEDMask,2*period/3,.15f,.15f/2-.5f,0);
00049 #elif defined(TGT_IS_QWERK)
00050   ledengine.cycle(1<<8,period,1,0,period/2);
00051   ledengine.cycle(1<<9,period,1);
00052 #elif defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00053   int red = RobotInfo::PowerRedLEDOffset - LEDOffset;
00054   int green = RobotInfo::PowerGreenLEDOffset - LEDOffset;
00055   ledengine.cycle(1<<red,period/2,1);
00056   ledengine.cycle(1<<green,period/2,1,0,period/4);
00057 #elif defined(TGT_CHIARA)
00058   ledengine.cycle(ChiaraInfo::RedLEDOffset, 2*period/3, 5.0f);
00059 #elif defined(TGT_CHIARA2)
00060   ledengine.cycle(Chiara2Info::AllLEDMask, 2*period/3, 5.0f);
00061 #elif defined(TGT_HANDEYE) || defined(TGT_HANDEYEZ)
00062 #elif defined(TGT_HAS_LEDS)
00063 #  warning EmergencyStop using generic LED special effects (last two LEDs will alternate)
00064   ledengine.cycle(1<<(NumLEDs-1),period,1,0,period/2);
00065 #  if TGT_HAS_LEDS>1
00066   ledengine.cycle(1<<(NumLEDs-2),period,1);
00067 #  endif
00068 #endif
00069   defaultMaxSpeed(.15f);
00070   takeSnapshot();
00071 }
00072 
00073 
00074 int EmergencyStopMC::updateOutputs() {
00075 #if defined(TGT_IS_CREATE) || defined(TGT_IS_KOBUKI) || defined(TGT_IS_CREATE2)
00076   if(trigger()) {
00077     setStopped(true);
00078   }
00079 #else
00080   if(trigger()) {
00081     if(!stilldown) {
00082       stilldown=true;
00083       timeoflastbtn=timeofthisbtn;
00084       timeofthisbtn=get_time();
00085       //      cout << "Press " << timeofthisbtn << ' ' << timeoflastbtn << endl;
00086     }
00087   //      cout << "Down" << endl;
00088   } else if(stilldown) {
00089   //      cout << "Release " << get_time() << endl;
00090     stilldown=false;
00091     if((get_time()-timeoflastbtn)<duration)
00092       setStopped(!paused);
00093   }
00094 #endif
00095   unsigned int curt=get_time();
00096   dirty=dirty || (curt<timeoflastrelease);
00097   if(!paused) {
00098     if(!dirty)
00099       return 0;
00100     if(curt>=timeoflastrelease) {
00101 #ifdef TGT_HAS_LEDS
00102       for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00103         motman->setOutput(this,i,0.f); //blank out LEDs to avoid residual background display
00104 #endif
00105 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00106       motman->setOutput(this,RobotInfo::PowerRedLEDOffset,OutputCmd(0.0f, 0.5f));
00107       motman->setOutput(this,RobotInfo::PowerGreenLEDOffset,OutputCmd(0.5f, 0.5f));
00108 #endif
00109       dirty=false;
00110       return 0;
00111     }
00112     float w = (curt>=timeoflastrelease) ? 0 : (static_cast<float>(timeoflastrelease-curt)/FADE_OUT_TIME);
00113     for(unsigned int i=0; i<NumOutputs; i++)
00114       cmds[i].weight=w;   
00115   } else {
00116     //immediately following a pause, just hold current position at first to prevent twitching if we were in motion
00117     if(curt-timeoflastfreeze>FrameTime*NumFrames*5) {
00118       //once joints have come to rest, respond to outside forces
00119       for(unsigned int i=0; i<NumPIDJoints; i++) {
00120         //exponential average of duty cycles to filter out noise
00121         piddutyavgs[i]=piddutyavgs[i]*.9f+state->pidduties[i]*.1f;
00122         //reset if there's something significantly out of place (perhaps we're being overridden)
00123         if(fabsf(state->outputs[PIDJointOffset+i]-cmds[PIDJointOffset+i].value)>.15f) {
00124           //if(PIDJointOffset+i==LFrLegOffset+RotatorOffset)
00125           //cout << "resetting from " << cmds[PIDJointOffset+i].value << " to " << state->outputs[PIDJointOffset+i] << endl;
00126           curPositions[PIDJointOffset+i]=cmds[PIDJointOffset+i].value=state->outputs[PIDJointOffset+i];
00127           dirty=true;
00128           targetReached=false;
00129         }
00130         //give if there's a force...
00131         if(fabsf(piddutyavgs[i])>pidcutoff) {
00132           cmds[PIDJointOffset+i].value-=piddutyavgs[PIDJointOffset+i]; //move in the direction of the force
00133           dirty=true;
00134           targetReached=false;
00135         }
00136       }
00137     }
00138   }
00139 #ifdef TGT_HAS_LEDS
00140   ledengine.updateLEDs(&cmds[LEDOffset]);
00141 #endif
00142 #if defined(TGT_ERS7)
00143   //a special Battlestar Galactica inspired effect for the ERS-7
00144   static float acts[5];
00145   static bool wasPaused=false;
00146   if(!wasPaused && paused) {
00147     for(int i=0; i<5; i++)
00148       acts[i]=0;
00149     wasPaused=paused;
00150   }
00151   float t=curt;
00152   t/=period;
00153   t=(((int)t)&1)?(int)t+1-t:(t-(int)t);
00154   t*=8;
00155   const float invsigma=-6;
00156   const float gamma=.83f;
00157   const float amp=.5f;
00158   float imp[5];
00159   // w is used to fade out LEDs when releasing estop
00160   float w = (paused || curt>=timeoflastrelease) ? 1 : (static_cast<float>(timeoflastrelease-curt)/FADE_OUT_TIME);
00161   for(int i=0; i<5; i++) {
00162     float p=invsigma*(t-i-2)*(t-i-2);
00163     if(p>-10) { // only bother with impulse if big enough
00164       // (in particular, saw exp returning -inf instead of 0 for <-85... bug in libm?)
00165       imp[i]=expf(p)*w;
00166       acts[i]+=amp*imp[i];
00167     } else {
00168       imp[i]=0;
00169     }
00170     acts[i]*=gamma*w;
00171   }
00172   cmds[ERS7Info::FaceLEDPanelOffset+ 6]=acts[0]/2+imp[0];
00173   cmds[ERS7Info::FaceLEDPanelOffset+ 8]=acts[1]/2+imp[1];
00174   cmds[ERS7Info::FaceLEDPanelOffset+10]=acts[2]/2+imp[2];
00175   cmds[ERS7Info::FaceLEDPanelOffset+ 9]=acts[3]/2+imp[3];
00176   cmds[ERS7Info::FaceLEDPanelOffset+ 7]=acts[4]/2+imp[4];
00177 #elif defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00178   if (!paused) {
00179     cmds[RobotInfo::PowerRedLEDOffset].set(0.2f,1);
00180     cmds[RobotInfo::PowerGreenLEDOffset].set(1,1);
00181   }
00182 #endif
00183   int changed=PostureMC::updateOutputs();
00184   dirty=(curt<timeoflastrelease);
00185   return changed;
00186 }
00187 
00188 void EmergencyStopMC::setActive(bool a) {
00189   if(paused) {
00190     if(!a && active)
00191       releaseJoints();
00192     else if(a && !active)
00193       freezeJoints();
00194   }
00195   active=a;
00196 }
00197 
00198 
00199 void EmergencyStopMC::setStopped(bool p, bool sound) {
00200   if(p!=paused) {
00201     paused=p;
00202     if(active) {
00203       if(paused) {
00204         freezeJoints();
00205         if(sound)
00206           sndman->playFile(config->motion.estop_on_snd);
00207         std::cout << "*** PAUSED ***" << std::endl;
00208       } else {
00209         releaseJoints();
00210         if(sound)
00211           sndman->playFile(config->motion.estop_off_snd);
00212         std::cout << "*** UNPAUSED ***" << std::endl;
00213       }
00214     }
00215   }
00216 }
00217 
00218 void EmergencyStopMC::freezeJoints() {
00219   dirty=true;
00220   targetReached=false;
00221   for(unsigned int i=0; i<NumOutputs; i++) {
00222     OutputCmd c=motman->getOutputCmd(i);
00223     curPositions[i]=cmds[i].value = (c.weight==0) ? state->outputs[i] : c.value;
00224   }
00225   for(unsigned int i=0; i<NumPIDJoints; i++)
00226     piddutyavgs[i]=0; //or: state->pidduties[i];
00227 #ifdef TGT_HAS_WHEELS
00228   //Wheels need to be set to 0 in e-stop mode
00229   for(unsigned int i=WheelOffset; i<WheelOffset+NumWheels; i++) {
00230     cmds[i].value = 0;
00231     curPositions[i] = 0;
00232   }
00233 #endif
00234 #ifndef TGT_HAS_LEDS
00235   // no LEDs, just go all the way through in one pass...
00236   for(unsigned int i=0; i<NumOutputs; i++)
00237     cmds[i].weight=1;
00238 #else
00239   for(unsigned int i=0; i<LEDOffset; i++)
00240     cmds[i].weight=1;
00241   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00242     cmds[i].unset(); // let other commands' LEDs "show through"
00243   for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00244     cmds[i].weight=1;
00245 #endif
00246 #if defined(TGT_ERS2xx) || defined(TGT_ERS210) || defined(TGT_ERS220)
00247   // because of the dual-boot ability, these models have to test dynamically
00248   // (TGT_ERS210 and TGT_ERS220 could make the assumption, but don't want to repeat the code...)
00249   if(RobotName == ERS210Info::TargetName) {
00250     int red=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlRedLEDOffset]);
00251     int blue=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlBluLEDOffset]);
00252     cmds[red].set(0,.5f);
00253     cmds[blue].set(0,.5f);
00254   } else if(RobotName == ERS220Info::TargetName) {
00255     int tl=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailLeftLEDOffset]);
00256     int tc=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailCenterLEDOffset]);
00257     int tr=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailRightLEDOffset]);
00258     int bl1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft1LEDOffset]);
00259     int bl2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft2LEDOffset]);
00260     int bl3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft3LEDOffset]);
00261     int br1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight1LEDOffset]);
00262     int br2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight2LEDOffset]);
00263     int br3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight3LEDOffset]);
00264     cmds[tl].set(0, .5f); cmds[tc].set(0, .5f); cmds[tr].set(0, .5f);
00265     cmds[bl1].set(0, .5f); cmds[bl2].set(0, .5f); cmds[bl3].set(0, .5f);
00266     cmds[br1].set(0, .5f); cmds[br2].set(0, .5f); cmds[br3].set(0, .5f);
00267   }
00268 #elif defined(TGT_ERS7)
00269   cmds[MdBackColorLEDOffset].set(0,.5f);
00270   for(int i=6; i<6+5; i++)
00271     cmds[FaceLEDPanelOffset+i].set(0,0.5f);
00272 #elif defined(TGT_CHIARA)
00273   cmds[ChiaraInfo::RedLEDOffset].set(0,.5f);
00274 #elif defined(TGT_CHIARA2)
00275   cmds[Chiara2Info::AllLEDMask].set(0,.5f);
00276 #elif defined(TGT_IS_QWERK)
00277   cmds[LEDOffset+8].set(0,.5f); // last two 'front' LEDs
00278   cmds[LEDOffset+9].set(0,.5f);
00279 #  if defined(TGT_QBOTPLUS)
00280   cmds[LEDOffset+10].set(1,1); // first digital out: extra red LED on the side
00281 #  endif
00282 #elif defined(TGT_IS_CREATE) | defined(TGT_IS_CREATE2)
00283   cmds[RobotInfo::PowerRedLEDOffset].set(0,.5f);
00284   cmds[RobotInfo::PowerGreenLEDOffset].set(0,.5f);
00285 #elif defined(TGT_HAS_LEDS)
00286   cmds[LEDOffset+NumLEDs-1].set(0,.5f);
00287 #  if TGT_HAS_LEDS>1
00288   cmds[LEDOffset+NumLEDs-2].set(0,.5f);
00289 #  endif
00290 #endif
00291   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::activateETID,0));
00292   timeoflastfreeze=get_time();
00293 }
00294 
00295 void EmergencyStopMC::releaseJoints() {
00296   dirty=true;
00297   targetReached=false;
00298   unsigned int curt=get_time();
00299   timeoflastrelease=curt+FADE_OUT_TIME;
00300   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::deactivateETID,curt-timeoflastfreeze));
00301 }
00302 
00303 bool EmergencyStopMC::trigger() {
00304   const WorldState * st=WorldState::getCurrent(); // this is need because trigger is a static, so it doesn't have access to the MC 'state' instance
00305 #if defined(TGT_ERS2xx) || defined(TGT_ERS210) || defined(TGT_ERS220)
00306   if(RobotName == ERS210Info::TargetName)
00307     return st->button_times[capabilities.getButtonOffset(ERS210Info::buttonNames[ERS210Info::BackButOffset])];
00308   if(RobotName == ERS220Info::TargetName)
00309     return st->button_times[capabilities.getButtonOffset(ERS220Info::buttonNames[ERS220Info::BackButOffset])];
00310 #elif defined(TGT_ERS7)
00311   return st->button_times[ERS7Info::FrontBackButOffset]+st->button_times[ERS7Info::MiddleBackButOffset]+st->button_times[ERS7Info::RearBackButOffset];
00312 #elif defined(TGT_IS_QWERK)
00313   return st->button_times[0];
00314 #elif defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00315   return (st->sensors[RobotInfo::ModeStateOffset] == RobotInfo::MODE_PASSIVE);
00316 #elif defined(TGT_IS_KOBUKI)
00317   return(st->buttons[RobotInfo::DropLeftWheelButOffset] > 0 || st->buttons[RobotInfo::DropRightWheelButOffset] > 0);
00318 #elif defined(TGT_IS_CHIARA)
00319   return st->button_times[RedButOffset];
00320 #else
00321 #  if defined(TGT_HAS_BUTTONS)
00322 #    warning Target model has buttons, but EmergencyStop does not know which to use for the trigger
00323 #  endif
00324   (void)st; // just to avoid unused variable warning
00325 #endif
00326   return false;
00327 }
00328 
00329 /*! @file
00330  * @brief Implements EmergencyStopMC, overrides all joints, allows modelling, blinks tail
00331  * @author ejt (Creator)
00332  */
00333