Modules/SensorBehaviorControl/ObstacleAvoiderOnGreenFieldERS7.cpp

Go to the documentation of this file.

/**
* @file ObstacleAvoiderOnGreenFieldERS7.cpp
* This file contains a class that demonstrates walking.
* 
* @author <A href=mailto:jhoffman@informatik.hu-berlin.de>Jan Hoffmann</A>
*/

#include "ObstacleAvoiderOnGreenFieldERS7.h"
#include "Tools/Debugging/GenericDebugData.h"



ObstacleAvoiderOnGreenFieldERS7::ObstacleAvoiderOnGreenFieldERS7(const SensorBehaviorControlInterfaces& interfaces)
: SensorBehaviorControl(interfaces), 
maxSpeedX(140),
distanceToGround(300),
distanceOffset(50),
distanceControlInterval(50),
speedX(0, 0.6, 0.0, 0, -maxSpeedX/2, maxSpeedX, 100),
speedY(0, 1, 0, 0, -50, 50, 50),
speedPhi(0, .1, 0, 0, -pi_4, pi_4, pi/10),
gridSize(4),
timeOfLastExecute(0),
localColTableInitialized(false)
{
}

void ObstacleAvoiderOnGreenFieldERS7::init() {}

void ObstacleAvoiderOnGreenFieldERS7::execute()
{
  /* The module was not active for a while */
  /*
  unsigned long currentTime = SystemCall::getCurrentSystemTime();
  if((currentTime - timeOfLastExecute) > 2000) init();
  timeOfLastExecute = currentTime;
  */

  /* [JH] use the "proper" way to access the color table */
  Image cameraImage = image;
  if(!localColTableInitialized)
  {
    cameraImage.setColorTable(&colorTable);
  }
  else 
  {
    cameraImage.setColorTable(&localColTable);    
  }
  
  
  double sum,
    greenRight = 0, 
    greenLeft = 0, 
    targetY = 0, 
    targetPhi, 
    targetX,
    psd = (sensorDataBuffer.lastFrame().data[SensorData::headPsdFar] / 1000) - 100.0; 
  
  int x, y, midX = image.cameraInfo.resolutionWidth / 2;//104

  distanceToGround = 350;

//  if (psd > distanceToGround * 1.4) // don't jump off cliff!
//    targetX = 0.0; // don't use negative nuber because robot might "reverse" off table
//  else

  
    targetX = psd - 200; // - (distanceToGround - distanceOffset - distanceControlInterval);
  //shouldn't this be = psp - distanceOffset - (distanceToGround) ?


    // now it's really hitting something!
    if (sensorDataBuffer.lastFrame().data[SensorData::bodyPsd] == 100000)
    {
      targetX = -100;
    }
//  targetX *= maxSpeedX/distanceControlInterval;
  
  //OUTPUT(idText, text, "target: " << targetX << ", psd: " << psd << ", speedX: " << speedX.getVal() << ", P speed: " << speedX.getWeightP());
  
  
  /** calc how much green is in both halves */
  for(x = gridSize/2; x < midX; x += gridSize)
  {
    for(y = gridSize/2; y < image.cameraInfo.resolutionHeight; y += gridSize)
    {
      if (cameraImage.getClassifiedColor(x, y) == green)
      {
        greenLeft++;
        //cameraImage.image[y][2][x] = 255;
      }
      else
      {
        //cameraImage.image[y][0][x] = 0;
      }

      if (cameraImage.getClassifiedColor(x + midX, y) == green)
      {
        greenRight++;
        //cameraImage.image[y][1][x + midX] = 255;
      }
      else 
      {
        //cameraImage.image[y][0][x + midX] = 255;
      }
    }
  }

  //INIT_DEBUG_IMAGE(classificationY, cameraImage);
  //SEND_DEBUG_IMAGE(classificationY);
  //OUTPUT(idText, text, " " << greenLeft << " : " << greenRight); 

  sum = greenLeft + greenRight;
  if (sum == 0) sum = 1;
  
  /** if robot is moving forward, turn where there's more
  *   green
  */
  if (speedX.getVal() > .7 * maxSpeedX)
  {
    targetPhi = 1.2 * (greenLeft - greenRight) / sum;
    //targetPhi *= .5;
  }
  /** if robot is too close to something, stick to the
  * direction it last turned to until obstacle is no
  * longer in front of robot
  */
  else 
  {
    if (speedPhi.getVal() < 0)
      targetPhi = -pi;
    else
      targetPhi = pi;
  }
  
  if (greenLeft > greenRight) 
  {
    ledRequest.redBottomLEDs = LEDRequest::leftOnly;
  }
  else if (greenLeft < greenRight)
  {
    ledRequest.redBottomLEDs = LEDRequest::rightOnly;
  }
  else if (greenLeft == greenRight)
  {
    if (greenLeft == 0)
      ledRequest.redBottomLEDs = LEDRequest::bothOff;
    else
      ledRequest.redBottomLEDs = LEDRequest::bothOn;
  }
  else 
    ledRequest.redBottomLEDs = LEDRequest::bothOff;
      
  ledRequest.redTopLEDs = LEDRequest::bothOff;
  
  if(sensorDataBuffer.lastFrame().data[SensorData::tailPan] > 0)
  {
    speedPhi.approximateVal(targetPhi);
    speedX.approximateVal(targetX);
    timeToCalibrate = 0;
    soundRequest.soundID = SoundRequest::okay;
    headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
  }
  else //stop
  {
    headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
    if (SystemCall::getTimeSince(timeToCalibrate) < 2000)
      soundRequest.soundID = SoundRequest::notokay;
    else 
      soundRequest.soundID = SoundRequest::none;
    speedPhi.approximateVal(0);
    speedX.approximateVal(0);
    if (timeToCalibrate > 0)
    {
      ledRequest.redTopLEDs = LEDRequest::bothOn;
      if (SystemCall::getTimeSince(timeToCalibrate) > 2000) // wait for one sec to allow motion to subside
      {
/*        if(robotState.getSwitches() == RobotState::headDown)
        {
          soundRequest.soundID = SoundRequest::rob005;
          calibrate();
        }
*/
      }
    }
    else
    {
      timeToCalibrate = SystemCall::getCurrentSystemTime();
      ledRequest.redTopLEDs = LEDRequest::bothOff;
    }
  }
     
  motionRequest.tailRequest.tailRequestID = TailRequest::twoPositionSwitchHorizontal;
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;

  /* set the motion request */
  motionRequest.motionType=MotionRequest::walk;
  if (fabs(speedX.getVal()) > 1)
    motionRequest.walkRequest.walkParams.translation.x = speedX.getVal();
  else  
    motionRequest.walkRequest.walkParams.translation.x = 0.0;
  if (fabs(speedY.getVal()) > 1)
    motionRequest.walkRequest.walkParams.translation.y = targetY;
  else 
    motionRequest.walkRequest.walkParams.translation.y = 0.0;
  if (fabs(speedPhi.getVal()) > .05)
    motionRequest.walkRequest.walkParams.rotation = speedPhi.getVal();
  else 
    motionRequest.walkRequest.walkParams.rotation = 0.0;
}



void ObstacleAvoiderOnGreenFieldERS7::calibrate()
{
  //distanceToGround = (sensorDataBuffer.lastFrame().data[SensorData::psd] / 1000) - 100;

  localColTable.clear();

  int x, y;

  for(x = gridSize/2; x < image.cameraInfo.resolutionWidth; x += gridSize)
  {
    for(y = gridSize/2; y < image.cameraInfo.resolutionHeight; y += gridSize)
    {
      localColTable.addColorClass(green, image.image[y][0][x], image.image[y][1][x], image.image[y][2][x], 2);
    }
  }

  localColTableInitialized = true;
}



bool ObstacleAvoiderOnGreenFieldERS7::handleMessage(InMessage& message)
{
  bool handled = false;
  
  switch(message.getMessageID())
  {
  case idGenericDebugData:
    GenericDebugData d;
    message.bin >> d;
    if(d.id == GenericDebugData::braitenbergPIDs)
    {
      OUTPUT(idText,text,"generic debug message (Braitenberg) handled by module ObstacleAvoiderOnGreenFieldERS7");
      
      speedX.setWeightP(d.data[0]);
      speedX.setWeightI(d.data[1]);
      speedX.setWeightD(d.data[2]);
      
      speedPhi.setWeightP(d.data[3]);
      speedPhi.setWeightI(d.data[4]);
      speedPhi.setWeightD(d.data[5]);
      
      distanceOffset = d.data[6];
      distanceControlInterval = d.data[7];
      distanceToGround = d.data[8];
    }
    handled = true;
    break;

  }
  return handled;
}


/*
* Change log :
* 
* $Log: ObstacleAvoiderOnGreenFieldERS7.cpp,v $
* Revision 1.4  2004/06/02 17:18:22  spranger
* MotionRequest cleanup
*
* Revision 1.3  2004/05/24 14:15:49  juengel
* New button evaluation.
*
* Revision 1.2  2004/05/23 12:08:26  loetzsch
* clean up in class HeadControlMode
*
* Revision 1.1.1.1  2004/05/22 17:20:53  cvsadm
* created new repository GT2004_WM
*
* Revision 1.3  2004/03/17 10:22:55  jhoffman
* added ERS7 support, removed some image size related bugs
*
* Revision 1.2  2004/03/08 02:11:50  roefer
* Interfaces should be const
*
* Revision 1.1  2004/02/27 14:16:20  jhoffman
* - screen layout "save" works properly
* - delete now has "confirm" dialog
*
*/

---