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