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), duration(600),
00016     pidcutoff(0.2), ledengine()
00017 {
00018   for(unsigned int i=0; i<NumPIDJoints; i++)
00019     piddutyavgs[i]=0;
00020   if(state->robotDesign&WorldState::ERS210Mask) {
00021     ledengine.cycle(ERS210Info::TlRedLEDMask,period,1,0,period/2);
00022     ledengine.cycle(ERS210Info::TlBluLEDMask,period,1);
00023   } else if(state->robotDesign&WorldState::ERS220Mask) {
00024     ledengine.cycle(ERS220Info::TailCenterLEDMask, period, 2.0f, -.5f, (int)(period * 0/5.5));
00025     ledengine.cycle(ERS220Info::TailLeftLEDMask|ERS220Info::TailRightLEDMask,   period, 2.0f, -.5f, (int)(period * 1/5.5));
00026     ledengine.cycle(ERS220Info::BackLeft3LEDMask|ERS220Info::BackRight1LEDMask, period, 2.0f, -.5f, (int)(period * 2/5.5));
00027     ledengine.cycle(ERS220Info::BackLeft2LEDMask|ERS220Info::BackRight2LEDMask, period, 2.0f, -.5f, (int)(period * 3/5.5));
00028     ledengine.cycle(ERS220Info::BackLeft1LEDMask|ERS220Info::BackRight3LEDMask, period, 2.0f, -.5f, (int)(period * 4/5.5));
00029   } else if(state->robotDesign&WorldState::ERS7Mask) {
00030     ledengine.cycle(1<<(ERS7Info::MdBackColorLEDOffset-LEDOffset),2*period/3,.15,.15/2-.5,0);
00031   } else {
00032     serr->printf("Emergency Stop: unknown model\n");
00033     ledengine.cycle(1<<(NumLEDs-1),period,1,0,period/2);
00034     ledengine.cycle(1<<(NumLEDs-2),period,1);
00035   }
00036   defaultMaxSpeed(.15);
00037   takeSnapshot();
00038 }
00039 
00040 
00041 int EmergencyStopMC::updateOutputs() {
00042   if(trigger()) {
00043     if(!stilldown) {
00044       stilldown=true;
00045       timeoflastbtn=timeofthisbtn;
00046       timeofthisbtn=get_time();
00047       //      cout << "Press " << timeofthisbtn << ' ' << timeoflastbtn << endl;
00048     }
00049     //    cout << "Down" << endl;
00050   } else if(stilldown) {
00051     //    cout << "Release " << get_time() << endl;
00052     stilldown=false;
00053     if((get_time()-timeoflastbtn)<duration)
00054       setStopped(!paused);
00055   }
00056   if(!paused)
00057     return 0;
00058   //immediately following a pause, just hold current position at first to prevent twitching if we were in motion
00059   if(get_time()-timeoflastfreeze>FrameTime*NumFrames*5) { 
00060     //once joints have come to rest, respond to outside forces
00061     for(unsigned int i=0; i<NumPIDJoints; i++) {
00062       //exponential average of duty cycles to filter out noise
00063       piddutyavgs[i]=piddutyavgs[i]*.9f+state->pidduties[i]*.1f;
00064       //reset if there's something significantly out of place (perhaps we're being overridden)
00065       if(fabsf(state->outputs[PIDJointOffset+i]-cmds[PIDJointOffset+i].value)>.15f) {
00066         //if(PIDJointOffset+i==LFrLegOffset+RotatorOffset)
00067         //cout << "resetting from " << cmds[PIDJointOffset+i].value << " to " << state->outputs[PIDJointOffset+i] << endl;
00068         curPositions[PIDJointOffset+i]=cmds[PIDJointOffset+i].value=state->outputs[PIDJointOffset+i];
00069         dirty=true;
00070         targetReached=false;
00071       }
00072       //give if there's a force...
00073       if(fabsf(piddutyavgs[i])>pidcutoff) {
00074         cmds[PIDJointOffset+i].value-=piddutyavgs[PIDJointOffset+i]; //move in the direction of the force
00075         dirty=true;
00076         targetReached=false;
00077       }
00078     }
00079   }
00080   ledengine.updateLEDs(&cmds[LEDOffset]);
00081   if(state->robotDesign&WorldState::ERS7Mask) {
00082     //a special Battlestar Galactica inspired effect for the ERS-7
00083     static float acts[5];
00084     static bool first=true;
00085     if(first) {
00086       for(int i=0; i<5; i++)
00087         acts[i]=0;
00088       first=false;
00089     }
00090     float t=get_time();
00091     t/=period;
00092     t=(((int)t)&1)?(int)t+1-t:(t-(int)t);
00093     t*=8;
00094     const float invsigma=-6;
00095     const float gamma=.83;
00096     const float amp=.5;
00097     float imp[5];
00098     for(int i=0; i<5; i++) {
00099       imp[i]=exp(invsigma*(t-i-2)*(t-i-2));
00100       acts[i]+=amp*imp[i];
00101       acts[i]*=gamma;
00102     }
00103     cmds[ERS7Info::FaceLEDPanelOffset+6].value=acts[0]/2+imp[0];
00104     cmds[ERS7Info::FaceLEDPanelOffset+7].value=acts[4]/2+imp[4];
00105     cmds[ERS7Info::FaceLEDPanelOffset+8].value=acts[1]/2+imp[1];
00106     cmds[ERS7Info::FaceLEDPanelOffset+9].value=acts[3]/2+imp[3];
00107     cmds[ERS7Info::FaceLEDPanelOffset+10].value=acts[2]/2+imp[2];
00108   }
00109   return PostureMC::updateOutputs();
00110 }
00111 
00112 void EmergencyStopMC::setActive(bool a) {
00113   if(paused) {
00114     if(!a && active)
00115       releaseJoints();
00116     else if(a && !active)
00117       freezeJoints();
00118   }
00119   active=a;
00120 }
00121 
00122 
00123 void EmergencyStopMC::setStopped(bool p, bool sound) {
00124   if(p!=paused) {
00125     paused=p;
00126     if(active) {
00127       if(paused) {
00128         freezeJoints();
00129         if(sound)
00130           sndman->PlayFile(config->motion.estop_on_snd);
00131         cout << "*** PAUSED ***" << endl;
00132       } else {
00133         releaseJoints();
00134         if(sound)
00135           sndman->PlayFile(config->motion.estop_off_snd);
00136         cout << "*** UNPAUSED ***" << endl;
00137       }
00138     }
00139   }
00140 }
00141 
00142 void EmergencyStopMC::freezeJoints() {
00143   dirty=true;
00144   targetReached=false;
00145   for(unsigned int i=0; i<NumOutputs; i++)
00146     curPositions[i]=cmds[i].value=motman->getOutputCmd(i).value;
00147   for(unsigned int i=0; i<NumPIDJoints; i++)
00148     piddutyavgs[i]=0; //or: state->pidduties[i];
00149   for(unsigned int i=0; i<LEDOffset; i++)
00150     cmds[i].weight=1;
00151   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00152     cmds[i].unset(); // let other commands' LEDs "show through"
00153   for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00154     cmds[i].weight=1;
00155   if(state->robotDesign&WorldState::ERS210Mask) {
00156     cmds[ERS210Info::TlRedLEDOffset].set(0,.5);
00157     cmds[ERS210Info::TlBluLEDOffset].set(0,.5);
00158   } else if(state->robotDesign&WorldState::ERS220Mask) {
00159     for(unsigned int i = 0; i < NumLEDs; i++)
00160       if((ERS220Info::TailLEDMask|ERS220Info::BackLEDMask) & (1 << i))
00161         cmds[LEDOffset + i].set(0, .5);
00162   } else if(state->robotDesign&WorldState::ERS7Mask) {
00163     cmds[ERS7Info::MdBackColorLEDOffset].set(0,.5);
00164     for(int i=6; i<6+5; i++)
00165       cmds[ERS7Info::FaceLEDPanelOffset+i].set(0,0.5);
00166   } else {
00167     cmds[LEDOffset+NumLEDs-1].set(0,.5);
00168     cmds[LEDOffset+NumLEDs-2].set(0,.5);
00169   }
00170   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::activateETID,0));
00171   timeoflastfreeze=get_time();
00172 }
00173 
00174 void EmergencyStopMC::releaseJoints() {
00175   dirty=true;
00176   targetReached=false;
00177   for(unsigned int i=0; i<NumOutputs; i++)
00178     cmds[i].unset();
00179   //these lines prevent residual display
00180   if(state->robotDesign&WorldState::ERS210Mask) {
00181     motman->setOutput(this,ERS210Info::TlRedLEDOffset,0.f);
00182     motman->setOutput(this,ERS210Info::TlBluLEDOffset,0.f);
00183   } else if(state->robotDesign&WorldState::ERS220Mask) {
00184     for(unsigned int i = 0; i < NumLEDs; i++)
00185       if((ERS220Info::TailLEDMask|ERS220Info::BackLEDMask) & (1 << i))
00186         motman->setOutput(this,LEDOffset + i,0.f);
00187   } else if(state->robotDesign&WorldState::ERS7Mask) {
00188     cmds[ERS7Info::MdBackColorLEDOffset].set(0,0.f);
00189     for(int i=6; i<6+5; i++)
00190       cmds[ERS7Info::FaceLEDPanelOffset+i].set(0,0.f);
00191   } else {
00192     cmds[LEDOffset+NumLEDs-1].set(0,0.f);
00193     cmds[LEDOffset+NumLEDs-2].set(0,0.f);
00194   }
00195   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::deactivateETID,get_time()-timeoflastfreeze));
00196 }
00197 
00198 bool EmergencyStopMC::trigger() {
00199   if(state->robotDesign&WorldState::ERS210Mask)
00200     return state->button_times[ERS210Info::BackButOffset];
00201   if(state->robotDesign&WorldState::ERS220Mask)
00202     return state->button_times[ERS220Info::BackButOffset];
00203   if(state->robotDesign&WorldState::ERS7Mask)
00204     return state->button_times[ERS7Info::FrontBackButOffset]+state->button_times[ERS7Info::MiddleBackButOffset]+state->button_times[ERS7Info::RearBackButOffset];
00205   serr->printf("EmergencyStopMC: unsupported model!\n");
00206   return false;
00207 }
00208 
00209 /*! @file
00210  * @brief Implements EmergencyStopMC, overrides all joints, allows modelling, blinks tail
00211  * @author ejt (Creator)
00212  *
00213  * $Author: ejt $
00214  * $Name: tekkotsu-2_4_1 $
00215  * $Revision: 1.26 $
00216  * $State: Exp $
00217  * $Date: 2005/06/06 18:30:19 $
00218  */
00219 

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:46 2005 by Doxygen 1.4.4