Modules/ImageProcessor/RasterImageProcessor/RFieldStateMachine.cpp

Go to the documentation of this file.

/** 
* @file RFieldStateMachine.cpp
* Implementation of class RFieldStateMachine.
*
* @author <A href=mailto:sadprofessor@web.de>Bernd Schmidt</A>
*/

#include "RFieldStateMachine.h"

RFieldStateMachine::RFieldStateMachine()
  :state(START)
{ 
  reset(0,0);
}
  
  RFieldStateMachine::~RFieldStateMachine(){}

  void RFieldStateMachine::reset(int x,int y){
  start.x = x;
  start.y = y;
  numberOfEdges = 0;
  obstacleFarPoint = -1;
  obstacleNearPoint = -1;
  state = START;
  isUnknownBot = false;
  }

void RFieldStateMachine::update(int x,int y,REdgeDetection& scanner){
  //follow the scan and update the states
  if (numberOfEdges == 20) return;
  rangeSum = 0;
  greenRange = 0;
  whiteRange = 0;
  grayRange = 0;

  edgeBuffer[numberOfEdges].x = x;
  edgeBuffer[numberOfEdges].y = y;

  types[numberOfEdges] = nothing;
  for (int i = 0; i<scanner.getBufferSize();i++){
    color = scanner.getColor(i);
    range = scanner.getRange(i);
    rangeSum = rangeSum + range;
    if (color == green) greenRange = greenRange + range;
    else if (color == white) whiteRange = whiteRange + range;
    else if (color == gray) grayRange = grayRange + range;

    if (greenRange > 2){ 
      if (obstacleNearPoint == 0) obstacleFarPoint = 0; 
      obstacleNearPoint = numberOfEdges;
    }

    findNextState();
    executeState();
  }
  numberOfEdges++;
} 

// TODO: implement state-array for state machine 
//(f.e. states[state]->findNextState())
void RFieldStateMachine::findNextState(){
  switch(state){
    case START:
      switch(color){
      case white: 
      if (whiteRange > 3) state = FOUND_WHITE;
        break;
      case green: 
        state = FOUND_FIELD;
        break;
      case red: 
        state = FOUND_RED_ROBOT;
        break;
      case blue: 
        state = FOUND_BLUE_ROBOT;
        break;
      case skyblue: 
        state = SKYBLUE_GOAL;
        break;
      case yellow: 
        state = YELLOW_GOAL;
        break;
      }
      break;
    case FOUND_WHITE: 
      switch(color){
      case green: 
        state = FOUND_BORDER;
        break;
      case red: 
        state = FOUND_RED_ROBOT;
        break;
      case blue: 
        state = FOUND_BLUE_ROBOT;
        break;
      case orange:
        state = FOUND_BALL;
        break;
      case gray:
        if (grayRange > 3) state = FOUND_UNKNOWN;
      }
      break;
    case FOUND_BORDER: 
      break;
    case FOUND_FIELD:
      switch(color){
      case orange: 
        state = FOUND_BALL;
        break;
      case red: 
        state = FOUND_RED_ROBOT;
        break;
      case blue: 
        state = FOUND_BLUE_ROBOT;
        break;
      case white:
        state = LINE_IN;
      }
      break;
    case LINE_IN: 
      switch(color){
      case green: 
        state = LINE_OUT;
        break;
      case red: 
        state = FOUND_RED_ROBOT;
        break;
      case blue: 
        state = FOUND_BLUE_ROBOT;
        break;
      case orange:
        state = FOUND_BALL;
        break;
      case gray:
        if (grayRange > 3) state = FOUND_UNKNOWN;
      }
      break;
    case LINE_OUT: break;
    case YELLOW_GOAL:
      switch(color){
      case green: 
        state = YELLOW_FINISHED;
        break;
      case red: 
        state = FOUND_RED_ROBOT;
        break;
      case blue: 
        state = FOUND_BLUE_ROBOT;
        break;
      case orange:
        state = FOUND_BALL;
        break;
      case white:
        state = FOUND_UNKNOWN;
        break;
      }
      break;
    case YELLOW_FINISHED: break;
    case SKYBLUE_GOAL:
      switch(color){
      case green: 
        state = SKYBLUE_FINISHED;
        break;
      case red: 
        state = FOUND_RED_ROBOT;
        break;
      case blue: 
        state = FOUND_BLUE_ROBOT;
        break;
      case orange:
        state = FOUND_BALL;
        break;
      case white:
        state = FOUND_UNKNOWN;
        break;
      }
      break;
    case SKYBLUE_FINISHED: break;
    case FOUND_RED_ROBOT: 
      switch(color){
      case green: 
        state = FINISHED_RED_ROBOT;
        break;
      case blue:
        state = FOUND_UNKNOWN;
        isUnknownBot = true;
        break;
      case orange:
        state = FOUND_BALL;
        break;
      }
      break;
    case FOUND_BLUE_ROBOT: 
      switch(color){
      case green: 
        state = FINISHED_BLUE_ROBOT;
        break;
      case red:
        state = FOUND_UNKNOWN;
        isUnknownBot = true;
        break;
      }
      break;
    case FINISHED_BLUE_ROBOT: break;
    case FINISHED_RED_ROBOT: break;
    case FOUND_BALL:
      if (color == green) state = FINISHED_BALL;
      break;
    case FINISHED_BALL: break;
    case FOUND_UNKNOWN: 
      if (isUnknownBot){
        if (color == green)
          state = FINISHED_UNKNOWN;
      }
      else{
        switch(color){
        case blue: 
          state = FOUND_BLUE_ROBOT;
          break;
        case red:
          state = FOUND_RED_ROBOT;
          break;
        case orange: 
          state = FOUND_BALL;
          break;
        case skyblue: 
          state = SKYBLUE_GOAL;
          break;
        case yellow: 
          state = YELLOW_GOAL;
          break;
        }
      }
      break;
    case FINISHED_UNKNOWN: 
      break;
    default: break; //machine is in undefined state
  }
}

// A little bit like XABSL-CODE but it seems to be efficient
void RFieldStateMachine::executeState(){
  switch(state){
    case START: break;
    case FOUND_WHITE: break;
    case FOUND_FIELD: break;
    case FOUND_BORDER: 
      state = FOUND_FIELD;
      if(greenRange<3){
        types[numberOfEdges] = border;
        obstacleFarPoint = numberOfEdges;
      }
      else if (numberOfEdges>0){
        types[numberOfEdges-1] = border;
        obstacleFarPoint = numberOfEdges-1;
      }
      break;
    case LINE_IN: break;
    case LINE_OUT: 
      state = FOUND_FIELD;
      if (numberOfEdges<1) break;
      if (greenRange<3){
        types[numberOfEdges] = line;
        //types[numberOfEdges-1] = line;
      }
      else{
        types[numberOfEdges-1] = line;
        //types[numberOfEdges-2] = line;
      }
      break;
    case YELLOW_GOAL: break;
    case YELLOW_FINISHED: 
      state = FOUND_FIELD;
      if(greenRange<3){
        types[numberOfEdges] = yellowGoal;
        obstacleFarPoint = numberOfEdges;
      }
      else if (numberOfEdges>0){ 
        types[numberOfEdges-1] = yellowGoal;
        obstacleFarPoint = numberOfEdges-1;
      }
      break;
    case SKYBLUE_GOAL: break;
    case SKYBLUE_FINISHED: 
      state = FOUND_FIELD;
      if(greenRange<3){
        types[numberOfEdges] = skyblueGoal;
        obstacleFarPoint = numberOfEdges;
      }
      else if (numberOfEdges>0){
        types[numberOfEdges-1] = skyblueGoal;
        obstacleFarPoint = numberOfEdges-1;
      }
      break;
    case FOUND_RED_ROBOT: break;
    case FOUND_BLUE_ROBOT: break;
    case FINISHED_BLUE_ROBOT:
      if(greenRange<3){
        types[numberOfEdges] = blueRobot;
        obstacleFarPoint = numberOfEdges;
      }
      else if (numberOfEdges>0){
        types[numberOfEdges-1] = blueRobot;
        obstacleFarPoint = numberOfEdges-1;
      }
      state = FOUND_FIELD;
      break;
    case FINISHED_RED_ROBOT: 
      state = FOUND_FIELD;
      if(greenRange<3){
        types[numberOfEdges] = redRobot;
        obstacleFarPoint = numberOfEdges;
      }
      else if (numberOfEdges>0){
        types[numberOfEdges-1] = redRobot;
        obstacleFarPoint = numberOfEdges-1;
      }
      break;
    case FOUND_BALL: break;
    case FINISHED_BALL: 
      state = FOUND_FIELD;
      //TODO(schmidtb): Integrate BallSpecialist into Field-Scan
      break;
    case FOUND_UNKNOWN: break;
    case FINISHED_UNKNOWN: 
      state = FOUND_FIELD;
      if(greenRange<3){
        types[numberOfEdges] = unknown;
        obstacleFarPoint = numberOfEdges;
      }
      else if (numberOfEdges>0){
        types[numberOfEdges-1] = unknown;
        obstacleFarPoint = numberOfEdges-1;
      }
      break;
    default: break; //machine is in undefined state
  }
} 
/*
* Change log :
* 
* $Log: RFieldStateMachine.cpp,v $
* Revision 1.6  2004/09/09 11:08:04  spranger
* renamed GT2004EdgeDetection to REdgeDetection for consistency
*
* Revision 1.5  2004/09/09 10:15:55  spranger
* fixed doxygen-errors
*
* Revision 1.4  2004/09/06 12:02:26  schmidtb
* commented almost all members, removed warnings in documentation

* did further code clean up
*
* Revision 1.3  2004/09/02 07:59:29  schmidtb
* Added RasterImageProcessor to repository, because we used it for the OpenChallenge.
*
* Revision 1.3  2004/05/25 13:27:34  schmidtb
* modified version of rip for open-challenge
*
* Revision 1.3  2004/04/15 19:08:34  pg_besc
* merged code
*
* Revision 1.2  2004/03/25 15:29:04  pg_besc
* made some changes
*
* Revision 1.1  2004/03/17 22:12:39  schmidtb
* established RFieldStateMachine
*
*
*/

---