Modules/LEDControl/GT2004LEDControl.cpp

Go to the documentation of this file.

/**
* @file GT2004LEDControl.cpp
* 
* This file contains a class for LED Control.
* @author Matthias Jüngel
* @author Max Risler
* @author Michael Wachter
*/

#include "Representations/Motion/JointDataBuffer.h"
#include "GT2004LEDControl.h"
#include "Tools/Player.h"
#include "Tools/RobotConfiguration.h"
#include "Platform/GTAssert.h"

GT2004LEDControl::GT2004LEDControl(LEDControlInterfaces& interfaces) 
: LEDControl(interfaces),
executeCallCount(0), lastMotionFrameNumber(0)
{
}

void GT2004LEDControl::showRequestedHeadLEDs()
{
  const int numOfBlinkSequences = 16;
  const int lengthOfBlinkSequences = 4;
  
  const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
  {
    {0,0,0,0},{0,0,0,1},{0,0,1,0},{0,0,1,1},
    {0,1,0,0},{0,1,0,1},{0,1,1,0},{0,1,1,1},
    {1,0,0,0},{1,0,0,1},{1,0,1,0},{1,0,1,1},
    {1,1,0,0},{1,1,0,1},{1,1,1,0},{1,1,1,1}
  };  
  
  int c = (executeCallCount) / 16;
  int i;
  
  i = (int)ledRequest.headWhiteLED;
  if(i > numOfBlinkSequences) i = 0;
  ledCode |= (headWhite * blinkSequences[i][c % lengthOfBlinkSequences]);
  
  i = (int)ledRequest.headOrangeLED;
  if(i > numOfBlinkSequences) i = 0;
  ledCode |= (headOrange * blinkSequences[i][c % lengthOfBlinkSequences]);
}

void GT2004LEDControl::showRequestedFaceLEDs()
{
  const int numOfBlinkSequences = 16;
  const int lengthOfBlinkSequences = 4;
  
  const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
  {
    {0,0,0,0},{0,0,0,1},{0,0,1,0},{0,0,1,1},
    {0,1,0,0},{0,1,0,1},{0,1,1,0},{0,1,1,1},
    {1,0,0,0},{1,0,0,1},{1,0,1,0},{1,0,1,1},
    {1,1,0,0},{1,1,0,1},{1,1,1,0},{1,1,1,1}
  };  
  
  int c = (executeCallCount) / 16;
  for(int faceLEDNumber = 1; faceLEDNumber <= 14; faceLEDNumber++)
  {
    int i = (int)ledRequest.faceLED[faceLEDNumber-1];
    if(i > numOfBlinkSequences) i = 0;
    
    int faceLed = (face1 * blinkSequences[i][c % lengthOfBlinkSequences]); 
    faceLed = faceLed << (faceLEDNumber - 1);
    ledCode |= faceLed;
  }
}

void GT2004LEDControl::showRequestedBackWhiteLEDs()
{
  const int numOfBlinkSequences = 16;
  const int lengthOfBlinkSequences = 4;
  
  const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
  {
    {0,0,0,0},{0,0,0,1},{0,0,1,0},{0,0,1,1},
    {0,1,0,0},{0,1,0,1},{0,1,1,0},{0,1,1,1},
    {1,0,0,0},{1,0,0,1},{1,0,1,0},{1,0,1,1},
    {1,1,0,0},{1,1,0,1},{1,1,1,0},{1,1,1,1}
  };  
  
  int c = (executeCallCount) / 16;
  int i;
  
  i = (int)ledRequest.backFrontWhiteLED;
  if(i > numOfBlinkSequences) i = 0;
  ledCode |= (backFrontWhite * blinkSequences[i][c % lengthOfBlinkSequences]);
  
  i = (int)ledRequest.backMiddleWhiteLED;
  if(i > numOfBlinkSequences) i = 0;
  ledCode |= (backMiddleWhite * blinkSequences[i][c % lengthOfBlinkSequences]);
  
  i = (int)ledRequest.backRearWhiteLED;
  if(i > numOfBlinkSequences) i = 0;
  ledCode |= (backRearWhite * blinkSequences[i][c % lengthOfBlinkSequences]);
}

void GT2004LEDControl::showRequestedBackColoredLEDs()
{
  const int numOfBlinkSequences = 16;
  const int lengthOfBlinkSequences = 4;
  
  const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
  {
    {0,0,0,0},{0,0,0,1},{0,0,1,0},{0,0,1,1},
    {0,1,0,0},{0,1,0,1},{0,1,1,0},{0,1,1,1},
    {1,0,0,0},{1,0,0,1},{1,0,1,0},{1,0,1,1},
    {1,1,0,0},{1,1,0,1},{1,1,1,0},{1,1,1,1}
  };  
  
  int c = (executeCallCount) / 16;
  int i;
  
  i = (int)ledRequest.backFrontBlueLED;
  if(i > numOfBlinkSequences) i = 0;
  ledCode |= (backFrontBlue * blinkSequences[i][c % lengthOfBlinkSequences]);
  
  i = (int)ledRequest.backMiddleOrangeLED;
  if(i > numOfBlinkSequences) i = 0;
  ledCode |= (backMiddleOrange * blinkSequences[i][c % lengthOfBlinkSequences]);
  
  i = (int)ledRequest.backRearRedLED;
  if(i > numOfBlinkSequences) i = 0;
  ledCode |= (backRearRed * blinkSequences[i][c % lengthOfBlinkSequences]);
}



void GT2004LEDControl::showBatteryState()
{
  const int numOfBlinkSequences = 2;
  const int lengthOfBlinkSequences = 4;
  const int blinkSequences[numOfBlinkSequences][lengthOfBlinkSequences] =
  {
    {1,1,1,1},{0,0,1,1}
  };  

  int battPower = SystemCall::getRemainingPower();

  int c = (executeCallCount) / 16;

  if (battPower < 25) 
    ledCode |= (wireless * blinkSequences[0][c % lengthOfBlinkSequences]);
  else if (battPower < 15) 
    ledCode |= (wireless * blinkSequences[1][c % lengthOfBlinkSequences]);
}

void GT2004LEDControl::showWLANStatus()
{
  
  int bits[] = {off, modeRed, modeRed + modeGreen + modeBlue, modeGreen, modeRed + modeGreen + modeBlue};
  int wLanCount = 0;
  if (wLanStatus & 1) wLanCount++;
  if (wLanStatus & 2) wLanCount++;
  if (wLanStatus & 4) wLanCount++;
  if (wLanStatus & 8) wLanCount++;

  ledCode |= bits[wLanCount];
}

void GT2004LEDControl::showCognitionProcessFrameLostWarning()
{
  if(ledRequest.showCognitionFrameLostWarning) 
  {
    ledCode |= headWhite;
    ledCode |= modeRed;
    ledCode |= modeGreen;
    ledCode |= modeBlue;
    ledCode |= wireless;
    ledCode |= face1;
    ledCode |= face2;
    ledCode |= face3;
    ledCode |= face4;
    ledCode |= face5;
    ledCode |= face6;
    ledCode |= face7;
    ledCode |= face8;
    ledCode |= face9;
    ledCode |= face10;
    ledCode |= face11;
    ledCode |= face12;
    ledCode |= face13;
    ledCode |= face14;
  }
}

void GT2004LEDControl::showMotionProcessFrameLostWarning()
{
  if( (frameNumber - lastMotionFrameNumber) != 1 ) 
  {
    ledCode |= backFrontBlue;
    ledCode |= backFrontWhite;
    ledCode |= backMiddleOrange;
    ledCode |= backMiddleWhite;
    ledCode |= backRearRed;
    ledCode |= backRearWhite;
  }
  lastMotionFrameNumber = frameNumber;
}


void GT2004LEDControl::execute()
{
  ledCode = off;
  
  showWLANStatus();
  showBatteryState();
  showRequestedFaceLEDs();
  showRequestedBackWhiteLEDs();
  showRequestedBackColoredLEDs();
  showRequestedHeadLEDs();

  showCognitionProcessFrameLostWarning();
  showMotionProcessFrameLostWarning();

  executeCallCount++;
  ledValue.data[0] = ledCode;
}

/*
* Change log :
* 
* $Log: GT2004LEDControl.cpp,v $
* Revision 1.5  2004/06/29 09:23:38  petters
* removed yellow from WLan Led Status
*
* Revision 1.4  2004/06/20 10:30:40  juengel
* Removed orange from frame-lost warning.
*
* Revision 1.3  2004/06/15 17:49:06  juengel
* Added showMotionProcessFrameLostWarning() and showCognitionProcessFrameLostWarning.
*
* Revision 1.2  2004/06/09 07:46:34  juengel
* Added showFrameLostWarning().
*
* Revision 1.1  2004/05/22 22:16:10  loetzsch
* renamed ATH2004LEDControl to GT2004LEDControl
*
* Revision 1.1.1.1  2004/05/22 17:20:14  cvsadm
* created new repository GT2004_WM
*
* Revision 1.9  2004/05/14 14:12:07  wachter
* - Added communication support for 5 robots
* - rewrote parts of team-communication to be faster and more stable
*
* Revision 1.8  2004/04/07 12:28:56  risler
* ddd checkin after go04 - first part
*
* Revision 1.2  2004/03/31 00:10:37  risler
* bugfix battery led now blinks as intended
*
* Revision 1.1.1.1  2004/03/29 08:28:47  Administrator
* initial transfer from tamara
*
* Revision 1.7  2004/03/25 21:20:31  juengel
* Added head LEDs.
*
* Revision 1.6  2004/03/25 19:50:25  juengel
* initialLedCodes are not used any more
*
* Revision 1.5  2004/03/25 17:39:17  loetzsch
* renamed initial-setup-mode to back-mode
*
* Revision 1.4  2004/03/21 19:08:16  juengel
* Added back*White LED support.
*
* Revision 1.3  2004/03/20 19:24:07  juengel
* Added face LEDs to LEDRequest
*
* Revision 1.2  2004/03/20 17:17:58  juengel
* no message
*
* Revision 1.1  2004/03/16 14:00:21  juengel
* Integrated Improvments from "Günne"
* -ATH2004ERS7Behavior
* -ATHHeadControl
* -KickSelectionTable
* -KickEditor
*
* Revision 1.1  2004/03/15 17:11:39  hoffmann
* - added ATH2004HeadControl
* - added ATH2004LEDControl
* - headmotiontester shows "tilt2"
* - motion process updates odometry while no new robotPose is received, added to motion request
* - some ui adjustments
* - added member function to "field" to find out if robot is in own penalty area for use in the obstacles locator
*
*/

---