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

# Modules/RobotStateDetector/GT2005RobotStateDetector.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005RobotStateDetector.cpp
    00003 *
    00004 * Implementation of class GT2005RobotStateDetector
    00005 *
    00006 * @author Max Risler
    00007 */
    00008
    00009 #include "GT2005RobotStateDetector.h"
    00010 #include "Platform/SystemCall.h"
    00011
    00012 GT2005RobotStateDetector::GT2005RobotStateDetector(const RobotStateDetectorInterfaces& interfaces)
    00013 : RobotStateDetector(interfaces)
    00014 {
    00015   int i,j,k;
    00016
    00017   for (i = 0; i < BodyPercept::numOfSwitches;switchDown[i++]=false);
    00018
    00019   for (i = 0; i < 6; i++)
    00020   {
    00021     timeSinceLastCollision[i] = 0;
    00022     consecutiveCollisionTime[i] = 0;
    00023   }
    00024
    00025   remindCollision = 750;
    00026
    00027   for (j = 0; j < maxFrameHistory; j++)
    00028   {
    00029     for (k = 0; k < robotSides; k++)
    00030     {
    00031       numOfCollisions[j][k] = false;
    00032     }
    00033   }
    00034
    00035   /****/
    00036   for (i = 0; i < BodyPercept::numOfSwitches;i++)
    00037   {
    00038     buttonPressed[i]=false;
    00039     buttonTime[i]=0;
    00040     buttonPressedTime[i]=0;
    00041     buttonDuration[i]=0;
    00042   }
    00043   anyBackButtonPressed=false;
    00044   anyBackButtonTime=0;
    00045   anyBackButtonPressedTime=0;
    00046   anyBackButtonDuration=0;
    00047   setNewButtonStatus();
    00048   /****/
    00049 }
    00050
    00051 void GT2005RobotStateDetector::execute()
    00052 {
    00053   robotState.setFrameNumber(bodyPercept.frameNumber);
    00054
    00055   calculateCollisionState();
    00056
    00057   robotState.setState(
    00058     (RobotState::States)bodyPercept.getState());
    00059
    00060   robotState.setMouthState(
    00061     (RobotState::MouthStates)bodyPercept.getMouthState());
    00062
    00063   robotState.setSomethingInFrontOfChest(bodyPercept.getBodyPSDHighValue());
    00064   robotState.setDistanceToSIFOC(bodyPercept.getBodyPSDDistance());
    00065   if(bodyPercept.getBodyPSDHighValue())
    00066   {
    00067     robotState.setTimeWhenSomethingWasInFrontOfChestLast(SystemCall::getCurrentSystemTime());
    00068   }
    00069
    00070   robotState.acceleration=bodyPercept.acceleration;
    00071
    00072   int i;
    00073   for (i = 0; i < BodyPercept::numOfSwitches; i++)
    00074   {
    00075     if ((bodyPercept.getSwitches() & (1<<i)) > 0)
    00076     {
    00077       if (!switchDown[i])
    00078       {
    00079         // switch pressed
    00080         switchDown[i] = true;
    00081         switchDownTime[i] = SystemCall::getCurrentSystemTime();
    00082
    00083         /***/
    00084         buttonPressed[i]=true;  //Button pressed
    00085         buttonTime[i]=SystemCall::getCurrentSystemTime(); //Aktuelle Zeit
    00086         buttonPressedTime[i]=0; //Later: CurrentTime-buttonTime, buttonPressedTime started
    00087         buttonDuration[i]=0;
    00088
    00089         switch(i)
    00090         {
    00091         case BodyPercept::backMiddle:
    00092         case BodyPercept::backBack:
    00093         case BodyPercept::backFront:
    00094           {
    00095             if (anyBackButtonPressed==false)
    00096             {
    00097               anyBackButtonPressed=true;
    00098               anyBackButtonTime=SystemCall::getCurrentSystemTime();
    00099               anyBackButtonPressedTime=0;
    00100               anyBackButtonDuration=0;
    00101             }
    00102             break;
    00103           }
    00104         }
    00105         setNewButtonStatus();
    00106         /***/
    00107
    00108       }
    00109     }
    00110     else
    00111     {
    00112       if (switchDown[i])
    00113       {
    00114         // switch released
    00115         switchDown[i] = false;
    00116
    00117         /***/
    00118         buttonPressed[i]=false;  //Button released
    00119         buttonPressedTime[i]=SystemCall::getCurrentSystemTime()-buttonTime[i]; //how long the button was pressed
    00120         buttonTime[i]=SystemCall::getCurrentSystemTime(); //when the button was pressed
    00121         buttonDuration[i]=0; // And it starts again
    00122         if (anyBackButtonPressed==true)
    00123         {
    00124           if ((buttonPressed[BodyPercept::backMiddle]==false)&&(buttonPressed[BodyPercept::backFront]==false)&&(buttonPressed[BodyPercept::backBack]==false))
    00125           {
    00126             anyBackButtonPressed=false;
    00127             anyBackButtonPressedTime=SystemCall::getCurrentSystemTime()-anyBackButtonTime;
    00128             anyBackButtonTime=SystemCall::getCurrentSystemTime();
    00129             anyBackButtonDuration=0;
    00130           }
    00131         }
    00132         setNewButtonStatus();
    00133         /***/
    00134       }
    00135     }
    00136   }
    00137   /****/
    00138   for (i = 0; i < BodyPercept::numOfSwitches; i++)  // update my duration time
    00139   {
    00140     buttonDuration[i]=SystemCall::getTimeSince(buttonTime[i]);
    00141     /*
    00142     OUTPUT(idText, text, "ButtonDuration: " << buttonDuration[i]);
    00143     OUTPUT(idText, text, "ButtonTime: " << buttonTime[i]);
    00144     OUTPUT(idText, text, "ButtonPressed: " << buttonPressed[i]);
    00145     OUTPUT(idText, text, "ButtonPressedTime: " << buttonPressedTime[i]);
    00146     */
    00147   }
    00148   anyBackButtonDuration=SystemCall::getTimeSince(anyBackButtonTime);
    00149   /*
    00150   OUTPUT(idText, text, "anyBackButtonDuration: " << anyBackButtonDuration);
    00151   OUTPUT(idText, text, "--------------- ");
    00152   */
    00153   setNewButtonStatus();
    00154   /****/
    00155
    00156   DEBUG_RESPONSE("Processes:Cognition - activate button-pressed-messages",
    00157     if(robotState.getButtonPressed(BodyPercept::head) &&
    00158       robotState.getButtonDuration(BodyPercept::head) < 10
    00159       )
    00160     {
    00161       OUTPUT(idText, text, "head-button-pressed");
    00162     }
    00163
    00164     if(robotState.getAnyBackButtonPressed() &&
    00165       robotState.getAnyBackButtonDuration() < 10
    00166       )
    00167     {
    00168       OUTPUT(idText, text, "back-button-pressed");
    00169     }
    00170
    00171     );
    00172
    00173 }
    00174
    00175 void GT2005RobotStateDetector::setNewButtonStatus()
    00176 {
    00177   robotState.setButtonPressed(buttonPressed);
    00178   robotState.setButtonTime(buttonTime);
    00179   robotState.setButtonPressedTime(buttonPressedTime);
    00180   robotState.setButtonDuration(buttonDuration);
    00181   robotState.setAnyBackButtonPressed(anyBackButtonPressed);
    00182   robotState.setAnyBackButtonTime(anyBackButtonTime);
    00183   robotState.setAnyBackButtonPressedTime(anyBackButtonPressedTime);
    00184   robotState.setAnyBackButtonDuration(anyBackButtonDuration);
    00185 }
    00186
    00187 void GT2005RobotStateDetector::calculateCollisionState()
    00188 {
    00189   // collisionTime  1. Component 0 fl, 1 fr, 2 hl, 3 hr, 4 head, 5 anyone of them
    00190   //                2. Component two previous collision times
    00191
    00192
    00193   long time = SystemCall::getCurrentSystemTime();
    00194
    00195   //setze alle states auf 0!!!
    00196
    00197   robotState.setCollisionFrontLeft(false);
    00198   robotState.setCollisionFrontRight(false);
    00199   robotState.setCollisionHindLeft(false);
    00200   robotState.setCollisionHindRight(false);
    00201   robotState.setCollisionHead(false);
    00202   robotState.setCollisionAggregate(false);
    00203
    00204
    00205   if ((time - timeSinceLastCollision[0]) >  remindCollision)
    00206   {
    00207     consecutiveCollisionTime[0] = time;
    00208     robotState.setConsecutiveCollisionTimeFrontLeft(0);
    00209   }
    00210
    00211   if ((time - timeSinceLastCollision[1]) >  remindCollision)
    00212   {
    00213     consecutiveCollisionTime[1] = time;
    00214     robotState.setConsecutiveCollisionTimeFrontRight(0);
    00215   }
    00216
    00217   if ((time - timeSinceLastCollision[2]) >  remindCollision)
    00218   {
    00219     consecutiveCollisionTime[2] = time;
    00220     robotState.setConsecutiveCollisionTimeHindLeft(0);
    00221   }
    00222
    00223   if ((time - timeSinceLastCollision[3]) >  remindCollision)
    00224   {
    00225     consecutiveCollisionTime[3] = time;
    00226     robotState.setConsecutiveCollisionTimeHindRight(0);
    00227   }
    00228
    00229   if ((time - timeSinceLastCollision[4]) >  remindCollision)
    00230   {
    00231     consecutiveCollisionTime[4] = time;
    00232     robotState.setConsecutiveCollisionTimeHead(0);
    00233   }
    00234
    00235   if ((time - timeSinceLastCollision[5]) >  remindCollision)
    00236   {
    00237     consecutiveCollisionTime[5] = time;
    00238     robotState.setConsecutiveCollisionTimeAggregate(0);
    00239   }
    00240
    00241   if (collisionPercept.getCollisionFrontLeft())
    00242   {
    00243     robotState.setCollisionFrontLeft(true);
    00244     if ((time-timeSinceLastCollision[0]) > remindCollision)
    00245     {
    00246       consecutiveCollisionTime[0] = time; // first collision
    00247     }
    00248     robotState.setConsecutiveCollisionTimeFrontLeft(time-consecutiveCollisionTime[0]);
    00249     timeSinceLastCollision[0] = time; //last collision
    00250   }
    00251
    00252   if (collisionPercept.getCollisionFrontRight())
    00253   {
    00254     robotState.setCollisionFrontRight(true);
    00255     if ((time-timeSinceLastCollision[1]) > remindCollision)
    00256     {
    00257       consecutiveCollisionTime[1] = time;
    00258     }
    00259     robotState.setConsecutiveCollisionTimeFrontRight(time-consecutiveCollisionTime[1]);
    00260     timeSinceLastCollision[1] = time;
    00261   }
    00262
    00263   if (collisionPercept.getCollisionHindLeft())
    00264   {
    00265     robotState.setCollisionHindLeft(true);
    00266     if ((time-timeSinceLastCollision[2]) > remindCollision)
    00267     {
    00268       consecutiveCollisionTime[2] = time;
    00269     }
    00270     robotState.setConsecutiveCollisionTimeHindLeft(time-consecutiveCollisionTime[2]);
    00271     timeSinceLastCollision[2] = time;
    00272   }
    00273
    00274   if (collisionPercept.getCollisionHindRight())
    00275   {
    00276     robotState.setCollisionHindRight(true);
    00277     if ((time-timeSinceLastCollision[3]) > remindCollision)
    00278     {
    00279       consecutiveCollisionTime[3] = time;
    00280     }
    00281     robotState.setConsecutiveCollisionTimeHindRight(time-consecutiveCollisionTime[3]);
    00282     timeSinceLastCollision[3] = time;
    00283   }
    00284
    00285   if (collisionPercept.getCollisionHead())
    00286   {
    00287     robotState.setCollisionHead(true);
    00288     if ((time-timeSinceLastCollision[4]) > remindCollision)
    00289     {
    00290       consecutiveCollisionTime[4] = time;
    00291     }
    00292     robotState.setConsecutiveCollisionTimeHead(time-consecutiveCollisionTime[4]);
    00293     timeSinceLastCollision[4] = time;
    00294   }
    00295
    00296   if (collisionPercept.getCollisionAggregate())
    00297   {
    00298     robotState.setCollisionAggregate(true);
    00299     if ((time-timeSinceLastCollision[5]) > remindCollision)
    00300     {
    00301       consecutiveCollisionTime[5] = time;
    00302     }
    00303     robotState.setConsecutiveCollisionTimeAggregate(time-consecutiveCollisionTime[5]);
    00304     timeSinceLastCollision[5] = time;
    00305   }
    00306
    00307   robotState.setOdometryDisturbance(collisionPercept.getOdometryDisturbance());
    00308
    00309   if (collisionPercept.getCollisionFrontLeft() || collisionPercept.getCollisionHindLeft())
    00310   {
    00311     setCollisionOnLeftSide(true);
    00312   }
    00313   else
    00314   {
    00315     setCollisionOnLeftSide(false);
    00316   }
    00317
    00318   if (collisionPercept.getCollisionFrontRight() || collisionPercept.getCollisionHindRight())
    00319   {
    00320     setCollisionOnRightSide(true);
    00321   }
    00322   else
    00323   {
    00324     setCollisionOnRightSide(false);
    00325   }
    00326
    00327   robotState.setCollisionSide(getCollisionSide());
    00328 }
    00329
    00330   /** Did collision occur at front left leg */
    00331 int GT2005RobotStateDetector::getCollisionSide()
    00332 {
    00333     int l = 0;
    00334     int r = 0;
    00335
    00336     for (int i = 0; i < maxFrameHistory; i++)
    00337     {
    00338       if (numOfCollisions[i][left])  {++l;}
    00339       if (numOfCollisions[i][right]) {++r;}
    00340     }
    00341
    00342     if (l > r) {return left;}
    00343     else if (l < r) {return right;}
    00344     else return middle;
    00345 }
    00346
    00347 void GT2005RobotStateDetector::setCollisionOnLeftSide(bool collision)
    00348 {
    00349   numOfCollisions[collisionPercept.frameNumber%maxFrameHistory][GT2005RobotStateDetector::left] = collision;
    00350 }
    00351
    00352 void GT2005RobotStateDetector::setCollisionOnRightSide(bool collision)
    00353 {
    00354   numOfCollisions[collisionPercept.frameNumber%maxFrameHistory][GT2005RobotStateDetector::right] = collision;
    00355 }
    00356