ArActionBumpers.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 "ArActionBumpers.h"
#include "ArRobot.h"

ArActionBumpers::ArActionBumpers(const char *name, 
                                          double backOffSpeed,
                                          int backOffTime, int turnTime,
                                          bool setMaximums) :
  ArAction(name, "Reacts to the bumpers triggering")
{
  setNextArgument(ArArg("back off speed", &myBackOffSpeed, 
                        "Speed at which to back away (mm/sec)"));
  myBackOffSpeed = backOffSpeed;

  setNextArgument(ArArg("back off time", &myBackOffTime,
                        "Number of msec to back up for (msec)"));
  myBackOffTime = backOffTime;

  myStopTime = 1000;

  setNextArgument(ArArg("turn time", &myTurnTime,
                        "Number of msec to allow for turn (msec)"));
  myTurnTime = turnTime;

  setNextArgument(ArArg("set maximums", &mySetMaximums,
                        "Whether to set maximum vels or not (bool)"));
  mySetMaximums = setMaximums;
  
  myFiring = false;
  mySpeed = 0.0;
  myHeading = 0.0;

  myBumpMask = (ArUtil::BIT1 | ArUtil::BIT2 | ArUtil::BIT3 | ArUtil::BIT4 | 
                ArUtil::BIT5 | ArUtil::BIT6 | ArUtil::BIT7 | ArUtil::BIT8); 
}

ArActionBumpers::~ArActionBumpers()
{

}

double ArActionBumpers::findDegreesToTurn(int bumpValue, int whichBumper)
{
  double totalTurn = 0;
  int numTurn = 0;
  int numBumpers;

  double turnRange = 135;

  if(whichBumper == 1) numBumpers = myRobot->getNumFrontBumpers();
  else numBumpers = myRobot->getNumRearBumpers();

  for (int i = 0; i < numBumpers; i++)
    {
      if((i == 0 && (bumpValue & ArUtil::BIT1)) || 
         (i == 1 && (bumpValue & ArUtil::BIT2)) ||
         (i == 2 && (bumpValue & ArUtil::BIT3)) || 
         (i == 3 && (bumpValue & ArUtil::BIT4)) ||
         (i == 4 && (bumpValue & ArUtil::BIT5)) || 
         (i == 5 && (bumpValue & ArUtil::BIT6)) ||
         (i == 6 && (bumpValue & ArUtil::BIT7)) || 
         (i == 7 && (bumpValue & ArUtil::BIT8)))
        {
          totalTurn = totalTurn +  (i * (turnRange / (double)numBumpers) + 
                                    ((turnRange / (double)numBumpers) / 2) - (turnRange / 2));
          ++numTurn;
        }
    } 
  
  totalTurn = totalTurn / (double)numTurn;

  if(totalTurn < 0) totalTurn = ((turnRange / 2) + totalTurn) * -1;
  else totalTurn = ((turnRange / 2) - totalTurn);

  return totalTurn;
}

ArActionDesired *ArActionBumpers::fire(ArActionDesired currentDesired)
{
  int frontBump;
  int rearBump;
  int whichBumper;

  if (myRobot->hasFrontBumpers())
    frontBump = ((myRobot->getStallValue() & 0xff00) >> 8) & myBumpMask;
  else
    frontBump = 0;
  if (myRobot->hasRearBumpers())
    rearBump = (myRobot->getStallValue() & 0xff) & myBumpMask;
  else
    rearBump = 0;

  myDesired.reset();
  if (myFiring)
  {

    if (myStartBack.mSecSince() < myBackOffTime)
    {
      if ((mySpeed < 0 && rearBump != 0) ||
          (mySpeed > 0 && frontBump != 0))
      {
        //printf("0 norot\n");
        myDesired.setVel(0);
      }
      else
      {
        //printf("%.0f norot\n", mySpeed);
        myDesired.setVel(mySpeed);
      }

      myDesired.setDeltaHeading(0);
      return &myDesired;
    }
    else if (myStartBack.mSecSince() < myBackOffTime + myTurnTime &&
             ArMath::fabs(ArMath::subAngle(myRobot->getTh(), myHeading)) > 3)
    {
      //printf("0 %.0f\n", myHeading);
      myDesired.setVel(0);
      myDesired.setHeading(myHeading);
      return &myDesired;
    }
    else if(stoppedSince.mSecSince() < myStopTime)
      {
        myDesired.setVel(0);
        myDesired.setDeltaHeading(0);
        return &myDesired;
      }
    
    myFiring = false;
  }

  if (myRobot->getVel() > 1)
  {
    if (frontBump == 0)
      return NULL;
    whichBumper = 1;
    printf("Bumped a forward bumper while going forward, turning %.0f \n",
           findDegreesToTurn(frontBump, whichBumper));
    myHeading = ArMath::addAngle(myRobot->getTh(), 
                                 findDegreesToTurn(frontBump, whichBumper));
    mySpeed = -myBackOffSpeed;
    myStartBack.setToNow();
  }
  else if (myRobot->getVel() < -1)
  {
    if (rearBump == 0)
      return NULL;
    whichBumper = 2;
    printf("Bumped a rear bumper while going backwards, turning %.0f \n",
           findDegreesToTurn(rearBump, whichBumper));
    myHeading = ArMath::subAngle(myRobot->getTh(), 
                                 findDegreesToTurn(rearBump, whichBumper));
    mySpeed = myBackOffSpeed;
    myStartBack.setToNow();
  } 
  else if(myRobot->getVel() > -1 && myRobot->getVel() < 1)
    {
      if(frontBump == 0 && rearBump == 0) return NULL;
      
      stoppedSince.setToNow();
    }
  else
    return NULL;

  //myStartBack.setToNow();
  myFiring = true;
  myDesired.setVel(mySpeed);
  myDesired.setHeading(myHeading);
  // Set the maximums if that feature is desired
  if (mySetMaximums)
  {
    if (mySpeed > 0)
      myDesired.setMaxVel(mySpeed);
    else
      myDesired.setMaxNegVel(mySpeed);
  }
  return &myDesired;
}  

---