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
00039 }
00040
00041 } else if(stilldown) {
00042
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
00053
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
00089 cout << "*** PAUSED ***" << endl;
00090 } else {
00091 releaseJoints();
00092 if(sound)
00093 sndman->PlayFile(config->motion.estop_off_snd);
00094
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();
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
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
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145