Homepage Demos Overview Downloads Tutorials Reference
Credits

LedEngine.cc

Go to the documentation of this file.
00001 #include "LedEngine.h"
00002 #include "MotionManager.h"
00003 #include "Shared/WorldState.h"
00004 #include "Shared/ERS210Info.h"
00005 #include "Shared/ERS220Info.h"
00006 #include "Shared/ERS7Info.h"
00007 
00008 /*! This is "Mimic the number" style */
00009 const LEDBitMask_t LedEngine::ERS210numMasks[11] = {
00010   ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopBrLEDMask, //0
00011   
00012   ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopLLEDMask,  //1
00013   
00014   ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask, //2
00015   
00016   ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::TopLLEDMask
00017   |ERS210Info::TopBrLEDMask, //3
00018   
00019   ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask,  //4
00020 
00021   ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopBrLEDMask, //5
00022 
00023   ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask
00024   |ERS210Info::TopRLEDMask|ERS210Info::TopBrLEDMask, //6
00025 
00026   ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,  //7
00027 
00028   ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask
00029   |ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask, //8
00030 
00031   ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask
00032   |ERS210Info::TopBrLEDMask,  //9
00033 
00034   ERS210Info::BotLLEDMask //.
00035 };
00036 /*! This is "Count the dots" style */
00037 const LEDBitMask_t LedEngine::ERS220numMasks[11] = {
00038   ERS220Info::ModeLEDMask, //0
00039 
00040   ERS220Info::FaceBackLeftLEDMask, //1
00041 
00042   ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask, //2
00043 
00044   ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask, //3
00045 
00046   ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00047   |ERS220Info::FaceFrontRightLEDMask, //4
00048 
00049   ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00050   |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask, //5
00051 
00052   ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00053   |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask, //6
00054 
00055   ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00056   |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask
00057   |ERS220Info::FaceFrontALEDMask, //7
00058 
00059   ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00060   |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask
00061   |ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask, //8
00062 
00063   ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00064   |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask
00065   |ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask|ERS220Info::FaceFrontCLEDMask, //9
00066 
00067   ERS220Info::FaceFrontLeftLEDMask //.
00068 };
00069 /*
00070 / *! This is "Mimic the number" style * /
00071 const LEDBitMask_t LedEngine::ERS7numMasks[11] = {
00072   (ERS7Info::FaceLEDPanelMask<< 5)|(ERS7Info::FaceLEDPanelMask<< 3), //0
00073   (ERS7Info::FaceLEDPanelMask<< 7)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 1), //1
00074   (ERS7Info::FaceLEDPanelMask<<10)|(ERS7Info::FaceLEDPanelMask<< 9)|(ERS7Info::FaceLEDPanelMask<< 5)|(ERS7Info::FaceLEDPanelMask<<3), //2
00075   (ERS7Info::FaceLEDPanelMask<< 7)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<<5), //3
00076   (ERS7Info::FaceLEDPanelMask<<10)|(ERS7Info::FaceLEDPanelMask<<11)|(ERS7Info::FaceLEDPanelMask<< 5)|(ERS7Info::FaceLEDPanelMask<<7)|(ERS7Info::FaceLEDPanelMask<<3)|(ERS7Info::FaceLEDPanelMask<<1), //4
00077   (ERS7Info::FaceLEDPanelMask<< 7)|(ERS7Info::FaceLEDPanelMask<< 9)|(ERS7Info::FaceLEDPanelMask<<11)|(ERS7Info::FaceLEDPanelMask<<5), //5
00078   (ERS7Info::FaceLEDPanelMask<< 9)|(ERS7Info::FaceLEDPanelMask<< 5)|(ERS7Info::FaceLEDPanelMask<< 3), //6
00079   (ERS7Info::FaceLEDPanelMask<< 9)|(ERS7Info::FaceLEDPanelMask<< 7)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<<1), //7
00080   (ERS7Info::FaceLEDPanelMask<<10)|(ERS7Info::FaceLEDPanelMask<<11)|(ERS7Info::FaceLEDPanelMask<< 9)|(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<<7)|(ERS7Info::FaceLEDPanelMask<<3), //8
00081   (ERS7Info::FaceLEDPanelMask<< 9)|(ERS7Info::FaceLEDPanelMask<< 5)|(ERS7Info::FaceLEDPanelMask<< 7)|(ERS7Info::FaceLEDPanelMask<<3)|(ERS7Info::FaceLEDPanelMask<<1), //9
00082   (ERS7Info::FaceLEDPanelMask<< 1) //.
00083 };
00084 */
00085 /*! This is "Count the dots" style */
00086 const LEDBitMask_t LedEngine::ERS7numMasks[11] = {
00087   0, //0
00088   (ERS7Info::FaceLEDPanelMask<<11), //1
00089 
00090   (ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<< 5), //2
00091 
00092   (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<<11)|(ERS7Info::FaceLEDPanelMask<< 3), //3
00093 
00094   (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8)
00095   |(ERS7Info::FaceLEDPanelMask<<9), //4
00096 
00097   (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8)
00098   |(ERS7Info::FaceLEDPanelMask<<9)|(ERS7Info::FaceLEDPanelMask<<11), //5
00099 
00100   (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4)
00101   |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7), //6
00102 
00103   (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4)
00104   |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)
00105   |(ERS7Info::FaceLEDPanelMask<<11), //7
00106 
00107   (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 4)
00108   |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)
00109   |(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9), //8
00110 
00111   (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 4)
00112   |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)
00113   |(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9)|(ERS7Info::FaceLEDPanelMask<<11), //9
00114 
00115   (ERS7Info::FaceLEDPanelMask<< 1) //.
00116 };
00117 
00118 
00119 LedEngine::LedEngine() : dirty(true), numCycling(0), nextFlashEnd((unsigned int)-1) {
00120   for(unsigned int i=0; i<NumLEDs; i++) {
00121     infos[i].flashtime=0;
00122     infos[i].starttime=0;
00123   }
00124   clear();
00125 }
00126 
00127 void LedEngine::recalcFlashEnd(void) {
00128   unsigned int t = get_time();
00129   nextFlashEnd=(unsigned int)-1;
00130   for(unsigned int i=0; i<NumLEDs; i++)
00131     if(infos[i].flashtime>t && nextFlashEnd>infos[i].flashtime)
00132       nextFlashEnd=infos[i].flashtime;
00133 }
00134 
00135 int LedEngine::isDirty() {
00136   unsigned int t = get_time();
00137   if(t>nextFlashEnd) {
00138     dirty=true;
00139     recalcFlashEnd();
00140   };
00141   return dirty;
00142 }
00143 
00144 int LedEngine::updateLEDs(const MotionCommand* caller, LEDBitMask_t mask/*=AllLEDMask*/) {
00145   unsigned int t = get_time();
00146   if (t>nextFlashEnd) recalcFlashEnd();
00147   for(unsigned int i=0; i<NumLEDs; i++)
00148     if((mask>>i)&1)
00149       for(unsigned int f=0; f<NumFrames; f++)
00150         motman->setOutput(caller, i+LEDOffset,calcValue(i,t+f*FrameTime),f);
00151   bool tmp=dirty;
00152   dirty = numCycling>0;
00153   return tmp;
00154 }
00155 
00156 int LedEngine::updateLEDs(OutputCmd cmds[NumLEDs]) {
00157   unsigned int t = get_time();
00158   if (t>nextFlashEnd) recalcFlashEnd();
00159   for(unsigned int i=0; i<NumLEDs; i++)
00160       cmds[i].value=calcValue(i,t);
00161   bool tmp=dirty;
00162   dirty = numCycling>0;
00163   return tmp;
00164 }
00165 
00166 int LedEngine::updateLEDFrames(OutputCmd cmds[NumLEDs][NumFrames]) {
00167   unsigned int t = get_time();
00168   if (t>nextFlashEnd) recalcFlashEnd();
00169   for(unsigned int i=0; i<NumLEDs; i++)
00170     for(unsigned int f=0; f<NumFrames; f++)
00171       cmds[i][f].value=calcValue(i,t+f*FrameTime);
00172   bool tmp=dirty;
00173   dirty = numCycling>0;
00174   return tmp;
00175 }
00176 
00177 void LedEngine::invert(LEDBitMask_t leds) {
00178   if(leds!=0) {
00179     dirty=true;
00180     for(unsigned int i=0; i<NumLEDs; i++)
00181       if((leds>>i)&1)
00182         if(infos[i].isCycling)
00183           infos[i].amp*=-1;
00184         else
00185           infos[i].value=1-infos[i].value;
00186   }
00187 }
00188 void LedEngine::set(LEDBitMask_t leds, float value) {
00189   if(leds!=0) {
00190     dirty=true;
00191     for(unsigned int i=0; i<NumLEDs; i++)
00192       if((leds>>i)&1) {
00193         infos[i].value=value;
00194         if(infos[i].isCycling) {
00195           numCycling--;
00196           infos[i].isCycling=false;
00197         }
00198       }
00199   }
00200 }
00201 void LedEngine::cflash(LEDBitMask_t leds, float value, unsigned int ms) {
00202   dirty=true;
00203   unsigned int t = get_time();
00204   if(t+ms<nextFlashEnd)
00205     nextFlashEnd=t+ms;
00206   for(unsigned int i=0; i<NumLEDs; i++) {
00207     infos[i].flashvalue=((leds>>i)&1)*value;
00208     infos[i].flashtime=t+ms;
00209   }
00210 }
00211 void LedEngine::flash(LEDBitMask_t leds, float value, unsigned int ms) {
00212   if(leds!=0) {
00213     dirty=true;
00214     unsigned int t = get_time();
00215     if(t+ms<nextFlashEnd)
00216       nextFlashEnd=t+ms;
00217     for(unsigned int i=0; i<NumLEDs; i++)
00218       if((leds>>i)&1) {
00219         infos[i].flashvalue=value;
00220         infos[i].flashtime=t+ms;
00221       }
00222   }
00223 }
00224 void LedEngine::flash(LEDBitMask_t leds, unsigned int ms) {
00225   if(leds!=0) {
00226     dirty=true;
00227     unsigned int t = get_time();
00228     if(t+ms<nextFlashEnd)
00229       nextFlashEnd=t+ms;
00230     for(unsigned int i=0; i<NumLEDs; i++)
00231       if((leds>>i)&1) {
00232         infos[i].flashvalue=calcFlash(calcValue(i,t));
00233         infos[i].flashtime=t+ms;
00234       }
00235   }
00236 }
00237 /*!@param leds the bitmask of leds to apply this to
00238  * @param period the period of the cycle (milliseconds), includes an on and off
00239  * @param amp the amplitude of the cycle - note that this is clipped at 0 and 1.
00240  * @param offset the vertical offset of the cycle - simply shifts the baseline of the cycle up or down
00241  * @param phase the phase within the cycle to start at (specify in milliseconds)
00242  *
00243  * When this function is called, the starting time is stored as current time + phase.
00244  *
00245  * The equation used is \f[\cos(\frac{2\pi(t-starttime)}{period})*(\frac{-amp}{2})+.5+offset\f]
00246  * 
00247  * The idea is that with a amplitude=1 and offset=0, it will start at
00248  * 0, ramp up to 1, and then ramp down again.  The arguments to this
00249  * function will let you control all parameters of the cycle.
00250  *
00251  * You can get a blink-on/off instead of cycle on/off by using a very large amplitude.
00252  */
00253 void LedEngine::cycle(LEDBitMask_t leds, unsigned int period, float amp, float offset, int phase) {
00254   //  cout << "cycle("<<leds<<","<<period<<","<<amp<<","<<offset<<","<<phase<<")"<<endl;
00255   if(leds!=0) {
00256     dirty=true;
00257     unsigned int start = get_time()+phase;
00258     for(unsigned int i=0; i<NumLEDs; i++)
00259       if((leds>>i)&1) {
00260         if(!infos[i].isCycling)
00261           numCycling++;
00262         infos[i].isCycling=true;
00263         infos[i].amp=amp;
00264         infos[i].period=period;
00265         infos[i].starttime=start;
00266         infos[i].offset=offset;
00267       }
00268   }
00269 }
00270 void LedEngine::clear() {
00271   for(unsigned int i=0; i<NumLEDs; i++) {
00272     infos[i].value=0;
00273     infos[i].isCycling=false;
00274   }
00275   numCycling=0;
00276   dirty=true;
00277 }
00278 
00279 void LedEngine::displayNumber(int x, numStyle_t style) {
00280   switch(style) {
00281   case onedigit: {
00282     const LEDBitMask_t * numMasks=ERS210numMasks; //Default to 210's mask on new models - might not have as many LEDs
00283     if(state->robotDesign & WorldState::ERS220Mask)
00284       numMasks=ERS220numMasks;
00285     if(state->robotDesign & WorldState::ERS7Mask)
00286       numMasks=ERS7numMasks;
00287     if(x>9 || x<-9) {
00288       ccycle(FaceLEDMask&~TopBrLEDMask,333,10,-5);
00289       infos[TopBrLEDOffset-LEDOffset].value=x<0?1:0;
00290     } else {
00291       clear();
00292       if(x<0) {
00293         set(numMasks[-x],1);
00294         infos[TopBrLEDOffset-LEDOffset].value=infos[TopBrLEDOffset-LEDOffset].value*.5+.25;
00295       } else
00296         set(numMasks[x],1);
00297     }
00298     } break;
00299   case twodigit:
00300     if(x>99 || x<-99) {
00301       ccycle(FaceLEDMask&~TopBrLEDMask,333,10,-5);
00302       infos[TopBrLEDOffset-LEDOffset].value=x<0?1:0;
00303     } else {
00304       clear();
00305       if(x<0) {
00306         infos[TopBrLEDOffset-LEDOffset].value=1;
00307         x=-x;
00308       }
00309       setOneOfTwo(x%10,BotLLEDOffset-LEDOffset,MidLLEDOffset-LEDOffset,TopLLEDOffset-LEDOffset);
00310       setOneOfTwo(x/10,BotRLEDOffset-LEDOffset,MidRLEDOffset-LEDOffset,TopRLEDOffset-LEDOffset);
00311     }
00312     break;
00313   }
00314 }
00315 void LedEngine::setOneOfTwo(unsigned int x, unsigned int low, unsigned int mid, unsigned int high) {
00316   if(x==0)
00317     return;
00318   float bg = ((x-1)/3)/3.0;
00319   float fg = bg+.333333333;
00320   switch(x%3) {
00321   case 1:
00322     infos[high].value=bg;
00323     infos[mid].value=bg;
00324     infos[low].value=fg;
00325     break;
00326   case 2:
00327     infos[high].value=bg;
00328     infos[mid].value=fg;
00329     infos[low].value=bg;
00330     break;
00331   case 0:
00332     infos[high].value=fg;
00333     infos[mid].value=bg;
00334     infos[low].value=bg;
00335     break;
00336   }
00337 }
00338 
00339 void LedEngine::displayPercent(float x, percentStyle_t left_style, percentStyle_t right_style) {
00340   clear();
00341   if(x<0) {
00342     set(FaceLEDMask,.25);
00343     return;
00344   }
00345   if(x>1) {
00346     set(FaceLEDMask,.75);
00347     return;
00348   }
00349   if(left_style==major)
00350     setColumn(x,BotLLEDMask,MidLLEDMask,TopLLEDMask,TopBrLEDMask);
00351   if(right_style==major)
00352     setColumn(x,BotRLEDMask,MidRLEDMask,TopRLEDMask,TopBrLEDMask);
00353   x*=4;
00354   x-=(int)x;
00355   if(left_style==minor)
00356     setColumn(x,BotLLEDMask,MidLLEDMask,TopLLEDMask,TopBrLEDMask);
00357   if(right_style==minor)
00358     setColumn(x,BotRLEDMask,MidRLEDMask,TopRLEDMask,TopBrLEDMask);
00359 }
00360 
00361 void LedEngine::setColumn(float x, unsigned int low, unsigned int mid, unsigned int high, unsigned int top) {
00362   LEDBitMask_t solid=0;
00363   LEDBitMask_t partial=0;
00364   switch((int)(4*x)) {
00365   case 4:
00366     solid|=top;
00367   case 3:
00368     solid|=high;
00369   case 2:
00370     solid|=mid;
00371   case 1:
00372     solid|=low;
00373   }
00374   switch((int)(4*x)) {
00375   case 3:
00376     partial=top; break;
00377   case 2:
00378     partial=high; break;
00379   case 1:
00380     partial=mid; break;
00381   case 0:
00382     partial=low; break;
00383   }
00384   float partialvalue=(x*4)-(int)(x*4);
00385   set(partial,partialvalue);
00386   set(solid,1);
00387 }
00388 
00389 
00390 /*! @file
00391  * @brief Implements LedEngine, which provides basic LED effects to anything that inherits or instantiates it
00392  * @author ejt (Creator)
00393  *
00394  * $Author: dst $
00395  * $Name: tekkotsu-2_2_2 $
00396  * $Revision: 1.15 $
00397  * $State: Exp $
00398  * $Date: 2004/09/12 04:22:37 $
00399  */

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:14 2005 by Doxygen 1.4.0