GT2005: Modules/LEDControl/GT2005LEDControl.cpp Source File

00001 /**
00002 * @file GT2005LEDControl.cpp
00003 * 
00004 * This file contains a class for LED Control.
00005 * @author Matthias Jüngel
00006 * @author Max Risler
00007 * @author Michael Wachter
00008 */
00009 
00010 #include "Representations/Motion/JointDataBuffer.h"
00011 #include "GT2005LEDControl.h"
00012 #include "Tools/Player.h"
00013 #include "Tools/RobotConfiguration.h"
00014 #include "Platform/GTAssert.h"
00015 #include "Platform/GTAssert.h"
00016 #include "Tools/Debugging/Debugging.h"
00017 #include "Tools/Debugging/DebugDataTable.h"
00018 
00019 GT2005LEDControl::GT2005LEDControl(LEDControlInterfaces& interfaces) 
00020 : LEDControl(interfaces),
00021 executeCallCount(0), lastMotionFrameNumber(0)
00022 {
00023 }
00024 
00025 void GT2005LEDControl::showRequestedHeadLEDs()
00026 {
00027   const int numOfBlinkSequences = 16;
00028   const int lengthOfBlinkSequences = 4;
00029   
00030   const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
00031   {
00032     {0,0,0,0},{0,0,0,1},{0,0,1,0},{0,0,1,1},
00033     {0,1,0,0},{0,1,0,1},{0,1,1,0},{0,1,1,1},
00034     {1,0,0,0},{1,0,0,1},{1,0,1,0},{1,0,1,1},
00035     {1,1,0,0},{1,1,0,1},{1,1,1,0},{1,1,1,1}
00036   };  
00037   
00038   int c = (executeCallCount) / 16;
00039   int i;
00040   
00041   i = (int)ledRequest.headWhiteLED;
00042   if(i > numOfBlinkSequences) i = 0;
00043 #ifndef LEDINTENSITY
00044   ledCode |= (headWhite * blinkSequences[i][c % lengthOfBlinkSequences]);
00045 #else
00046   ledValue.intensity[0][LEDValue::headWhite] = blinkSequences[i][c % lengthOfBlinkSequences]==1 ? 255 : 0;
00047 #endif
00048   
00049   i = (int)ledRequest.headOrangeLED;
00050   if(i > numOfBlinkSequences) i = 0;
00051 #ifndef LEDINTENSITY
00052   ledCode |= (headOrange * blinkSequences[i][c % lengthOfBlinkSequences]);
00053 #else
00054   ledValue.intensity[0][LEDValue::headOrange] = blinkSequences[i][c % lengthOfBlinkSequences]==1 ? 255 : 0;
00055 #endif
00056 }
00057 
00058 void GT2005LEDControl::showRequestedFaceLEDs()
00059 {
00060   const int numOfBlinkSequences = 16;
00061   const int lengthOfBlinkSequences = 4;
00062   
00063   const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
00064   {
00065     {0,0,0,0},{0,0,0,1},{0,0,1,0},{0,0,1,1},
00066     {0,1,0,0},{0,1,0,1},{0,1,1,0},{0,1,1,1},
00067     {1,0,0,0},{1,0,0,1},{1,0,1,0},{1,0,1,1},
00068     {1,1,0,0},{1,1,0,1},{1,1,1,0},{1,1,1,1}
00069   };  
00070   
00071   int c = (executeCallCount) / 16;
00072   {
00073   for(int faceLEDNumber = 1; faceLEDNumber <= 14; faceLEDNumber++)
00074   {
00075     int i = (int)ledRequest.faceLED[faceLEDNumber-1];
00076     if(i > numOfBlinkSequences) i = 0;
00077     
00078 #ifndef LEDINTENSITY
00079     int faceLed = (face1 * blinkSequences[i][c % lengthOfBlinkSequences]); 
00080     faceLed = faceLed << (faceLEDNumber - 1);
00081     ledCode |= faceLed;
00082 #else
00083     ledValue.intensity[0][LEDValue::face1 + faceLEDNumber - 1] 
00084       = blinkSequences[i][c % lengthOfBlinkSequences]==1 ? 255 : 0;
00085 #endif
00086 
00087   }
00088 }
00089 }
00090 
00091 void GT2005LEDControl::showRequestedBackWhiteLEDs()
00092 {
00093   const int numOfBlinkSequences = 16;
00094   const int lengthOfBlinkSequences = 4;
00095   
00096   const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
00097   {
00098     {0,0,0,0},{0,0,0,1},{0,0,1,0},{0,0,1,1},
00099     {0,1,0,0},{0,1,0,1},{0,1,1,0},{0,1,1,1},
00100     {1,0,0,0},{1,0,0,1},{1,0,1,0},{1,0,1,1},
00101     {1,1,0,0},{1,1,0,1},{1,1,1,0},{1,1,1,1}
00102   };  
00103   
00104   int c = (executeCallCount) / 16;
00105   int i;
00106   
00107   i = (int)ledRequest.backFrontWhiteLED;
00108   if(i > numOfBlinkSequences) i = 0;
00109 #ifndef LEDINTENSITY
00110   ledCode |= (backFrontWhite * blinkSequences[i][c % lengthOfBlinkSequences]);
00111 #else
00112     ledValue.intensity[0][LEDValue::backFrontWhite] 
00113       = blinkSequences[i][c % lengthOfBlinkSequences]==1 ? 255 : 0;
00114 #endif
00115   
00116   i = (int)ledRequest.backMiddleWhiteLED;
00117   if(i > numOfBlinkSequences) i = 0;
00118 #ifndef LEDINTENSITY
00119   ledCode |= (backMiddleWhite * blinkSequences[i][c % lengthOfBlinkSequences]);
00120 #else
00121     ledValue.intensity[0][LEDValue::backMiddleWhite] 
00122       = blinkSequences[i][c % lengthOfBlinkSequences]==1 ? 255 : 0;
00123 #endif
00124   
00125   i = (int)ledRequest.backRearWhiteLED;
00126   if(i > numOfBlinkSequences) i = 0;
00127 #ifndef LEDINTENSITY
00128   ledCode |= (backRearWhite * blinkSequences[i][c % lengthOfBlinkSequences]);
00129 #else
00130     ledValue.intensity[0][LEDValue::backRearWhite] 
00131       = blinkSequences[i][c % lengthOfBlinkSequences]==1 ? 255 : 0;
00132 #endif
00133 }
00134 
00135 void GT2005LEDControl::showRequestedBackColoredLEDs()
00136 {
00137   const int numOfBlinkSequences = 16;
00138   const int lengthOfBlinkSequences = 4;
00139   
00140   const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
00141   {
00142     {0,0,0,0},{0,0,0,1},{0,0,1,0},{0,0,1,1},
00143     {0,1,0,0},{0,1,0,1},{0,1,1,0},{0,1,1,1},
00144     {1,0,0,0},{1,0,0,1},{1,0,1,0},{1,0,1,1},
00145     {1,1,0,0},{1,1,0,1},{1,1,1,0},{1,1,1,1}
00146   };  
00147   
00148   int c = (executeCallCount) / 16;
00149   int i;
00150   
00151   i = (int)ledRequest.backFrontBlueLED;
00152   if(i > numOfBlinkSequences) i = 0;
00153 #ifndef LEDINTENSITY
00154   ledCode |= (backFrontBlue * blinkSequences[i][c % lengthOfBlinkSequences]);
00155 #else
00156     ledValue.intensity[0][LEDValue::backFrontBlue] 
00157       = blinkSequences[i][c % lengthOfBlinkSequences]==1 ? 255 : 0;
00158 #endif
00159   
00160   i = (int)ledRequest.backMiddleOrangeLED;
00161   if(i > numOfBlinkSequences) i = 0;
00162 #ifndef LEDINTENSITY
00163   ledCode |= (backMiddleOrange * blinkSequences[i][c % lengthOfBlinkSequences]);
00164 #else
00165     ledValue.intensity[0][LEDValue::backMiddleOrange] 
00166       = blinkSequences[i][c % lengthOfBlinkSequences]==1 ? 255 : 0;
00167 #endif
00168   
00169   i = (int)ledRequest.backRearRedLED;
00170   if(i > numOfBlinkSequences) i = 0;
00171 #ifndef LEDINTENSITY
00172   ledCode |= (backRearRed * blinkSequences[i][c % lengthOfBlinkSequences]);
00173 #else
00174     ledValue.intensity[0][LEDValue::backRearRed] 
00175       = blinkSequences[i][c % lengthOfBlinkSequences]==1 ? 255 : 0;
00176 #endif
00177 }
00178 
00179 
00180 
00181 void GT2005LEDControl::showBatteryState()
00182 {
00183   const int numOfBlinkSequences = 2;
00184   const int lengthOfBlinkSequences = 4;
00185   const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
00186   {
00187     {1,1,1,1},{0,0,1,1}
00188   };  
00189 
00190   int battPower = SystemCall::getRemainingPower();
00191 
00192   int c = (executeCallCount) / 16;
00193 
00194   if (battPower < 25) 
00195 #ifndef LEDINTENSITY
00196     ledCode |= (wireless * blinkSequences[0][c % lengthOfBlinkSequences]);
00197 #else
00198     ledValue.intensity[0][LEDValue::wireless] 
00199       = 255 * (100-battPower) / 100;
00200 #endif
00201   else if (battPower < 15) 
00202 #ifndef LEDINTENSITY
00203     ledCode |= (wireless * blinkSequences[1][c % lengthOfBlinkSequences]);
00204 #else
00205     ledValue.intensity[0][LEDValue::wireless] 
00206     = blinkSequences[1][c % lengthOfBlinkSequences]==1?255 * (100-battPower) / 100:0;
00207 #endif
00208 }
00209 
00210 void GT2005LEDControl::showCognitionFps()
00211 {
00212   if(ledRequest.cognitionFps >= 29)
00213   {
00214     ledCode |= face7;
00215     ledCode |= face9;
00216     ledCode |= face11;
00217     ledCode |= face10;
00218     ledCode |= face8;
00219   }
00220   else if(ledRequest.cognitionFps >= 26)
00221   {
00222     ledCode |= face7;
00223     ledCode |= face9;
00224     ledCode |= face11;
00225     ledCode |= face10;
00226     ledCode &= ~face8;
00227   }
00228   else if(ledRequest.cognitionFps >= 23)
00229   {
00230     ledCode |= face7;
00231     ledCode |= face9;
00232     ledCode |= face11;
00233     ledCode &= ~face10;
00234     ledCode &= ~face8;
00235   }
00236   else if(ledRequest.cognitionFps >= 20)
00237   {
00238     ledCode |= face7;
00239     ledCode |= face9;
00240     ledCode &= ~face11;
00241     ledCode &= ~face10;
00242     ledCode &= ~face8;
00243   }
00244   else if(ledRequest.cognitionFps >= 17)
00245   {
00246     ledCode |= face7;
00247     ledCode &= ~face9;
00248     ledCode &= ~face11;
00249     ledCode &= ~face10;
00250     ledCode &= ~face8;
00251   }
00252   else
00253   {
00254     ledCode &= ~face7;
00255     ledCode &= ~face9;
00256     ledCode &= ~face11;
00257     ledCode &= ~face10;
00258     ledCode &= ~face8;
00259   }
00260 
00261 
00262 }
00263 
00264 void GT2005LEDControl::showWLANStatus()
00265 {
00266   // color to be used dependending on count of roboters this robot is connected to
00267   // {0,1,2,3,4,5}
00268   int bits[] = {off, modeRed + modeGreen + modeBlue, modeGreen, modeBlue, modeRed + modeGreen + modeBlue};
00269   int wLanCount = 0;
00270   if (wLanStatus & 1) wLanCount++;
00271   if (wLanStatus & 2) wLanCount++;
00272   if (wLanStatus & 4) wLanCount++;
00273   if (wLanStatus & 8) wLanCount++;
00274 
00275 #ifndef LEDINTENSITY
00276   ledCode |= bits[wLanCount];
00277 #else
00278   ledValue.intensity[0][LEDValue::modeRed] = (bits[wLanCount] && modeRed) != 0 ?255:0;
00279   ledValue.intensity[0][LEDValue::modeGreen] = (bits[wLanCount] && modeGreen) != 0 ?255:0;
00280   ledValue.intensity[0][LEDValue::modeBlue] = (bits[wLanCount] && modeBlue) != 0 ?255:0;
00281 #endif
00282 }
00283 
00284 void GT2005LEDControl::showCognitionProcessFrameLostWarning()
00285 {
00286   if(ledRequest.showCognitionFrameLostWarning) 
00287   {
00288     ledCode |= headWhite;
00289     ledCode |= modeRed;
00290     ledCode |= modeGreen;
00291     ledCode |= modeBlue;
00292     ledCode |= wireless;
00293     ledCode |= face1;
00294     ledCode |= face2;
00295     ledCode |= face3;
00296     ledCode |= face4;
00297     ledCode |= face5;
00298     ledCode |= face6;
00299     ledCode |= face7;
00300     ledCode |= face8;
00301     ledCode |= face9;
00302     ledCode |= face10;
00303     ledCode |= face11;
00304     ledCode |= face12;
00305     ledCode |= face13;
00306     ledCode |= face14;
00307   }
00308 }
00309 
00310 void GT2005LEDControl::showMotionProcessFrameLostWarning()
00311 {
00312   if( (frameNumber - lastMotionFrameNumber) != 1 ) 
00313   {
00314     ledCode |= backFrontBlue;
00315     ledCode |= backFrontWhite;
00316     ledCode |= backMiddleOrange;
00317     ledCode |= backMiddleWhite;
00318     ledCode |= backRearRed;
00319     ledCode |= backRearWhite;
00320   }
00321   lastMotionFrameNumber = frameNumber;
00322 }
00323 #ifdef NDEBUG
00324 #define SHOWLED( nameon,nameoff,tmp ) \
00325   show##tmp##(); \
00326 
00327 #else
00328 #define SHOWLED( nameon,nameoff,tmp ) \
00329 { bool enabled = true; \
00330 DEBUG_RESPONSE(name, enabled = true;); \
00331 DEBUG_RESPONSE(name, enabled = false;); \
00332   if (enabled) \
00333     show##tmp##(); \
00334 } 
00335 #endif
00336 
00337 void GT2005LEDControl::execute()
00338 {
00339   ledCode = off;
00340 
00341   DEBUG_RESPONSE_NOT("LED, tail, mouth:don't show WLAN Status", showWLANStatus(); );
00342   DEBUG_RESPONSE_NOT("LED, tail, mouth:don't show BatteryState", showBatteryState(); );
00343   DEBUG_RESPONSE_NOT("LED, tail, mouth:don't show RequestedFaceLEDs", showRequestedFaceLEDs(); );
00344   DEBUG_RESPONSE_NOT("LED, tail, mouth:don't show RequestedBackWhiteLEDs", showRequestedBackWhiteLEDs(); );
00345   DEBUG_RESPONSE_NOT("LED, tail, mouth:don't show RequestedBackColoredLEDs", showRequestedBackColoredLEDs(); );
00346   DEBUG_RESPONSE_NOT("LED, tail, mouth:don't show RequestedHeadLEDs", showRequestedHeadLEDs(); );
00347 
00348   DEBUG_RESPONSE("LED, tail, mouth:show CognitionProcessFrameLostWarning", showCognitionProcessFrameLostWarning(); );
00349   DEBUG_RESPONSE("LED, tail, mouth:show MotionProcessFrameLostWarning", showMotionProcessFrameLostWarning(); );
00350   DEBUG_RESPONSE("Processes:Cognition - measure fps", showCognitionFps(); );
00351 #ifdef NDEBUG
00352 //  showCognitionFps();
00353   showMotionProcessFrameLostWarning();
00354 #endif
00355 
00356   executeCallCount++;
00357   ledValue.data[0] = ledCode;
00358 //  ledValue.modeA = ledRequest.faceLEDModeA;
00359 //    ledValue.modeA = false;
00360   //static int tmp;
00361   //MODIFY("LED:TEST:VALUE",tmp);
00362   //ledValue.intensity[0][LEDValue::backMiddleOrange] = 0;
00363   //ledValue.intensity[0][LEDValue::backRearRed] = 0;
00364   //ledValue.intensity[0][LEDValue::backFrontBlue] = 0;
00365   //ledValue.intensity[0][LEDValue::backRearWhite] = 255;
00366   //ledValue.intensity[0][LEDValue::backMiddleWhite] = tmp;
00367   //ledValue.intensity[0][LEDValue::backFrontWhite] = 0;
00368   ledValue.modeA = false;
00369 }