EmergencyStopMC.ccGo 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
00048 }
00049
00050 } else if(stilldown) {
00051
00052 stilldown=false;
00053 if((get_time()-timeoflastbtn)<duration)
00054 setStopped(!paused);
00055 }
00056 if(!paused)
00057 return 0;
00058
00059 if(get_time()-timeoflastfreeze>FrameTime*NumFrames*5) {
00060
00061 for(unsigned int i=0; i<NumPIDJoints; i++) {
00062
00063 piddutyavgs[i]=piddutyavgs[i]*.9f+state->pidduties[i]*.1f;
00064
00065 if(fabsf(state->outputs[PIDJointOffset+i]-cmds[PIDJointOffset+i].value)>.15f) {
00066
00067
00068 curPositions[PIDJointOffset+i]=cmds[PIDJointOffset+i].value=state->outputs[PIDJointOffset+i];
00069 dirty=true;
00070 targetReached=false;
00071 }
00072
00073 if(fabsf(piddutyavgs[i])>pidcutoff) {
00074 cmds[PIDJointOffset+i].value-=piddutyavgs[PIDJointOffset+i];
00075 dirty=true;
00076 targetReached=false;
00077 }
00078 }
00079 }
00080 ledengine.updateLEDs(&cmds[LEDOffset]);
00081 if(state->robotDesign&WorldState::ERS7Mask) {
00082
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;
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();
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
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
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
|