Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/LEDControl/GT2005LEDControl.cpp

Go to the documentation of this 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 }