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 ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopLLEDMask,
00012 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00013 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00014 ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask,
00015 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopBrLEDMask,
00016 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopBrLEDMask,
00017 ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00018 ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00019 ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask,
00020 ERS210Info::BotLLEDMask
00021 };
00022
00023 const LEDBitMask_t LedEngine::ERS220numMasks[11] = {
00024 ERS220Info::ModeLEDMask,
00025 ERS220Info::FaceBackLeftLEDMask,
00026 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask,
00027 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask,
00028 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask,
00029 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask,
00030 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask,
00031 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask|ERS220Info::FaceFrontALEDMask,
00032 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask|ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask,
00033 ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask|ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask|ERS220Info::FaceFrontCLEDMask,
00034 ERS220Info::FaceFrontLeftLEDMask
00035 };
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 const LEDBitMask_t LedEngine::ERS7numMasks[11] = {
00054 0,
00055 (ERS7Info::FaceLEDPanelMask<<11),
00056 (ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<< 5),
00057 (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<<11)|(ERS7Info::FaceLEDPanelMask<< 3),
00058 (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9),
00059 (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9)|(ERS7Info::FaceLEDPanelMask<<11),
00060 (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7),
00061 (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)|(ERS7Info::FaceLEDPanelMask<<11),
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),
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),
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) {
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
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194 void LedEngine::cycle(LEDBitMask_t leds, unsigned int period, float amp, float offset, int phase) {
00195
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;
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
00332
00333
00334
00335
00336
00337
00338
00339
00340