ArActionStallRecover.cpp

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481

*/

#include "ArExport.h"
#include "ariaOSDef.h"
#include <time.h>
#include "ArRobot.h"
#include "ArResolver.h"
#include "ArActionStallRecover.h"

AREXPORT
ArActionStallRecover::ArActionStallRecover(const char * name,
                                           double obstacleDistance, 
                                           int cyclesToMove, 
                                           double speed, 
                                           double degreesToTurn) :
    ArAction(name, "Recovers the robot from a stall.")
{
  setNextArgument(ArArg("obstacle distance", &myObstacleDistance, 
                "Distance at which not to move because of obstacle. (mm)"));
  myObstacleDistance = obstacleDistance;
  setNextArgument(ArArg("cycles to move", &myCyclesToMove, 
                        "Number of cycles to move (# of cycles)"));
  myCyclesToMove = cyclesToMove;
  setNextArgument(ArArg("speed", &mySpeed, 
                        "Speed at which to back up or go forward (mm/sec)"));
  mySpeed = speed;
  setNextArgument(ArArg("degrees to turn", &myDegreesToTurn, 
                        "Number of Degrees to turn (deg)"));
  myDegreesToTurn = degreesToTurn;
  myState = STATE_NOTHING;

  myResolver = NULL;
  mySequenceNum = 0;
  addSequence(BACK | TURN);
  addSequence(FORWARD | TURN);
  addSequence(BACK);
  addSequence(FORWARD);
  addSequence(TURN_LEFT);
  addSequence(TURN_RIGHT);
  addSequence(BACK | TURN_LEFT);
  addSequence(BACK | TURN_RIGHT);
  addSequence(FORWARD | TURN_LEFT);
  addSequence(FORWARD | TURN_RIGHT);
  mySequencePos = 0;
  myLastFired = 0;
}

AREXPORT
ArActionStallRecover::~ArActionStallRecover()
{


}

void ArActionStallRecover::addSequence(int sequence)
{
  mySequence[mySequenceNum] = sequence;
  ++mySequenceNum;
}

AREXPORT
ArActionDesired *ArActionStallRecover::fire(ArActionDesired currentDesired)
{
  if (currentDesired.getVelStrength() >= 1.0 || 
      currentDesired.getDeltaHeadingStrength() >= 1.0)
  {
    myState = STATE_NOTHING;
    myCount = -1;
    return NULL;
  }

  myActionDesired.reset();
  switch (myState)
  {
  case STATE_NOTHING:
    if (!myRobot->isLeftMotorStalled() && !myRobot->isRightMotorStalled())
      return NULL;
    if (time(NULL) - myLastFired <= 5)
    {
      //printf("Didn't reset... %ld time diff\n", time(NULL) - myLastFired);
      ++mySequencePos;
    }
    else 
    {
      //printf("Reset\n");
      mySequencePos = 0;
    }
    if (mySequencePos >= mySequenceNum)
      mySequencePos = 0;
    myDoing = mySequence[mySequencePos];
    /* 
    if (myDoing & FORWARD)
      printf("Forward ");
    if (myDoing & BACK)
      printf("Back ");
    if (myDoing & TURN)
      printf("turn ");
    if (myDoing & TURN_LEFT)
      printf("turnLeft ");
    if (myDoing & TURN_RIGHT)
      printf("turnRight ");
    if (myDoing == 0)
      printf("Not doing anything, messed up");
    printf("\n");
    */
    myActionDesired.setVel(0);
    myActionDesired.setDeltaHeading(0);
  case STATE_GOING:
    myLastFired = time(NULL);
    doit();
    break;
  default:
    break;
  }
  return &myActionDesired;
}

void ArActionStallRecover::doit(void)
{
  double leftDist, rightDist;
  double dist;
  int turnDirection;
  bool transFired, rotFired;

  if (myCount == -1 || myState != STATE_GOING)
  {
    myCount = myCyclesToMove;
    myState = STATE_GOING;
    mySideStalled = 0;
    if (myRobot->isLeftMotorStalled())
      mySideStalled += 1;
    if (myRobot->isRightMotorStalled())
      mySideStalled += 2;
    if (myDoing & TURN) 
    {
      rightDist = myRobot->checkRangeDevicesCurrentPolar(-120, -60);
      leftDist = myRobot->checkRangeDevicesCurrentPolar(60, 120);
      if (mySideStalled == 1 || rightDist < 0)
        turnDirection = -1;
      else if (mySideStalled == 2 || leftDist < 0)
        turnDirection = 1;
      else if (rightDist < leftDist)
        turnDirection = 1;
      else
        turnDirection = -1;
      myDesiredHeading = myRobot->getTh() + turnDirection * myDegreesToTurn;
    }
    else if (myDoing & TURN_LEFT)
      myDesiredHeading = myRobot->getTh() + myDegreesToTurn;
    else if (myDoing & TURN_RIGHT)
      myDesiredHeading = myRobot->getTh() + -1 * myDegreesToTurn;
  }
  else
    --myCount;

  if ((myDoing & MOVEMASK))
  {
    if (myDoing & FORWARD)
      dist = (myRobot->checkRangeDevicesCurrentPolar(-120, 120) -
              myRobot->getRobotRadius());
    else if (myDoing & BACK)
      dist = (myRobot->checkRangeDevicesCurrentPolar(120, -120) -
              myRobot->getRobotRadius());
    if ((myCount <= 0 || 
         (dist  > 0 && dist < myObstacleDistance &&
          myCount <= myCyclesToMove / 2)))
    {
      //printf("Not backing ");
      myActionDesired.setVel(0);
      transFired = false;
    } 
    else
    {
      //printf("backing ");
      if (myDoing & BACK)
        myActionDesired.setVel(mySpeed * -1);
      if (myDoing & FORWARD)
        myActionDesired.setVel(mySpeed);
      transFired = true;
    }
  } 
  else
    transFired = false;

  if ((myDoing & TURNMASK) && 
      (myCount <= 0 || myDegreesToTurn == 0 || 
       ArMath::fabs(ArMath::subAngle(myRobot->getTh(),myDesiredHeading)) < 3))
  {
    //printf("Not turning \n");
    myActionDesired.setDeltaHeading(0);
    rotFired = false;
  }
  else if (myDoing & TURNMASK)
  {
    //printf("turning \n");
    myActionDesired.setHeading(myDesiredHeading);
    rotFired = true;
  } 
  else
    rotFired = false;

  if (myCount <= 0 || (!transFired && !rotFired))
  {
    myState = STATE_NOTHING;
    return;
  }
}

void ArActionStallRecover::addToConfig(ArConfig* config, const char* sectionName, int priority)
{
  if (config == NULL || sectionName == NULL)
  {
    ArLog::log(ArLog::Terse, "Could not add ArActionStallRecoverToConfig because config (%p) or section (%p) are null", config, sectionName);
    return;
  }
  config->addParam(ArConfigArg("StallRecoverSpeed", &mySpeed, "Speed at which to back away when stalled.", priority), sectionName, ArPriority::DETAILED);
  config->addParam(ArConfigArg("StallRecoverDuration", &myCyclesToMove, "Cycles of operation to move when recovering from stall.", priority), sectionName, ArPriority::DETAILED);
  config->addParam(ArConfigArg("StallRecoverRotation", &myDegreesToTurn, "Amount of rotation when recovering (degrees).", priority), sectionName, ArPriority::DETAILED);
}

---