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 #ifdef TGT_HAS_LED_PANEL
00009
00010 const LEDBitMask_t LedEngine::defaultMimicNumMasks[11] = {
00011 BotRLEDMask|BotLLEDMask|TopBrLEDMask,
00012 BotLLEDMask|MidLLEDMask|TopLLEDMask,
00013 BotRLEDMask|BotLLEDMask|TopLLEDMask|TopBrLEDMask,
00014 BotRLEDMask|BotLLEDMask|MidRLEDMask|TopLLEDMask|TopBrLEDMask,
00015 BotLLEDMask|MidLLEDMask|TopRLEDMask|TopLLEDMask,
00016 BotRLEDMask|BotLLEDMask|TopRLEDMask|TopBrLEDMask,
00017 BotRLEDMask|BotLLEDMask|MidRLEDMask|MidLLEDMask|TopRLEDMask|TopBrLEDMask,
00018 BotLLEDMask|MidLLEDMask|TopLLEDMask|TopBrLEDMask,
00019 BotRLEDMask|BotLLEDMask|MidRLEDMask|MidLLEDMask|TopRLEDMask|TopLLEDMask|TopBrLEDMask,
00020 BotLLEDMask|MidLLEDMask|TopRLEDMask|TopLLEDMask|TopBrLEDMask,
00021 BotLLEDMask
00022 };
00023
00024 const LEDBitMask_t LedEngine::ERS220numMasks[11] = {
00025 ERS220Info::ModeLEDMask,
00026
00027 ERS220Info::FaceBackLeftLEDMask,
00028
00029 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask,
00030
00031 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask,
00032
00033 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00034 |ERS220Info::FaceFrontRightLEDMask,
00035
00036 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00037 |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask,
00038
00039 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00040 |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask,
00041
00042 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00043 |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask
00044 |ERS220Info::FaceFrontALEDMask,
00045
00046 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00047 |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask
00048 |ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask,
00049
00050 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask
00051 |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask
00052 |ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask|ERS220Info::FaceFrontCLEDMask,
00053
00054 ERS220Info::FaceFrontLeftLEDMask
00055 };
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073 const LEDBitMask_t LedEngine::ERS7numMasks[11] = {
00074 0,
00075 (ERS7Info::FaceLEDPanelMask<<11),
00076
00077 (ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<< 5),
00078
00079 (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<<11)|(ERS7Info::FaceLEDPanelMask<< 3),
00080
00081 (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8)
00082 |(ERS7Info::FaceLEDPanelMask<<9),
00083
00084 (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8)
00085 |(ERS7Info::FaceLEDPanelMask<<9)|(ERS7Info::FaceLEDPanelMask<<11),
00086
00087 (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4)
00088 |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7),
00089
00090 (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4)
00091 |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)
00092 |(ERS7Info::FaceLEDPanelMask<<11),
00093
00094 (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 4)
00095 |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)
00096 |(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9),
00097
00098 (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 4)
00099 |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)
00100 |(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9)|(ERS7Info::FaceLEDPanelMask<<11),
00101
00102 (ERS7Info::FaceLEDPanelMask<< 1)
00103 };
00104 #endif // has led panel
00105
00106
00107 const LEDBitMask_t LedEngine::defaultCountNumMasks[11] = {
00108 (1<<0)|(1<<8),
00109 (1<<0),
00110 (1<<0)|(1<<1),
00111 (1<<0)|(1<<1)|(1<<2),
00112 (1<<0)|(1<<1)|(1<<2)|(1<<3),
00113 (1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4),
00114 (1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4)|(1<<5),
00115 (1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4)|(1<<5)|(1<<6),
00116 (1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4)|(1<<5)|(1<<6)|(1<<7),
00117 (1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4)|(1<<5)|(1<<6)|(1<<7)|(1<<8),
00118 (1<<1)|(1<<3)|(1<<5)|(1<<7)
00119 };
00120
00121
00122 LedEngine::LedEngine() : dirty(true), numCycling(0), nextFlashEnd((unsigned int)-1) {
00123 for(unsigned int i=0; i<NumLEDs; i++) {
00124 infos[i].flashtime=0;
00125 infos[i].starttime=0;
00126 }
00127 clear();
00128 }
00129
00130 void LedEngine::recalcFlashEnd(void) {
00131 unsigned int t = get_time();
00132 nextFlashEnd=(unsigned int)-1;
00133 for(unsigned int i=0; i<NumLEDs; i++)
00134 if(infos[i].flashtime>t && nextFlashEnd>infos[i].flashtime)
00135 nextFlashEnd=infos[i].flashtime;
00136 }
00137
00138 int LedEngine::isDirty() {
00139 unsigned int t = get_time();
00140 if(t>nextFlashEnd) {
00141 dirty=true;
00142 recalcFlashEnd();
00143 };
00144 return dirty;
00145 }
00146
00147 int LedEngine::updateLEDs(const MotionCommand* caller, LEDBitMask_t mask) {
00148 unsigned int t = get_time();
00149 if (t>nextFlashEnd) recalcFlashEnd();
00150 #ifdef TGT_HAS_LEDS
00151 for(unsigned int i=0; i<NumLEDs; i++)
00152 if((mask>>i)&1)
00153 for(unsigned int f=0; f<NumFrames; f++)
00154 motman->setOutput(caller, i+LEDOffset,calcValue(i,t+f*FrameTime),f);
00155 #endif
00156 bool tmp=dirty;
00157 dirty = numCycling>0;
00158 return tmp;
00159 }
00160
00161 int LedEngine::updateLEDs(OutputCmd cmds[NumLEDs]) {
00162 unsigned int t = get_time();
00163 if (t>nextFlashEnd) recalcFlashEnd();
00164 for(unsigned int i=0; i<NumLEDs; i++)
00165 cmds[i].value=calcValue(i,t);
00166 bool tmp=dirty;
00167 dirty = numCycling>0;
00168 return tmp;
00169 }
00170
00171 int LedEngine::updateLEDFrames(OutputCmd cmds[NumLEDs][NumFrames]) {
00172 unsigned int t = get_time();
00173 if (t>nextFlashEnd) recalcFlashEnd();
00174 for(unsigned int i=0; i<NumLEDs; i++)
00175 for(unsigned int f=0; f<NumFrames; f++)
00176 cmds[i][f].value=calcValue(i,t+f*FrameTime);
00177 bool tmp=dirty;
00178 dirty = numCycling>0;
00179 return tmp;
00180 }
00181
00182 void LedEngine::invert(LEDBitMask_t leds) {
00183 if(leds!=0) {
00184 dirty=true;
00185 for(unsigned int i=0; i<NumLEDs; i++)
00186 if((leds>>i)&1)
00187 if(infos[i].isCycling)
00188 infos[i].amp*=-1;
00189 else
00190 infos[i].value=1-infos[i].value;
00191 }
00192 }
00193 void LedEngine::set(LEDBitMask_t leds, float value) {
00194 if(leds!=0) {
00195 dirty=true;
00196 for(unsigned int i=0; i<NumLEDs; i++)
00197 if((leds>>i)&1) {
00198 infos[i].value=value;
00199 if(infos[i].isCycling) {
00200 numCycling--;
00201 infos[i].isCycling=false;
00202 }
00203 }
00204 }
00205 }
00206 void LedEngine::cflash(LEDBitMask_t leds, float value, unsigned int ms) {
00207 dirty=true;
00208 unsigned int t = get_time();
00209 if(t+ms<nextFlashEnd)
00210 nextFlashEnd=t+ms;
00211 for(unsigned int i=0; i<NumLEDs; i++) {
00212 infos[i].flashvalue=((leds>>i)&1)*value;
00213 infos[i].flashtime=t+ms;
00214 }
00215 }
00216 void LedEngine::flash(LEDBitMask_t leds, float value, unsigned int ms) {
00217 if(leds!=0) {
00218 dirty=true;
00219 unsigned int t = get_time();
00220 if(t+ms<nextFlashEnd)
00221 nextFlashEnd=t+ms;
00222 for(unsigned int i=0; i<NumLEDs; i++)
00223 if((leds>>i)&1) {
00224 infos[i].flashvalue=value;
00225 infos[i].flashtime=t+ms;
00226 }
00227 }
00228 }
00229 void LedEngine::flash(LEDBitMask_t leds, unsigned int ms) {
00230 if(leds!=0) {
00231 dirty=true;
00232 unsigned int t = get_time();
00233 if(t+ms<nextFlashEnd)
00234 nextFlashEnd=t+ms;
00235 for(unsigned int i=0; i<NumLEDs; i++)
00236 if((leds>>i)&1) {
00237 infos[i].flashvalue=calcFlash(calcValue(i,t));
00238 infos[i].flashtime=t+ms;
00239 }
00240 }
00241 }
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258 void LedEngine::cycle(LEDBitMask_t leds, unsigned int period, float amp, float offset, int phase) {
00259
00260 if(leds!=0) {
00261 dirty=true;
00262 unsigned int start = get_time()+phase;
00263 for(unsigned int i=0; i<NumLEDs; i++)
00264 if((leds>>i)&1) {
00265 if(!infos[i].isCycling)
00266 numCycling++;
00267 infos[i].isCycling=true;
00268 infos[i].amp=amp;
00269 infos[i].period=period;
00270 infos[i].starttime=start;
00271 infos[i].offset=offset;
00272 }
00273 }
00274 }
00275 void LedEngine::clear() {
00276 for(unsigned int i=0; i<NumLEDs; i++) {
00277 infos[i].value=0;
00278 infos[i].flashtime=0;
00279 infos[i].isCycling=false;
00280 }
00281 numCycling=0;
00282 dirty=true;
00283 }
00284
00285 void LedEngine::extendFlash(unsigned int ms) {
00286 for(unsigned int i=0; i<NumLEDs; i++)
00287 if(infos[i].flashtime!=0)
00288 infos[i].flashtime+=ms;
00289 if(nextFlashEnd!=0)
00290 nextFlashEnd+=ms;
00291 dirty=true;
00292 }
00293
00294 void LedEngine::displayNumber(int x, numStyle_t style) {
00295 switch(style) {
00296 case onedigit: {
00297 #ifdef TGT_HAS_LED_PANEL
00298 const LEDBitMask_t * numMasks=defaultCountNumMasks;
00299 if(RobotName == ERS210Info::TargetName)
00300 numMasks=defaultMimicNumMasks;
00301 else if(RobotName == ERS220Info::TargetName)
00302 numMasks=ERS220numMasks;
00303 else if(RobotName == ERS7Info::TargetName)
00304 numMasks=ERS7numMasks;
00305 if(x>9 || x<-9) {
00306 ccycle(FaceLEDMask&~TopBrLEDMask,333,10,-5);
00307 infos[TopBrLEDOffset-LEDOffset].value=x<0?1:0;
00308 } else {
00309 clear();
00310 if(x<0) {
00311 set(numMasks[-x],1);
00312 infos[TopBrLEDOffset-LEDOffset].value=infos[TopBrLEDOffset-LEDOffset].value*.5+.25;
00313 } else
00314 set(numMasks[x],1);
00315 }
00316 #elif defined(TGT_HAS_LEDS)
00317 if(NumLEDs<9)
00318 return;
00319 const LEDBitMask_t * numMasks=defaultCountNumMasks;
00320 if(x>9 || x<-9) {
00321 ccycle(~(1<<(NumLEDs-1)),333,10,-5);
00322 infos[NumLEDs-1].value=x<0?1:0;
00323 } else {
00324 clear();
00325 if(x<0) {
00326 set(numMasks[-x],1);
00327 infos[NumLEDs-1].value=infos[NumLEDs-1].value*.5+.25;
00328 } else
00329 set(numMasks[x],1);
00330 }
00331 #endif
00332 } break;
00333 case twodigit:
00334 #ifdef TGT_HAS_LED_PANEL
00335 if(x>99 || x<-99) {
00336 ccycle(FaceLEDMask&~TopBrLEDMask,333,10,-5);
00337 infos[TopBrLEDOffset-LEDOffset].value=x<0?1:0;
00338 } else {
00339 clear();
00340 if(x<0) {
00341 infos[TopBrLEDOffset-LEDOffset].value=1;
00342 x=-x;
00343 }
00344 setOneOfTwo(x/10,BotRLEDOffset-LEDOffset,MidRLEDOffset-LEDOffset,TopRLEDOffset-LEDOffset);
00345 setOneOfTwo(x%10,BotLLEDOffset-LEDOffset,MidLLEDOffset-LEDOffset,TopLLEDOffset-LEDOffset);
00346 }
00347 #elif defined(TGT_HAS_LEDS)
00348 if(NumLEDs<3)
00349 return;
00350 if(x>99 || x<-99) {
00351 ccycle(~(1<<(NumLEDs-1)),333,10,-5);
00352 infos[NumLEDs-1].value=x<0?1:0;
00353 } else {
00354 clear();
00355 if(x<0) {
00356 infos[NumLEDs-1].value=1;
00357 x=-x;
00358 }
00359 setOneOfTwo(x/10,NumLEDs-3,NumLEDs-2,NumLEDs-1);
00360 setOneOfTwo(x%10,2,1,0);
00361 }
00362 #endif
00363 break;
00364 }
00365 }
00366 void LedEngine::setOneOfTwo(unsigned int x, unsigned int low, unsigned int mid, unsigned int high) {
00367 if(x==0)
00368 return;
00369 float bg = ((x-1)/3)/3.0;
00370 float fg = bg+.333333333;
00371 if(RobotName == ERS7Info::TargetName)
00372 bg*=bg;
00373 switch(x%3) {
00374 case 1:
00375 infos[high].value=bg;
00376 infos[mid].value=bg;
00377 infos[low].value=fg;
00378 break;
00379 case 2:
00380 infos[high].value=bg;
00381 infos[mid].value=fg;
00382 infos[low].value=bg;
00383 break;
00384 case 0:
00385 infos[high].value=fg;
00386 infos[mid].value=bg;
00387 infos[low].value=bg;
00388 break;
00389 }
00390 }
00391
00392 void LedEngine::displayPercent(float x, percentStyle_t left_style, percentStyle_t right_style) {
00393 clear();
00394 #ifdef TGT_HAS_LED_PANEL
00395 if(x<0) {
00396 set(FaceLEDMask,.25);
00397 return;
00398 }
00399 if(x>1) {
00400 set(FaceLEDMask,.75);
00401 return;
00402 }
00403 if(left_style==major)
00404 setColumn(x,BotLLEDMask,MidLLEDMask,TopLLEDMask,TopBrLEDMask);
00405 if(right_style==major)
00406 setColumn(x,BotRLEDMask,MidRLEDMask,TopRLEDMask,TopBrLEDMask);
00407 x*=4;
00408 x-=(int)x;
00409 if(left_style==minor)
00410 setColumn(x,BotLLEDMask,MidLLEDMask,TopLLEDMask,TopBrLEDMask);
00411 if(right_style==minor)
00412 setColumn(x,BotRLEDMask,MidRLEDMask,TopRLEDMask,TopBrLEDMask);
00413 #else
00414 if(x<0) {
00415 set(~0,.25);
00416 return;
00417 }
00418 if(x>1) {
00419 set(~0,.75);
00420 return;
00421 }
00422 if(left_style==major)
00423 setColumn(x,3,2,1,0);
00424 if(right_style==major)
00425 setColumn(x,NumLEDs-4,NumLEDs-3,NumLEDs-2,NumLEDs-1);
00426 x*=4;
00427 x-=(int)x;
00428 if(left_style==minor)
00429 setColumn(x,3,2,1,0);
00430 if(right_style==minor)
00431 setColumn(x,NumLEDs-4,NumLEDs-3,NumLEDs-2,NumLEDs-1);
00432 #endif
00433 }
00434
00435 void LedEngine::setColumn(float x, unsigned int low, unsigned int mid, unsigned int high, unsigned int top) {
00436 LEDBitMask_t solid=0;
00437 LEDBitMask_t partial=0;
00438 switch((int)(4*x)) {
00439 case 4:
00440 solid|=top;
00441 case 3:
00442 solid|=high;
00443 case 2:
00444 solid|=mid;
00445 case 1:
00446 solid|=low;
00447 }
00448 switch((int)(4*x)) {
00449 case 3:
00450 partial=top; break;
00451 case 2:
00452 partial=high; break;
00453 case 1:
00454 partial=mid; break;
00455 case 0:
00456 partial=low; break;
00457 }
00458 float partialvalue=(x*4)-(int)(x*4);
00459 set(partial,partialvalue);
00460 set(solid,1);
00461 }
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471
00472
00473