Homepage Demos Overview Downloads Tutorials 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 "SoundPlay/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(static_cast<unsigned char>(0.16*255)), ledengine()
00017 {
00018   setAutoPrune(false);
00019   for(unsigned int i=0; i<NumPIDJoints; i++)
00020     piddutyavgs[i]=0;
00021   if(state->robotDesign&WorldState::ERS210Mask) {
00022     ledengine.cycle(ERS210Info::TlRedLEDMask,period,1,0,period/2);
00023     ledengine.cycle(ERS210Info::TlBluLEDMask,period,1);
00024   } else if(state->robotDesign&WorldState::ERS220Mask) {
00025     ledengine.cycle(ERS220Info::TailCenterLEDMask, period, 2.0f, -.5f, (int)(period * 0/5.5));
00026     ledengine.cycle(ERS220Info::TailLeftLEDMask|ERS220Info::TailRightLEDMask,   period, 2.0f, -.5f, (int)(period * 1/5.5));
00027     ledengine.cycle(ERS220Info::BackLeft3LEDMask|ERS220Info::BackRight1LEDMask, period, 2.0f, -.5f, (int)(period * 2/5.5));
00028     ledengine.cycle(ERS220Info::BackLeft2LEDMask|ERS220Info::BackRight2LEDMask, period, 2.0f, -.5f, (int)(period * 3/5.5));
00029     ledengine.cycle(ERS220Info::BackLeft1LEDMask|ERS220Info::BackRight3LEDMask, period, 2.0f, -.5f, (int)(period * 4/5.5));
00030   } else if(state->robotDesign&WorldState::ERS7Mask) {
00031     ledengine.cycle(1<<(ERS7Info::MdBackColorLEDOffset-LEDOffset),2*period/3,.15,.15/2-.5,0);
00032   } else {
00033     serr->printf("Emergency Stop: unknown model\n");
00034     ledengine.cycle(1<<(NumLEDs-1),period,1,0,period/2);
00035     ledengine.cycle(1<<(NumLEDs-2),period,1);
00036   }
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   for(unsigned int i=0; i<NumPIDJoints; i++) {
00059     piddutyavgs[i]=static_cast<unsigned char>(piddutyavgs[i]*.8+fabs(state->pidduties[i])*255*.2);
00060     //reset if there's a force...
00061     if(piddutyavgs[i]>pidcutoff) {
00062       cmds[i].value-=state->pidduties[i]/10;
00063       //        cmds[i].value=state->outputs[i];
00064       //        cout << outputNames[i] << ' ' << state->pidduties[i] << ' ' << state->outputs[i] << endl;
00065     }
00066     //reset if there's just something out of place (perhaps we're being overridden)
00067     if(fabs(state->outputs[i]-cmds[i].value)>.1)
00068       cmds[i].value=state->outputs[i];
00069   }
00070   ledengine.updateLEDs(&cmds[LEDOffset]);
00071   if(state->robotDesign&WorldState::ERS7Mask) {
00072     //a special Battlestar Galactica inspired effect for the ERS-7
00073     static float acts[5];
00074     static bool first=true;
00075     if(first) {
00076       for(int i=0; i<5; i++)
00077         acts[i]=0;
00078       first=false;
00079     }
00080     float t=get_time();
00081     t/=period;
00082     t=(((int)t)&1)?(int)t+1-t:(t-(int)t);
00083     t*=8;
00084     const float invsigma=-6;
00085     const float gamma=.83;
00086     const float amp=.5;
00087     float imp[5];
00088     for(int i=0; i<5; i++) {
00089       imp[i]=exp(invsigma*(t-i-2)*(t-i-2));
00090       acts[i]+=amp*imp[i];
00091       acts[i]*=gamma;
00092     }
00093     cmds[ERS7Info::FaceLEDPanelOffset+6].value=acts[0]/2+imp[0];
00094     cmds[ERS7Info::FaceLEDPanelOffset+7].value=acts[4]/2+imp[4];
00095     cmds[ERS7Info::FaceLEDPanelOffset+8].value=acts[1]/2+imp[1];
00096     cmds[ERS7Info::FaceLEDPanelOffset+9].value=acts[3]/2+imp[3];
00097     cmds[ERS7Info::FaceLEDPanelOffset+10].value=acts[2]/2+imp[2];
00098   }
00099   return PostureMC::updateOutputs();
00100 }
00101 
00102 void EmergencyStopMC::takeSnapshot() {
00103   for(unsigned int i=0; i<NumOutputs; i++)
00104     cmds[i].value=state->outputs[i];
00105   dirty=true;
00106 }
00107 
00108 void EmergencyStopMC::setActive(bool a) {
00109   if(paused) {
00110     if(!a && active)
00111       releaseJoints();
00112     else if(a && !active)
00113       freezeJoints();
00114   }
00115   active=a;
00116 }
00117 
00118 
00119 void EmergencyStopMC::setStopped(bool p, bool sound) {
00120   if(p!=paused) {
00121     paused=p;
00122     if(active) {
00123       dirty=true;
00124       if(paused) {
00125         freezeJoints();
00126         if(sound)
00127           sndman->PlayFile(config->motion.estop_on_snd);
00128         timeoflastfreeze=get_time();
00129         //      pprintf(TextOutputStream,"*** PAUSED ***\n");
00130         cout << "*** PAUSED ***" << endl;
00131       } else {
00132         releaseJoints();
00133         if(sound)
00134           sndman->PlayFile(config->motion.estop_off_snd);
00135         //      pprintf(TextOutputStream,"*** UNPAUSED ***\n");
00136         cout << "*** UNPAUSED ***" << endl;
00137       }
00138     }
00139   }
00140 }
00141 
00142 void EmergencyStopMC::freezeJoints() {
00143   takeSnapshot();
00144   for(unsigned int i=0; i<LEDOffset; i++)
00145     cmds[i].weight=1;
00146   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00147     cmds[i].unset(); // let other commands' LEDs "show through"
00148   for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00149     cmds[i].weight=1;
00150   if(state->robotDesign&WorldState::ERS210Mask) {
00151     cmds[ERS210Info::TlRedLEDOffset].set(0,.5);
00152     cmds[ERS210Info::TlBluLEDOffset].set(0,.5);
00153   } else if(state->robotDesign&WorldState::ERS220Mask) {
00154     for(unsigned int i = 0; i < NumLEDs; i++)
00155       if((ERS220Info::TailLEDMask|ERS220Info::BackLEDMask) & (1 << i))
00156         cmds[LEDOffset + i].set(0, .5);
00157   } else if(state->robotDesign&WorldState::ERS7Mask) {
00158     cmds[ERS7Info::MdBackColorLEDOffset].set(0,.5);
00159     for(int i=6; i<6+5; i++)
00160       cmds[ERS7Info::FaceLEDPanelOffset+i].set(0,0.5);
00161   } else {
00162     cmds[LEDOffset+NumLEDs-1].set(0,.5);
00163     cmds[LEDOffset+NumLEDs-2].set(0,.5);
00164   }
00165   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::activateETID,0));
00166 }
00167 
00168 void EmergencyStopMC::releaseJoints() {
00169   for(unsigned int i=0; i<NumOutputs; i++)
00170     cmds[i].unset();
00171   //these lines prevent residual display
00172   if(state->robotDesign&WorldState::ERS210Mask) {
00173     motman->setOutput(this,ERS210Info::TlRedLEDOffset,0.f);
00174     motman->setOutput(this,ERS210Info::TlBluLEDOffset,0.f);
00175   } else if(state->robotDesign&WorldState::ERS220Mask) {
00176     for(unsigned int i = 0; i < NumLEDs; i++)
00177       if((ERS220Info::TailLEDMask|ERS220Info::BackLEDMask) & (1 << i))
00178         motman->setOutput(this,LEDOffset + i,0.f);
00179   } else if(state->robotDesign&WorldState::ERS7Mask) {
00180     cmds[ERS7Info::MdBackColorLEDOffset].set(0,0.f);
00181     for(int i=6; i<6+5; i++)
00182       cmds[ERS7Info::FaceLEDPanelOffset+i].set(0,0.f);
00183   } else {
00184     cmds[LEDOffset+NumLEDs-1].set(0,0.f);
00185     cmds[LEDOffset+NumLEDs-2].set(0,0.f);
00186   }
00187   postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::deactivateETID,get_time()-timeoflastfreeze));
00188 }
00189 
00190 bool EmergencyStopMC::trigger() {
00191   if(state->robotDesign&WorldState::ERS210Mask)
00192     return state->button_times[ERS210Info::BackButOffset];
00193   if(state->robotDesign&WorldState::ERS220Mask)
00194     return state->button_times[ERS220Info::BackButOffset];
00195   if(state->robotDesign&WorldState::ERS7Mask)
00196     return state->button_times[ERS7Info::FrontBackButOffset]+state->button_times[ERS7Info::MiddleBackButOffset]+state->button_times[ERS7Info::RearBackButOffset];
00197   serr->printf("EmergencyStopMC: unsupported model!\n");
00198   return false;
00199 }
00200 
00201 /*! @file
00202  * @brief Implements EmergencyStopMC, overrides all joints, allows modelling, blinks tail
00203  * @author ejt (Creator)
00204  *
00205  * $Author: ejt $
00206  * $Name: tekkotsu-2_1 $
00207  * $Revision: 1.20 $
00208  * $State: Exp $
00209  * $Date: 2004/02/06 23:13:46 $
00210  */
00211 

Tekkotsu v2.1
Generated Tue Mar 16 23:19:13 2004 by Doxygen 1.3.5