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   dirty=true;
00144   unsigned int t = get_time();
00145   if(t+ms<dirtyTime)
00146     dirtyTime=t+ms;
00147   for(unsigned int i=0; i<NumLEDs; i++) {
00148     infos[i].flashvalue=((leds>>i)&1)*value;
00149     infos[i].flashtime=t+ms;
00150   }
00151 }
00152 void LedEngine::flash(LEDBitMask_t leds, float value, unsigned int ms) {
00153   if(leds!=0) {
00154     dirty=true;
00155     unsigned int t = get_time();
00156     if(t+ms<dirtyTime)
00157       dirtyTime=t+ms;
00158     for(unsigned int i=0; i<NumLEDs; i++)
00159       if((leds>>i)&1) {
00160         infos[i].flashvalue=value;
00161         infos[i].flashtime=t+ms;
00162       }
00163   }
00164 }
00165 void LedEngine::flash(LEDBitMask_t leds, unsigned int ms) {
00166   if(leds!=0) {
00167     dirty=true;
00168     unsigned int t = get_time();
00169     if(t+ms<dirtyTime)
00170       dirtyTime=t+ms;
00171     for(unsigned int i=0; i<NumLEDs; i++)
00172       if((leds>>i)&1) {
00173         infos[i].flashvalue=calcFlash(calcValue(i,t));
00174         infos[i].flashtime=t+ms;
00175       }
00176   }
00177 }
00178 /*!@param leds the bitmask of leds to apply this to
00179  * @param period the period of the cycle (milliseconds), includes an on and off
00180  * @param amp the amplitude of the cycle - note that this is clipped at 0 and 1.
00181  * @param offset the vertical offset of the cycle - simply shifts the baseline of the cycle up or down
00182  * @param phase the phase within the cycle to start at (specify in milliseconds)
00183  *
00184  * When this function is called, the starting time is stored as current time + phase.
00185  *
00186  * The equation used is \f[\cos(\frac{2\pi(t-starttime)}{period})*(\frac{-amp}{2})+.5+offset\f]
00187  * 
00188  * The idea is that with a amplitude=1 and offset=0, it will start at
00189  * 0, ramp up to 1, and then ramp down again.  The arguments to this
00190  * function will let you control all parameters of the cycle.
00191  *
00192  * You can get a blink-on/off instead of cycle on/off by using a very large amplitude.
00193  */
00194 void LedEngine::cycle(LEDBitMask_t leds, unsigned int period, float amp, float offset, int phase) {
00195   //  cout << "cycle("<<leds<<","<<period<<","<<amp<<","<<offset<<","<<phase<<")"<<endl;
00196   if(leds!=0) {
00197     dirty=true;
00198     unsigned int start = get_time()+phase;
00199     for(unsigned int i=0; i<NumLEDs; i++)
00200       if((leds>>i)&1) {
00201         if(!infos[i].isCycling)
00202           numCycling++;
00203         infos[i].isCycling=true;
00204         infos[i].amp=amp;
00205         infos[i].period=period;
00206         infos[i].starttime=start;
00207         infos[i].offset=offset;
00208       }
00209   }
00210 }
00211 void LedEngine::clear() {
00212   for(unsigned int i=0; i<NumLEDs; i++) {
00213     infos[i].value=0;
00214     infos[i].isCycling=false;
00215   }
00216   numCycling=0;
00217   dirty=true;
00218 }
00219 
00220 void LedEngine::displayNumber(int x, numStyle_t style) {
00221   switch(style) {
00222   case onedigit: {
00223     const LEDBitMask_t * numMasks=ERS210numMasks; //Default to 210's mask on new models - might not have as many LEDs
00224     if(state->robotDesign & WorldState::ERS220Mask)
00225       numMasks=ERS220numMasks;
00226     if(state->robotDesign & WorldState::ERS7Mask)
00227       numMasks=ERS7numMasks;
00228     if(x>9 || x<-9) {
00229       ccycle(FaceLEDMask&~TopBrLEDMask,333,10,-5);
00230       infos[TopBrLEDOffset-LEDOffset].value=x<0?1:0;
00231     } else {
00232       clear();
00233       if(x<0) {
00234         set(numMasks[-x],1);
00235         infos[TopBrLEDOffset-LEDOffset].value=infos[TopBrLEDOffset-LEDOffset].value*.5+.25;
00236       } else
00237         set(numMasks[x],1);
00238     }
00239     } break;
00240   case twodigit:
00241     if(x>99 || x<-99) {
00242       ccycle(FaceLEDMask&~TopBrLEDMask,333,10,-5);
00243       infos[TopBrLEDOffset-LEDOffset].value=x<0?1:0;
00244     } else {
00245       clear();
00246       if(x<0) {
00247         infos[TopBrLEDOffset-LEDOffset].value=1;
00248         x=-x;
00249       }
00250       setOneOfTwo(x%10,BotLLEDOffset-LEDOffset,MidLLEDOffset-LEDOffset,TopLLEDOffset-LEDOffset);
00251       setOneOfTwo(x/10,BotRLEDOffset-LEDOffset,MidRLEDOffset-LEDOffset,TopRLEDOffset-LEDOffset);
00252     }
00253     break;
00254   }
00255 }
00256 void LedEngine::setOneOfTwo(unsigned int x, unsigned int low, unsigned int mid, unsigned int high) {
00257   if(x==0)
00258     return;
00259   float bg = ((x-1)/3)/3.0;
00260   float fg = bg+.333333333;
00261   switch(x%3) {
00262   case 1:
00263     infos[high].value=bg;
00264     infos[mid].value=bg;
00265     infos[low].value=fg;
00266     break;
00267   case 2:
00268     infos[high].value=bg;
00269     infos[mid].value=fg;
00270     infos[low].value=bg;
00271     break;
00272   case 0:
00273     infos[high].value=fg;
00274     infos[mid].value=bg;
00275     infos[low].value=bg;
00276     break;
00277   }
00278 }
00279 
00280 void LedEngine::displayPercent(float x, percentStyle_t left_style, percentStyle_t right_style) {
00281   clear();
00282   if(x<0) {
00283     set(FaceLEDMask,.25);
00284     return;
00285   }
00286   if(x>1) {
00287     set(FaceLEDMask,.75);
00288     return;
00289   }
00290   if(left_style==major)
00291     setColumn(x,BotLLEDMask,MidLLEDMask,TopLLEDMask,TopBrLEDMask);
00292   if(right_style==major)
00293     setColumn(x,BotRLEDMask,MidRLEDMask,TopRLEDMask,TopBrLEDMask);
00294   x*=4;
00295   x-=(int)x;
00296   if(left_style==minor)
00297     setColumn(x,BotLLEDMask,MidLLEDMask,TopLLEDMask,TopBrLEDMask);
00298   if(right_style==minor)
00299     setColumn(x,BotRLEDMask,MidRLEDMask,TopRLEDMask,TopBrLEDMask);
00300 }
00301 
00302 void LedEngine::setColumn(float x, unsigned int low, unsigned int mid, unsigned int high, unsigned int top) {
00303   LEDBitMask_t solid=0;
00304   LEDBitMask_t partial=0;
00305   switch((int)(4*x)) {
00306   case 4:
00307     solid|=top;
00308   case 3:
00309     solid|=high;
00310   case 2:
00311     solid|=mid;
00312   case 1:
00313     solid|=low;
00314   }
00315   switch((int)(4*x)) {
00316   case 3:
00317     partial=top; break;
00318   case 2:
00319     partial=high; break;
00320   case 1:
00321     partial=mid; break;
00322   case 0:
00323     partial=low; break;
00324   }
00325   float partialvalue=(x*4)-(int)(x*4);
00326   set(partial,partialvalue);
00327   set(solid,1);
00328 }
00329 
00330 
00331 /*! @file
00332  * @brief Implements LedEngine, which provides basic LED effects to anything that inherits or instantiates it
00333  * @author ejt (Creator)
00334  *
00335  * $Author: ejt $
00336  * $Name: tekkotsu-2_1 $
00337  * $Revision: 1.12 $
00338  * $State: Exp $
00339  * $Date: 2004/02/03 01:17:57 $
00340  */

Tekkotsu v2.1
Generated Tue Mar 16 23:19:13 2004 by Doxygen 1.3.5