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 }
00038
00039
00040 int EmergencyStopMC::updateOutputs() {
00041 if(trigger()) {
00042 if(!stilldown) {
00043 stilldown=true;
00044 timeoflastbtn=timeofthisbtn;
00045 timeofthisbtn=get_time();
00046
00047 }
00048
00049 } else if(stilldown) {
00050
00051 stilldown=false;
00052 if((get_time()-timeoflastbtn)<duration)
00053 setStopped(!paused);
00054 }
00055 if(paused) {
00056 for(unsigned int i=0; i<NumPIDJoints; i++) {
00057 piddutyavgs[i]=static_cast<unsigned char>(piddutyavgs[i]*.8+fabs(state->pidduties[i])*255*.2);
00058
00059 if(piddutyavgs[i]>pidcutoff) {
00060 cmds[i].value-=state->pidduties[i]/10;
00061
00062
00063 }
00064
00065 if(fabs(state->outputs[i]-cmds[i].value)>.1)
00066 cmds[i].value=state->outputs[i];
00067 }
00068 }
00069 ledengine.updateLEDs(&cmds[LEDOffset]);
00070 if(paused && state->robotDesign&WorldState::ERS7Mask) {
00071
00072 static float acts[5];
00073 static bool first=true;
00074 if(first) {
00075 for(int i=0; i<5; i++)
00076 acts[i]=0;
00077 first=false;
00078 }
00079 float t=get_time();
00080 t/=period;
00081 t=(((int)t)&1)?(int)t+1-t:(t-(int)t);
00082 t*=8;
00083 const float invsigma=-6;
00084 const float gamma=.83;
00085 const float amp=.5;
00086 float imp[5];
00087 for(int i=0; i<5; i++) {
00088 imp[i]=exp(invsigma*(t-i-2)*(t-i-2));
00089 acts[i]+=amp*imp[i];
00090 acts[i]*=gamma;
00091 }
00092 cmds[ERS7Info::FaceLEDPanelOffset+6].value=acts[0]/2+imp[0];
00093 cmds[ERS7Info::FaceLEDPanelOffset+7].value=acts[4]/2+imp[4];
00094 cmds[ERS7Info::FaceLEDPanelOffset+8].value=acts[1]/2+imp[1];
00095 cmds[ERS7Info::FaceLEDPanelOffset+9].value=acts[3]/2+imp[3];
00096 cmds[ERS7Info::FaceLEDPanelOffset+10].value=acts[2]/2+imp[2];
00097 }
00098 return PostureMC::updateOutputs();
00099 }
00100
00101 void EmergencyStopMC::takeSnapshot() {
00102 for(unsigned int i=0; i<NumOutputs; i++)
00103 cmds[i].value=state->outputs[i];
00104 dirty=true;
00105 }
00106
00107 void EmergencyStopMC::setActive(bool a) {
00108 if(paused) {
00109 if(!a && active)
00110 releaseJoints();
00111 else if(a && !active)
00112 freezeJoints();
00113 }
00114 active=a;
00115 }
00116
00117
00118 void EmergencyStopMC::setStopped(bool p, bool sound) {
00119 if(p!=paused) {
00120 paused=p;
00121 if(active) {
00122 dirty=true;
00123 if(paused) {
00124 freezeJoints();
00125 if(sound)
00126 sndman->PlayFile(config->motion.estop_on_snd);
00127 timeoflastfreeze=get_time();
00128
00129 cout << "*** PAUSED ***" << endl;
00130 } else {
00131 releaseJoints();
00132 if(sound)
00133 sndman->PlayFile(config->motion.estop_off_snd);
00134
00135 cout << "*** UNPAUSED ***" << endl;
00136 }
00137 }
00138 }
00139 }
00140
00141 void EmergencyStopMC::freezeJoints() {
00142 takeSnapshot();
00143 for(unsigned int i=0; i<LEDOffset; i++)
00144 cmds[i].weight=1;
00145 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00146 cmds[i].unset();
00147 for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00148 cmds[i].weight=1;
00149 if(state->robotDesign&WorldState::ERS210Mask) {
00150 cmds[ERS210Info::TlRedLEDOffset].set(0,.5);
00151 cmds[ERS210Info::TlBluLEDOffset].set(0,.5);
00152 } else if(state->robotDesign&WorldState::ERS220Mask) {
00153 for(unsigned int i = 0; i < NumLEDs; i++)
00154 if((ERS220Info::TailLEDMask|ERS220Info::BackLEDMask) & (1 << i))
00155 cmds[LEDOffset + i].set(0, .5);
00156 } else if(state->robotDesign&WorldState::ERS7Mask) {
00157 cmds[ERS7Info::MdBackColorLEDOffset].set(0,.5);
00158 for(int i=6; i<6+5; i++)
00159 cmds[ERS7Info::FaceLEDPanelOffset+i].set(0,0.5);
00160 } else {
00161 cmds[LEDOffset+NumLEDs-1].set(0,.5);
00162 cmds[LEDOffset+NumLEDs-2].set(0,.5);
00163 }
00164 postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::activateETID,0));
00165 }
00166
00167 void EmergencyStopMC::releaseJoints() {
00168 for(unsigned int i=0; i<NumOutputs; i++)
00169 cmds[i].unset();
00170
00171 if(state->robotDesign&WorldState::ERS210Mask) {
00172 motman->setOutput(this,ERS210Info::TlRedLEDOffset,0.f);
00173 motman->setOutput(this,ERS210Info::TlBluLEDOffset,0.f);
00174 } else if(state->robotDesign&WorldState::ERS220Mask) {
00175 for(unsigned int i = 0; i < NumLEDs; i++)
00176 if((ERS220Info::TailLEDMask|ERS220Info::BackLEDMask) & (1 << i))
00177 motman->setOutput(this,LEDOffset + i,0.f);
00178 } else if(state->robotDesign&WorldState::ERS7Mask) {
00179 cmds[ERS7Info::MdBackColorLEDOffset].set(0,0.f);
00180 for(int i=6; i<6+5; i++)
00181 cmds[ERS7Info::FaceLEDPanelOffset+i].set(0,0.f);
00182 } else {
00183 cmds[LEDOffset+NumLEDs-1].set(0,0.f);
00184 cmds[LEDOffset+NumLEDs-2].set(0,0.f);
00185 }
00186 postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::deactivateETID,get_time()-timeoflastfreeze));
00187 }
00188
00189 bool EmergencyStopMC::trigger() {
00190 if(state->robotDesign&WorldState::ERS210Mask)
00191 return state->button_times[ERS210Info::BackButOffset];
00192 if(state->robotDesign&WorldState::ERS220Mask)
00193 return state->button_times[ERS220Info::BackButOffset];
00194 if(state->robotDesign&WorldState::ERS7Mask)
00195 return state->button_times[ERS7Info::FrontBackButOffset]+state->button_times[ERS7Info::MiddleBackButOffset]+state->button_times[ERS7Info::RearBackButOffset];
00196 serr->printf("EmergencyStopMC: unsupported model!\n");
00197 return false;
00198 }
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210