Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

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

Tekkotsu v1.4
Generated Sat Jul 19 00:06:30 2003 by Doxygen 1.3.2