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), timeoflastrelease(0), duration(600),
00016 pidcutoff(0.2), ledengine()
00017 {
00018 for(unsigned int i=0; i<NumPIDJoints; i++)
00019 piddutyavgs[i]=0;
00020 #ifdef TGT_HAS_LEDS
00021 if(RobotName == ERS210Info::TargetName) {
00022 int red=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlRedLEDOffset]) - LEDOffset;
00023 int blue=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlBluLEDOffset]) - LEDOffset;
00024 ledengine.cycle((1<<red),period,1,0,period/2);
00025 ledengine.cycle((1<<blue),period,1);
00026 } else if(RobotName == ERS220Info::TargetName) {
00027 int tl=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailLeftLEDOffset]) - LEDOffset;
00028 int tc=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailCenterLEDOffset]) - LEDOffset;
00029 int tr=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailRightLEDOffset]) - LEDOffset;
00030 int bl1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft1LEDOffset]) - LEDOffset;
00031 int bl2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft2LEDOffset]) - LEDOffset;
00032 int bl3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft3LEDOffset]) - LEDOffset;
00033 int br1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight1LEDOffset]) - LEDOffset;
00034 int br2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight2LEDOffset]) - LEDOffset;
00035 int br3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight3LEDOffset]) - LEDOffset;
00036 ledengine.cycle((1<<tc), period, 2.0f, -.5f, (int)(period * 0/5.5));
00037 ledengine.cycle((1<<tl)|(1<<tr), period, 2.0f, -.5f, (int)(period * 1/5.5));
00038 ledengine.cycle((1<<bl3)|(1<<br1), period, 2.0f, -.5f, (int)(period * 2/5.5));
00039 ledengine.cycle((1<<bl2)|(1<<br2), period, 2.0f, -.5f, (int)(period * 3/5.5));
00040 ledengine.cycle((1<<bl1)|(1<<br3), period, 2.0f, -.5f, (int)(period * 4/5.5));
00041 } else if(RobotName == ERS7Info::TargetName) {
00042 int mb=capabilities.getOutputOffset(ERS7Info::outputNames[ERS7Info::MdBackColorLEDOffset]) - LEDOffset;
00043 ledengine.cycle((1<<mb),2*period/3,.15,.15/2-.5,0);
00044 } else {
00045 serr->printf("Emergency Stop: unknown model, no LED special effects\n");
00046 ledengine.cycle(1<<(NumLEDs-1),period,1,0,period/2);
00047 ledengine.cycle(1<<(NumLEDs-2),period,1);
00048 }
00049 #endif
00050 defaultMaxSpeed(.15);
00051 takeSnapshot();
00052 }
00053
00054
00055 int EmergencyStopMC::updateOutputs() {
00056 if(trigger()) {
00057 if(!stilldown) {
00058 stilldown=true;
00059 timeoflastbtn=timeofthisbtn;
00060 timeofthisbtn=get_time();
00061
00062 }
00063
00064 } else if(stilldown) {
00065
00066 stilldown=false;
00067 if((get_time()-timeoflastbtn)<duration)
00068 setStopped(!paused);
00069 }
00070 unsigned int curt=get_time();
00071 dirty=dirty || (curt<timeoflastrelease);
00072 if(!paused) {
00073 if(!dirty)
00074 return 0;
00075 if(curt>=timeoflastrelease) {
00076 #ifdef TGT_HAS_LEDS
00077 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00078 motman->setOutput(this,i,0.f);
00079 #endif
00080 dirty=false;
00081 return 0;
00082 }
00083 float w = (curt>=timeoflastrelease) ? 0 : (static_cast<float>(timeoflastrelease-curt)/FADE_OUT_TIME);
00084 for(unsigned int i=0; i<NumOutputs; i++)
00085 cmds[i].weight=w;
00086 } else {
00087
00088 if(curt-timeoflastfreeze>FrameTime*NumFrames*5) {
00089
00090 for(unsigned int i=0; i<NumPIDJoints; i++) {
00091
00092 piddutyavgs[i]=piddutyavgs[i]*.9f+state->pidduties[i]*.1f;
00093
00094 if(fabsf(state->outputs[PIDJointOffset+i]-cmds[PIDJointOffset+i].value)>.15f) {
00095
00096
00097 curPositions[PIDJointOffset+i]=cmds[PIDJointOffset+i].value=state->outputs[PIDJointOffset+i];
00098 dirty=true;
00099 targetReached=false;
00100 }
00101
00102 if(fabsf(piddutyavgs[i])>pidcutoff) {
00103 cmds[PIDJointOffset+i].value-=piddutyavgs[PIDJointOffset+i];
00104 dirty=true;
00105 targetReached=false;
00106 }
00107 }
00108 }
00109 }
00110 #ifdef TGT_HAS_LEDS
00111 ledengine.updateLEDs(&cmds[LEDOffset]);
00112 #endif
00113 if(RobotName == ERS7Info::TargetName) {
00114
00115 static float acts[5];
00116 static bool wasPaused=false;
00117 if(!wasPaused && paused) {
00118 for(int i=0; i<5; i++)
00119 acts[i]=0;
00120 wasPaused=paused;
00121 }
00122 float t=curt;
00123 t/=period;
00124 t=(((int)t)&1)?(int)t+1-t:(t-(int)t);
00125 t*=8;
00126 const float invsigma=-6;
00127 const float gamma=.83;
00128 const float amp=.5;
00129 float imp[5];
00130
00131 float w = (paused || curt>=timeoflastrelease) ? 1 : (static_cast<float>(timeoflastrelease-curt)/FADE_OUT_TIME);
00132 for(int i=0; i<5; i++) {
00133 float p=invsigma*(t-i-2)*(t-i-2);
00134 if(p>-10) {
00135
00136 imp[i]=expf(p)*w;
00137 acts[i]+=amp*imp[i];
00138 } else {
00139 imp[i]=0;
00140 }
00141 acts[i]*=gamma*w;
00142 }
00143 cmds[ERS7Info::FaceLEDPanelOffset+ 6]=acts[0]/2+imp[0];
00144 cmds[ERS7Info::FaceLEDPanelOffset+ 8]=acts[1]/2+imp[1];
00145 cmds[ERS7Info::FaceLEDPanelOffset+10]=acts[2]/2+imp[2];
00146 cmds[ERS7Info::FaceLEDPanelOffset+ 9]=acts[3]/2+imp[3];
00147 cmds[ERS7Info::FaceLEDPanelOffset+ 7]=acts[4]/2+imp[4];
00148 }
00149 int changed=PostureMC::updateOutputs();
00150 dirty=(curt<timeoflastrelease);
00151 return changed;
00152 }
00153
00154 void EmergencyStopMC::setActive(bool a) {
00155 if(paused) {
00156 if(!a && active)
00157 releaseJoints();
00158 else if(a && !active)
00159 freezeJoints();
00160 }
00161 active=a;
00162 }
00163
00164
00165 void EmergencyStopMC::setStopped(bool p, bool sound) {
00166 if(p!=paused) {
00167 paused=p;
00168 if(active) {
00169 if(paused) {
00170 freezeJoints();
00171 if(sound)
00172 sndman->playFile(config->motion.estop_on_snd);
00173 std::cout << "*** PAUSED ***" << std::endl;
00174 } else {
00175 releaseJoints();
00176 if(sound)
00177 sndman->playFile(config->motion.estop_off_snd);
00178 std::cout << "*** UNPAUSED ***" << std::endl;
00179 }
00180 }
00181 }
00182 }
00183
00184 void EmergencyStopMC::freezeJoints() {
00185 dirty=true;
00186 targetReached=false;
00187 for(unsigned int i=0; i<NumOutputs; i++) {
00188 OutputCmd c=motman->getOutputCmd(i);
00189 curPositions[i]=cmds[i].value = (c.weight==0) ? state->outputs[i] : c.value;
00190 }
00191 for(unsigned int i=0; i<NumPIDJoints; i++)
00192 piddutyavgs[i]=0;
00193 #ifdef TGT_HAS_WHEELS
00194
00195 for(unsigned int i=WheelOffset; i<WheelOffset+NumWheels; i++) {
00196 cmds[i].value = 0.0;
00197 curPositions[i] = 0;
00198 }
00199 #endif
00200 #ifndef TGT_HAS_LEDS
00201
00202 for(unsigned int i=0; i<NumOutputs; i++)
00203 cmds[i].weight=1;
00204 #else
00205 for(unsigned int i=0; i<LEDOffset; i++)
00206 cmds[i].weight=1;
00207 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00208 cmds[i].unset();
00209 for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00210 cmds[i].weight=1;
00211 if(RobotName == ERS210Info::TargetName) {
00212 int red=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlRedLEDOffset]);
00213 int blue=capabilities.getOutputOffset(ERS210Info::outputNames[ERS210Info::TlBluLEDOffset]);
00214 cmds[red].set(0,.5);
00215 cmds[blue].set(0,.5);
00216 } else if(RobotName == ERS220Info::TargetName) {
00217 int tl=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailLeftLEDOffset]);
00218 int tc=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailCenterLEDOffset]);
00219 int tr=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::TailRightLEDOffset]);
00220 int bl1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft1LEDOffset]);
00221 int bl2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft2LEDOffset]);
00222 int bl3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackLeft3LEDOffset]);
00223 int br1=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight1LEDOffset]);
00224 int br2=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight2LEDOffset]);
00225 int br3=capabilities.getOutputOffset(ERS220Info::outputNames[ERS220Info::BackRight3LEDOffset]);
00226 cmds[tl].set(0, .5); cmds[tc].set(0, .5); cmds[tr].set(0, .5);
00227 cmds[bl1].set(0, .5); cmds[bl2].set(0, .5); cmds[bl3].set(0, .5);
00228 cmds[br1].set(0, .5); cmds[br2].set(0, .5); cmds[br3].set(0, .5);
00229 } else if(RobotName == ERS7Info::TargetName) {
00230 cmds[ERS7Info::MdBackColorLEDOffset].set(0,.5);
00231 for(int i=6; i<6+5; i++)
00232 cmds[ERS7Info::FaceLEDPanelOffset+i].set(0,0.5);
00233 } else {
00234 cmds[LEDOffset+NumLEDs-1].set(0,.5);
00235 cmds[LEDOffset+NumLEDs-2].set(0,.5);
00236 }
00237 #endif
00238 postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::activateETID,0));
00239 timeoflastfreeze=get_time();
00240 }
00241
00242 void EmergencyStopMC::releaseJoints() {
00243 dirty=true;
00244 targetReached=false;
00245 unsigned int curt=get_time();
00246 timeoflastrelease=curt+FADE_OUT_TIME;
00247 postEvent(EventBase(EventBase::estopEGID,getID(),EventBase::deactivateETID,curt-timeoflastfreeze));
00248 }
00249
00250 bool EmergencyStopMC::trigger() {
00251 WorldState * st=WorldState::getCurrent();
00252 if(RobotName == ERS210Info::TargetName)
00253 return st->button_times[capabilities.getButtonOffset(ERS210Info::buttonNames[ERS210Info::BackButOffset])];
00254 if(RobotName == ERS220Info::TargetName)
00255 return st->button_times[capabilities.getButtonOffset(ERS220Info::buttonNames[ERS220Info::BackButOffset])];
00256 if(RobotName == ERS7Info::TargetName)
00257 return st->button_times[ERS7Info::FrontBackButOffset]+st->button_times[ERS7Info::MiddleBackButOffset]+st->button_times[ERS7Info::RearBackButOffset];
00258 #ifdef TGT_HAS_BUTTONS
00259
00260 std::string tmpName = RobotName;
00261 serr->printf("EmergencyStopMC: %s unsupported model!\n",tmpName.c_str());
00262 #endif
00263 return false;
00264 }
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276