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,
00009 ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopLLEDMask,
00010 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00011 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00012 ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask,
00013 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopBrLEDMask,
00014 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopBrLEDMask,
00015 ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00016 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00017 ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00018 ERS210Info::BotLLEDMask
00019 };
00020 const LEDBitMask_t LedEngine::ERS220numMasks[11] = {
00021 ERS220Info::ModeLEDMask,
00022 ERS220Info::FaceBackLeftLEDMask,
00023 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask,
00024 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask,
00025 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask,
00026 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask,
00027 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask,
00028 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask|ERS220Info::FaceFrontALEDMask,
00029 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask|ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask,
00030 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask|ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask|ERS220Info::FaceFrontCLEDMask,
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) {
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
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150 void LedEngine::cycle(LEDBitMask_t leds, unsigned int period, float amp, float offset, int phase) {
00151
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;
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
00286
00287
00288
00289
00290
00291
00292
00293
00294