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 if(leds!=0) {
00144 dirty=true;
00145 unsigned int t = get_time();
00146 if(t+ms<dirtyTime)
00147 dirtyTime=t+ms;
00148 for(unsigned int i=0; i<NumLEDs; i++) {
00149 infos[i].flashvalue=((leds>>i)&1)*value;
00150 infos[i].flashtime=t+ms;
00151 }
00152 }
00153 }
00154 void LedEngine::flash(LEDBitMask_t leds, unsigned int ms) {
00155 if(leds!=0) {
00156 dirty=true;
00157 unsigned int t = get_time();
00158 if(t+ms<dirtyTime)
00159 dirtyTime=t+ms;
00160 for(unsigned int i=0; i<NumLEDs; i++)
00161 if((leds>>i)&1) {
00162 infos[i].flashvalue=calcFlash(calcValue(i,t));
00163 infos[i].flashtime=t+ms;
00164 }
00165 }
00166 }
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183 void LedEngine::cycle(LEDBitMask_t leds, unsigned int period, float amp, float offset, int phase) {
00184
00185 if(leds!=0) {
00186 dirty=true;
00187 unsigned int start = get_time()+phase;
00188 for(unsigned int i=0; i<NumLEDs; i++)
00189 if((leds>>i)&1) {
00190 if(!infos[i].isCycling)
00191 numCycling++;
00192 infos[i].isCycling=true;
00193 infos[i].amp=amp;
00194 infos[i].period=period;
00195 infos[i].starttime=start;
00196 infos[i].offset=offset;
00197 }
00198 }
00199 }
00200 void LedEngine::clear() {
00201 for(unsigned int i=0; i<NumLEDs; i++) {
00202 infos[i].value=0;
00203 infos[i].isCycling=false;
00204 }
00205 numCycling=0;
00206 dirty=true;
00207 }
00208
00209 void LedEngine::displayNumber(int x, numStyle_t style) {
00210 switch(style) {
00211 case onedigit: {
00212 const LEDBitMask_t * numMasks=ERS210numMasks;
00213 if(state->robotDesign & WorldState::ERS220Mask)
00214 numMasks=ERS220numMasks;
00215 if(state->robotDesign & WorldState::ERS7Mask)
00216 numMasks=ERS7numMasks;
00217 if(x>9 || x<-9) {
00218 ccycle(FaceLEDMask&~TopBrLEDMask,333,10,-5);
00219 infos[TopBrLEDOffset-LEDOffset].value=x<0?1:0;
00220 } else {
00221 clear();
00222 if(x<0) {
00223 set(numMasks[-x],1);
00224 infos[TopBrLEDOffset-LEDOffset].value=infos[TopBrLEDOffset-LEDOffset].value*.5+.25;
00225 } else
00226 set(numMasks[x],1);
00227 }
00228 } break;
00229 case twodigit:
00230 if(x>99 || x<-99) {
00231 ccycle(FaceLEDMask&~TopBrLEDMask,333,10,-5);
00232 infos[TopBrLEDOffset-LEDOffset].value=x<0?1:0;
00233 } else {
00234 clear();
00235 if(x<0) {
00236 infos[TopBrLEDOffset-LEDOffset].value=1;
00237 x=-x;
00238 }
00239 setOneOfTwo(x%10,BotLLEDOffset-LEDOffset,MidLLEDOffset-LEDOffset,TopLLEDOffset-LEDOffset);
00240 setOneOfTwo(x/10,BotRLEDOffset-LEDOffset,MidRLEDOffset-LEDOffset,TopRLEDOffset-LEDOffset);
00241 }
00242 break;
00243 }
00244 }
00245 void LedEngine::setOneOfTwo(unsigned int x, unsigned int low, unsigned int mid, unsigned int high) {
00246 if(x==0)
00247 return;
00248 float bg = ((x-1)/3)/3.0;
00249 float fg = bg+.333333333;
00250 switch(x%3) {
00251 case 1:
00252 infos[high].value=bg;
00253 infos[mid].value=bg;
00254 infos[low].value=fg;
00255 break;
00256 case 2:
00257 infos[high].value=bg;
00258 infos[mid].value=fg;
00259 infos[low].value=bg;
00260 break;
00261 case 0:
00262 infos[high].value=fg;
00263 infos[mid].value=bg;
00264 infos[low].value=bg;
00265 break;
00266 }
00267 }
00268
00269 void LedEngine::displayPercent(float x, percentStyle_t left_style, percentStyle_t right_style) {
00270 clear();
00271 if(x<0) {
00272 set(FaceLEDMask,.25);
00273 return;
00274 }
00275 if(x>1) {
00276 set(FaceLEDMask,.75);
00277 return;
00278 }
00279 if(left_style==major)
00280 setColumn(x,BotLLEDMask,MidLLEDMask,TopLLEDMask,TopBrLEDMask);
00281 if(right_style==major)
00282 setColumn(x,BotRLEDMask,MidRLEDMask,TopRLEDMask,TopBrLEDMask);
00283 x*=4;
00284 x-=(int)x;
00285 if(left_style==minor)
00286 setColumn(x,BotLLEDMask,MidLLEDMask,TopLLEDMask,TopBrLEDMask);
00287 if(right_style==minor)
00288 setColumn(x,BotRLEDMask,MidRLEDMask,TopRLEDMask,TopBrLEDMask);
00289 }
00290
00291 void LedEngine::setColumn(float x, unsigned int low, unsigned int mid, unsigned int high, unsigned int top) {
00292 LEDBitMask_t solid=0;
00293 LEDBitMask_t partial=0;
00294 switch((int)(4*x)) {
00295 case 4:
00296 solid|=top;
00297 case 3:
00298 solid|=high;
00299 case 2:
00300 solid|=mid;
00301 case 1:
00302 solid|=low;
00303 }
00304 switch((int)(4*x)) {
00305 case 3:
00306 partial=top; break;
00307 case 2:
00308 partial=high; break;
00309 case 1:
00310 partial=mid; break;
00311 case 0:
00312 partial=low; break;
00313 }
00314 float partialvalue=(x*4)-(int)(x*4);
00315 set(partial,partialvalue);
00316 set(solid,1);
00317 }
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329