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

Tekkotsu v2.0
Generated Wed Jan 21 03:20:29 2004 by Doxygen 1.3.4