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

ArActionTriangleDriveTo::ArActionTriangleDriveTo(
        const char *name, double finalDistFromVertex,  
        double approachDistFromVertex, double speed,
        double closeDist, double acquireTurnSpeed) :
  ArAction(name, "Finds a triangle and drives in front of it")
{
  setNextArgument(ArArg("final dist from vertex", &myFinalDistFromVertex, 
                        "Distance from vertex we want to be at (mm)"));
  myFinalDistFromVertex = finalDistFromVertex;

  setNextArgument(ArArg("approach dist from vertex", &myApproachDistFromVertex,
                        "Distance from vertex we'll go to before going to final (0 goes straight to final) (mm)"));
  myApproachDistFromVertex = approachDistFromVertex;

  setNextArgument(ArArg("speed", &mySpeed, "speed to drive at (mm/sec)"));
  mySpeed = speed;

  setNextArgument(ArArg("close dist", &myCloseDist, 
                        "how close we have to get to our final point (mm)"));
  myCloseDist = closeDist;

  setNextArgument(ArArg("acquire turn speed", &myAcquireTurnSpeed,
                "if we are aqcquiring the rot vel to turn at (deg/sec)"));
  myAcquireTurnSpeed = acquireTurnSpeed;

  myLaser = NULL;
  myLineFinder = NULL;
  myOwnLineFinder = false;
  myState = STATE_INACTIVE;
  mySaveData = false;
  myData = NULL;
  myVertexSeen = false;
  // set all our defaults
  setTriangleParams();
  setVertexUnseenStopMSecs();
  setAcquire();
  //setIgnoreTriangleDist();
  myPrinting = false;
  myAdjustVertex = false;
  myGotoVertex = false;
}

void ArActionTriangleDriveTo::setParameters(
        double finalDistFromVertex,  
        double approachDistFromVertex, double speed,
        double closeDist, double acquireTurnSpeed) 
{
  myFinalDistFromVertex = finalDistFromVertex;
  myApproachDistFromVertex = approachDistFromVertex;
  mySpeed = speed;
  myCloseDist = closeDist;
  myAcquireTurnSpeed = acquireTurnSpeed;
}

ArActionTriangleDriveTo::~ArActionTriangleDriveTo()
{
  if (myOwnLineFinder)
    delete myLineFinder;
}

void ArActionTriangleDriveTo::activate(void)
{
  if (myPrinting)
    ArLog::log(ArLog::Normal, "ArActionTriangleDriveTo: Activating");
  ArAction::activate();
  myVertexSeen = false;
  myVertexSeenLast.setToNow();
  if (myLineFinder != NULL)
    myState = STATE_ACQUIRE;
  else
    myState = STATE_FAILED;
}

void ArActionTriangleDriveTo::deactivate(void)
{
  ArAction::deactivate();
  myState = STATE_INACTIVE;
}

void ArActionTriangleDriveTo::setRobot(ArRobot *robot)
{
  ArAction::setRobot(robot);
  if (myLineFinder == NULL && myRobot != NULL)
  {
    if ((myLaser = myRobot->findRangeDevice("laser")) != NULL)
    {
      myLineFinder = new ArLineFinder(myLaser);
      myOwnLineFinder = true;
    }
  }
}

void ArActionTriangleDriveTo::setLineFinder(ArLineFinder *lineFinder)
{
  if (myLineFinder != NULL && myOwnLineFinder)
    delete myLineFinder;

  myLineFinder = lineFinder;
  myOwnLineFinder = false;
}

ArPose ArActionTriangleDriveTo::findPoseFromVertex(
        double distFromVertex)
{
  ArPose ret;
  ArPose vertex;
  vertex = myRobot->getEncoderTransform().doTransform(myVertex);

  ret.setX((vertex.getX() + ArMath::cos(vertex.getTh()) * distFromVertex));
  ret.setY((vertex.getY() + ArMath::sin(vertex.getTh()) * distFromVertex));
  return ret;
}

void ArActionTriangleDriveTo::findTriangle(bool initial, 
                                                    bool goStraight)
{
  if (myGotLinesCounter != myRobot->getCounter())
  {
    ArTime now;
    now.setToNow();
    myLines = myLineFinder->getLines();
    //printf("took %d\n", now.mSecSince());
  }
  myGotLinesCounter = myRobot->getCounter();
  //  myLineFinder->saveLast();
  
  int start;
  int len = myLines->size();
  FILE *corners = NULL;
  /*if ((corners = fopen("corners", "w+")) == NULL)
  {
    printf("Couldn't open corners file\n");
  }
  */
  // length of line 1
  double line1Dist;
  // length of line 2
  double line2Dist;
  // distance from the end of line1 to the beginning of line2
  double distLine1ToLine2;
  // difference in angle between line1 and line2
  double angleBetween;
  // difference between the angleBetween and the angleBetween we want
  double angleDelta;
  double line1Delta;
  double line2Delta;
  // the score for the line evaluated
  double lineScore;
  // our position of this vertex
  ArPose vertex;
  // the score for our good line
  double goodLineScore = 0.0;
  // my pose for the good vertex
  ArPose goodVertex;
  ArLineSegment vertexLine;
  ArPose adjustedVertex;

  ArPose lastVertex;
  
  lastVertex = myRobot->getEncoderTransform().doTransform(myVertex);

  for (start = 0; start < len; start++)
  {
    if (start + 1 < len)
    {
      line1Dist = (*myLines)[start]->getEndPoint1().findDistanceTo(
              (*myLines)[start]->getEndPoint2());
      line2Dist = (*myLines)[start+1]->getEndPoint1().findDistanceTo(
              (*myLines)[start+1]->getEndPoint2());
      distLine1ToLine2 = (*myLines)[start]->getEndPoint2().findDistanceTo(
              (*myLines)[start+1]->getEndPoint1());
      angleBetween = ArMath::subAngle(180, 
                           ArMath::subAngle((*myLines)[start]->getLineAngle(),
                                           (*myLines)[start+1]->getLineAngle()));
      angleDelta = ArMath::fabs(ArMath::subAngle(angleBetween, myAngleBetween));
      line1Delta = ArMath::fabs(line1Dist - myLine1Length);
      line2Delta = ArMath::fabs(line2Dist - myLine2Length);
      if (0)
        ArLog::log(ArLog::Normal, "dl1l2 %5.0f l1d %5.0f l2d %5.0f thdt %5.0f l1dt %5.0f l2dt %5.0f", 
                   distLine1ToLine2,
                   line1Dist,
                   line2Dist,
                   angleDelta,
                   line1Delta, line2Delta);
      // if any of these is true the line is just too bad and we should bail
      if (line1Delta > 125 || line2Delta > 125 ||
          angleDelta > 15 || distLine1ToLine2 > 100)
        continue;


      ArPose intersection;


      ArLine *line1Line = new ArLine(*(*myLines)[start]->getLine());
      ArLine *line2Line = new ArLine(*(*myLines)[start+1]->getLine());
      if (!line1Line->intersects(line2Line, &intersection))
      {
        ArLog::log(ArLog::Terse, "ArActionTriangeDriveTo: couldn't find intersection of lines (shouldn't happen)");
        return;
      }
      delete line1Line;
      delete line2Line;


      
      /*
        printf("corner at polar %5.0f %5.0f pose (%.0f %.0f) angleDelta %.0f %.0f\n", 
        myFinder->getLinesTakenPose().findAngleTo(intersection),
        myFinder->getLinesTakenPose().findDistanceTo(intersection),
        intersection.getX(), intersection.getY(),
        angleDelta,
        ArMath::fabs((line1Delta + line2Delta) / 2));
      */
      if (corners != NULL)
        fprintf(corners, "%.0f %.0f\n", intersection.getX(), 
                intersection.getY());
      
      vertex.setPose(intersection);
      /*
      vertex.setTh(ArMath::subAngle(ArMath::atan2((*myLines)[start]->getY1() - 
                                                  (*myLines)[start+1]->getY2(),
                                                  (*myLines)[start]->getX1() - 
                                          (*myLines)[start+1]->getX2()),
                                    90));;
      */
      vertex.setTh(ArMath::addAngle((*myLines)[start]->getLineAngle(),
                                    angleBetween / 2));

      vertexLine.newEndPoints(vertex.getX(), vertex.getY(),
                      vertex.getX() + ArMath::cos(vertex.getTh()) * 10000,
                       vertex.getY() + ArMath::sin(vertex.getTh()) * 10000);


      if (myAdjustVertex) 
      {
        ArPose end1 = (*myLines)[start]->getEndPoint1();
        ArPose end2 = (*myLines)[start+1]->getEndPoint2();
        ArPose vertexLocal = vertex;
        end1 = myRobot->getToLocalTransform().doTransform(end1);
        end2 = myRobot->getToLocalTransform().doTransform(end2);
        vertexLocal = myRobot->getToLocalTransform().doTransform(vertexLocal);

        ArPose closest;
        /* old way that checked too large an area
        myLaser->currentReadingBox(0, end1.getY(),
                                   ArUtil::findMax(end1.getX(), end2.getX()),
                                   end2.getY(),
                                   &closest);
        */

        // new way that looks 100 mm closer to the robot than the
        // vertex this may not be quite right because there's a chance
        // the vertex won't be closest to the robot but that will be
        // at an angle deep enough we probably won't see the other
        // side the of the triangle so we should be okay
        myLaser->currentReadingBox(vertexLocal.getX() - 100, end1.getY(),
                                   ArUtil::findMax(end1.getX(), end2.getX()),
                                   end2.getY(),
                                   &closest);
        closest = myRobot->getToGlobalTransform().doTransform(closest);
        //printf("%g %g %g %g close %g %g\n", end1.getX(), end1.getY(), 
        //end2.getX(), end2.getY(), closest.getX(), closest.getY());
        
        adjustedVertex.setPose(
                closest.getX(), closest.getY(),
                ArMath::addAngle((*myLines)[start]->getLineAngle(),
                                 angleBetween / 2));

      } 


      //printf("One that may be good enough\n");
      // if we got here we should calculate the score for the line
      // first we calculate the score based on it matching
      lineScore = 0;
      lineScore += 10 - 1 * (line1Delta / 25);
      //printf("1 %.0f\n", lineScore);
      lineScore += 10 - 1 * (line2Delta / 25);
      //printf("2 %.0f\n", lineScore);
      lineScore += 10 - 1 * (angleDelta / 2);
      //printf("3 %.0f\n", lineScore);
      // now we calculate based on its position 
      
      // if we're in our initial one we mostly want to make sure its
      // in front of us
      if (initial)
      {
        if (0)
          printf("%.0f (%.3f) %.0f\n", 
                 ArMath::subAngle(myRobot->getTh(),
                                  myRobot->getPose().findAngleTo(vertex)),
                 90 - ArMath::fabs(myRobot->findDeltaHeadingTo(vertex)) / 90,
                 vertexLine.getDistToLine(myRobot->getPose()));
        
        //ArMath::subAngle(myRobot->getTh(),
        //myRobot->getPose().findAngleTo(vertex)))) / 90,

        // weight it more heavily for the lines in front of us
        lineScore *= .5 + .5 * (30 - 
                                ArMath::fabs(
                                        ArMath::subAngle(myRobot->getTh(),
                               myRobot->getPose().findAngleTo(vertex)))) / 30;
        //printf("angle %.0f\n", lineScore);
        lineScore *= .5 + .5 * (1500 - 
                                vertexLine.getDistToLine(myRobot->getPose())) / 1500;
        //printf("disttoline %.0f\n", lineScore);

        // weight it more heavily if the vertex points towards along
        // the same line as the line from the robot to the vertex
        lineScore *= .5 + .5 * (30 - fabs(ArMath::subAngle(
                vertex.getTh(), vertex.findAngleTo(myRobot->getPose()))));
        //printf("anglefrompointing %.0f\n", lineScore);
        

      }
      // for not the initial one weight them more heavily if they're
      // close to us
      else
      {
        double dist = lastVertex.findDistanceTo(vertex);
        if (dist < 100)
          lineScore *= 1;
        if (dist < 200)
          lineScore *= .8;
        else if (dist < 400)
          lineScore *= .5;
        else if (dist < 600)
          lineScore *= .2;
        else if (dist < 800)
          lineScore *= .1;
        else
          lineScore *= 0;

        double angleFromLast = fabs(ArMath::subAngle(lastVertex.getTh(), 
                                                vertex.getTh()));
        //printf("AngleFromLast %.0f %.0f %.0f\n", angleFromLast, lastVertex.getTh(), vertex.getTh());
        if (angleFromLast < 5)
          lineScore *= 1;
        else if (angleFromLast < 10)
          lineScore *= .75;
        else if (angleFromLast < 20)
          lineScore *= .5;
        else if (angleFromLast < 30)
          lineScore *= .25;
        else 
          lineScore *= 0;

        if (goStraight)
        {
          double angle = fabs(myRobot->findDeltaHeadingTo(vertex));
          if (angle < 2)
            lineScore *= 1;
          else if (angle < 5)
            lineScore *= .5;
          else if (angle < 10)
            lineScore *= .1;
          else
            lineScore *= 0;
        }
      }
      //printf("4 %.0f\n", lineScore);
      // if the match is too bad just bail
      if (lineScore < 5)
      {
        continue;
      }
      // otherwise see if its better then our previous one, if so set
      // our actual vertex to it
      if (goodLineScore < 1 || lineScore > goodLineScore)
      {
        if (0)
          printf("#### %.0f %.0f %.0f at %.0f %.0f\n", 
                 vertex.getX(), vertex.getY(), vertex.getTh(),
                 myRobot->getPose().findAngleTo(vertex),
                 myRobot->getPose().findDistanceTo(vertex));
        goodLineScore = lineScore;
        if (myAdjustVertex)
          myVertex = myRobot->getEncoderTransform().doInvTransform(
                  adjustedVertex);
        else
          myVertex = myRobot->getEncoderTransform().doInvTransform(vertex);
        myVertexSeen = true;
        myVertexSeenLast.setToNow();
      }
      continue;

    }
  }
  if (corners != NULL)
    fclose(corners);
  myDataMutex.lock();
  if (mySaveData)
  {
    if (myData != NULL)
      myData->setLinesAndVertex(
              myLines, myRobot->getEncoderTransform().doTransform(myVertex));
  }
  myDataMutex.unlock();
}

ArActionDesired *ArActionTriangleDriveTo::fire(
        ArActionDesired currentDesired)
{
  myDesired.reset();
  double dist;
  double angle;
  double vel;
  ArPose approach;
  ArPose final;
  ArPose vertex;
  
  myDataMutex.lock();
  if (myData != NULL)
  {
    delete myData;
    myData = NULL;
  }
  if (mySaveData)
    myData = new Data;
  myDataMutex.unlock();

  vertex = myRobot->getEncoderTransform().doTransform(myVertex);

  switch (myState)
  {
  case STATE_INACTIVE:
    return NULL;
  case STATE_SUCCEEDED:
  case STATE_FAILED:
    myDesired.setVel(0);
    myDesired.setRotVel(0);
    return &myDesired;
  case STATE_ACQUIRE:
    if (myPrinting)
      ArLog::log(ArLog::Normal, "Acquire");
    findTriangle(true);
    if (!myVertexSeen && 
        myVertexSeenLast.mSecSince() > myVertexUnseenStopMSecs)
    {
      if (myAcquire)
      {
        myState = STATE_SEARCHING;
        return fire(currentDesired);
      }
      else
      {
        if (myPrinting)
          ArLog::log(ArLog::Normal, "Failed");
        myState = STATE_FAILED;
        return fire(currentDesired);
      }
    } 
    if (myVertexSeen)
    {
      approach = findPoseFromVertex(myApproachDistFromVertex);
      final = findPoseFromVertex(myFinalDistFromVertex);
      if (mySaveData)
      {
        myDataMutex.lock();
        if (myData != NULL)
        {
          myData->setApproach(approach);
          myData->setFinal(final);
        }
        myDataMutex.unlock();
      }
      // if we aren't approaching or if its behind us go straight to
      // final
      if (myPrinting)
        ArLog::log(ArLog::Normal, "%.0f", ArMath::fabs(myRobot->findDeltaHeadingTo(approach)));
      if (myApproachDistFromVertex <= 0 || 
          ArMath::fabs(myRobot->findDeltaHeadingTo(approach)) > 90 ||
          (ArMath::fabs(myRobot->findDeltaHeadingTo(approach)) > 45 &&  
           myRobot->findDistanceTo(final) < myRobot->findDistanceTo(approach)))

      {
        if (myGotoVertex)
        {
          if (myPrinting)
            ArLog::log(ArLog::Normal, "Going to vertex");
          myState = STATE_GOTO_VERTEX;
        }
        else
        {
          if (myPrinting)
            ArLog::log(ArLog::Normal, "Going to final");
          myState = STATE_GOTO_FINAL;
        }
        return fire(currentDesired);
      }
      // otherwise we go to our approach
      else
      {
        myState = STATE_GOTO_APPROACH;
        return fire(currentDesired);
      }
    }
    myDesired.setVel(0);
    myDesired.setRotVel(0);
    return &myDesired;
  case STATE_SEARCHING:
    if (myPrinting)
      ArLog::log(ArLog::Normal, "Searching");
    myVertexSeen = false;
    findTriangle(true);
    if (myVertexSeen)
    {
      myState = STATE_ACQUIRE;
      return fire(currentDesired);
    }
    myDesired.setVel(0);
    myDesired.setRotVel(myAcquireTurnSpeed);
    return &myDesired;
    break;
  case STATE_GOTO_APPROACH:
    findTriangle(false);
    if (!myVertexSeen && 
        myVertexSeenLast.mSecSince() > myVertexUnseenStopMSecs)
    {
      if (myAcquire)
      {
        myState = STATE_SEARCHING;
        return fire(currentDesired);
      }
      else
      {
        ArLog::log(ArLog::Normal, "ArActionTriangleDriveTo: Failed");
        myState = STATE_FAILED;
        return fire(currentDesired);
      }
    } 
    approach = findPoseFromVertex(myApproachDistFromVertex);
    if (mySaveData)
    {
      myDataMutex.lock();
      if (myData != NULL)
        myData->setApproach(approach);
      myDataMutex.unlock();
    }
    dist = myRobot->getPose().findDistanceTo(approach);
    angle = myRobot->getPose().findAngleTo(approach);
    if (dist < myCloseDist || 
        (dist < myCloseDist * 2 && 
         ArMath::fabs(myRobot->findDeltaHeadingTo(approach)) > 30))
    {
      if (myPrinting)
        ArLog::log(ArLog::Normal, "Goto approach there");
      myState = STATE_ALIGN_APPROACH;
      return fire(currentDesired);
    }
    myDesired.setHeading(angle);
    vel = sqrt(dist * 400 * 2);
    vel *= (180 - ArMath::fabs(myRobot->findDeltaHeadingTo(approach))) / 180;
    if (vel < 0)
      vel = 0;
    if (vel > mySpeed)
      vel = mySpeed;
    myDesired.setVel(vel);
    if (myPrinting)
      ArLog::log(ArLog::Normal, "Goto approach speed %.0f dist %.0f angle %.0f", vel, dist, angle);
    return &myDesired;
  case STATE_ALIGN_APPROACH:
    angle = myRobot->getPose().findAngleTo(vertex);
    if (myPrinting)
      ArLog::log(ArLog::Normal, "Align approach %.0f %.0f", myRobot->getTh(),
                 angle);
    if (ArMath::fabs(ArMath::subAngle(myRobot->getTh(), angle)) < 2 &&
        ArMath::fabs(myRobot->getVel()) < 5)
    {
      //ArLog::log(ArLog::Normal, "finaldist %.0f", ArMath::fabs(myFinalDistFromVertex));
      if (myGotoVertex)
      {
        if (myPrinting)
          ArLog::log(ArLog::Normal, "Going to vertex");
        myState = STATE_GOTO_VERTEX;
      }
      else
      {
        if (myPrinting)
          ArLog::log(ArLog::Normal, "Going to final");
        myState = STATE_GOTO_FINAL;
      }
      return fire(currentDesired);
    }
    myDesired.setHeading(angle);
    myDesired.setVel(0);
    return &myDesired;
  case STATE_GOTO_VERTEX:
    /*
    final = findPoseFromVertex(0);
    if (myRobot->findDistanceTo(final) > 
        myFinalDistFromVertex + myIgnoreTriangleDist)
    */
    findTriangle(false, true);

    if (!myVertexSeen && 
        myVertexSeenLast.mSecSince() > myVertexUnseenStopMSecs)
    {
      ArLog::log(ArLog::Normal, "ActionTriangle: Failed");
      myState = STATE_FAILED;
      return fire(currentDesired);
    } 
    final = findPoseFromVertex(0);
    if (mySaveData)
    {
      myDataMutex.lock();
      if (myData != NULL)
        myData->setFinal(final);
      myDataMutex.unlock();
    }
    dist = myRobot->findDistanceTo(final);
    dist -= myFinalDistFromVertex;
    angle = myRobot->findDeltaHeadingTo(final);
    if (ArMath::fabs(angle) > 10)
      {
        ArLog::log(ArLog::Normal, "ActionTriangle: FAILING because trying to turn %.0f degrees to something %.0f away that we saw %ld ms ago ", angle, dist, myVertexSeenLast.mSecSince());
        myState = STATE_FAILED;
        return fire(currentDesired);
      }
    else
      {
        myDesired.setDeltaHeading(angle);
      }
    if (dist > 0)
      vel = sqrt(dist * 100 * 2);
    else
      vel = 0;
    if (vel < 0)
      vel = 0;
    if (vel > mySpeed)
      vel = mySpeed;
    myDesired.setVel(vel);
    if (dist <= 0 && fabs(myRobot->getVel()) < 5)
    {
      ArLog::log(ArLog::Normal, "ArActionTriangleDriveTo: Succeeded (vertex) %g", dist);
      myState = STATE_SUCCEEDED;
      return fire(currentDesired);
    }
    if (myPrinting)
      ArLog::log(ArLog::Normal, "Goto vertex speed %.0f dist %.0f angle %.0f %ld ago",
                 vel, dist, myRobot->findDeltaHeadingTo(final),
                 myVertexSeenLast.mSecSince());
    return &myDesired;

  case STATE_GOTO_FINAL:
    // see if we are close enough we just keep the same reading,
    // otherwise we get the new reading

    final = findPoseFromVertex(myFinalDistFromVertex);

    //if (myRobot->findDistanceTo(final) > myIgnoreTriangleDist)
    if (myRobot->findDistanceTo(final) > 250)
      findTriangle(false);

    if (!myVertexSeen && 
        myVertexSeenLast.mSecSince() > myVertexUnseenStopMSecs)
    {
      if (myAcquire)
      {
        myState = STATE_SEARCHING;
        return fire(currentDesired);
      }
      else
      {
        ArLog::log(ArLog::Normal, "ArActionTriangleDriveTo: Failed");
        myState = STATE_FAILED;
        return fire(currentDesired);
      }
    } 
    final = findPoseFromVertex(myFinalDistFromVertex);
    if (mySaveData)
    {
      myDataMutex.lock();
      if (myData != NULL)
        myData->setFinal(final);
      myDataMutex.unlock();
    }
    dist = myRobot->getPose().findDistanceTo(final);
    angle = myRobot->getPose().findAngleTo(final);
    if (myPrinting)
      ArLog::log(ArLog::Normal, 
                 "final %.0f away at %.0f vertex %.0f away %ld ago", 
                 dist, myRobot->findDeltaHeadingTo(final),
                 myRobot->findDistanceTo(vertex), 
                 myVertexSeenLast.mSecSince());

    if ((dist < 5) || 
        (myRobot->getVel() > 0 && dist < myCloseDist && 
         ArMath::fabs(myRobot->findDeltaHeadingTo(final)) > 20) ||
        (myRobot->getVel() < 0 && dist < myCloseDist && 
         ArMath::fabs(myRobot->findDeltaHeadingTo(final)) < 160) ||
        (ArMath::fabs(myRobot->getVel()) < 5 && dist < myCloseDist))
    {
      if (myPrinting)
        ArLog::log(ArLog::Normal, "Goto final there");
      myState = STATE_ALIGN_FINAL;
      return fire(currentDesired);
    }

    if (ArMath::fabs(ArMath::subAngle(myRobot->getTh(),
                                      angle)) < 90)
    {
      myDesired.setHeading(angle);
      vel = sqrt(dist * 100 * 2);
      vel *= (45 - ArMath::fabs(myRobot->findDeltaHeadingTo(final))) / 45;
      if (vel < 0)
        vel = 0;
      if (vel > mySpeed)
        vel = mySpeed;
      myDesired.setVel(vel);
      if (myPrinting)
        ArLog::log(ArLog::Normal, "Goto final speed %.0f dist %.0f angle %.0f", vel, dist, myRobot->findDeltaHeadingTo(final));

      return &myDesired;
    }
    else
    {
      myDesired.setHeading(
              ArMath::subAngle(myRobot->getPose().findAngleTo(final), 180));
      vel = -1 * sqrt(dist * 100 * 2);
      vel *= (45 - ArMath::fabs(ArMath::subAngle(180,
                                 myRobot->findDeltaHeadingTo(final)))) / 45;
      if (vel > 0)
        vel = 0;
      if (vel < -mySpeed)
        vel = -mySpeed;
      myDesired.setVel(vel);
      if (myPrinting)
        ArLog::log(ArLog::Normal, "Goto final (backing) speed %.0f dist %.0f angle %.0f (turning to %.0f)", vel, dist, angle, 
                   ArMath::subAngle(180, myRobot->findDeltaHeadingTo(final)));
      return &myDesired;
    }
  case STATE_ALIGN_FINAL:
    angle = myRobot->getPose().findAngleTo(vertex);
    if (ArMath::fabs(ArMath::subAngle(myRobot->getTh(), angle)) < 2 &&
        ArMath::fabs(myRobot->getVel()) < 5)
    {
      ArLog::log(ArLog::Normal, "ArActionTriangleDriveTo: Succeeded");
      myState = STATE_SUCCEEDED;
      return fire(currentDesired);
    }
    if (myPrinting)
      ArLog::log(ArLog::Normal, "Align final");
    myDesired.setHeading(angle);
    myDesired.setVel(0);
    return &myDesired;
  }

  findTriangle(true);
  return NULL;

}

ArActionTriangleDriveTo::Data *ArActionTriangleDriveTo::getData(void)
{
  Data *data = NULL;
  myDataMutex.lock();
  data = myData;
  myData = NULL;
  myDataMutex.unlock();
  return data;
}

---