ArServerModeDrive.cpp

Go to the documentation of this file.

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     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 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

#include "Aria.h"
#include "ArExport.h"
#include "ArServerModeDrive.h"

AREXPORT ArServerModeDrive::ArServerModeDrive(ArServerBase *server, 
                                              ArRobot *robot,
                                              bool takeControlOnJoystick) : 
  ArServerMode(robot, server, "drive"),
  myDriveGroup(robot),
  myServerDriveJoystickCB(this, &ArServerModeDrive::serverDriveJoystick),
  myJoyUserTaskCB(this, &ArServerModeDrive::joyUserTask),
  myServerSafeDrivingEnableCB(this, 
                              &ArServerModeDrive::serverSafeDrivingEnable),
  myServerSafeDrivingDisableCB(this, 
                              &ArServerModeDrive::serverSafeDrivingDisable)
{
  myHandlerCommands = NULL;
  myTakeControlOnJoystick = takeControlOnJoystick;
  myDriveSafely = true;
  myJoydriveAction.setStopIfNoButtonPressed(false);
  myDriveGroup.addAction(&myJoydriveAction, 75);
  myDriveGroup.addAction(&myStopAction, 60);
  myInputAction = myDriveGroup.getActionInput();
  setThrottleParams(200, 2000);
  myExtraUnsafeAction = NULL;
  if (myServer != NULL)
  {
    addModeData("driveJoystick", 
                "drives the robot as with a joystick",
                &myServerDriveJoystickCB,
                "byte2: vel, byte2: rotVel",
                "none", "Movement", "RETURN_NONE");    
  }
  if ((myJoyHandler = Aria::getJoyHandler()) == NULL)
  {
    myJoyHandler = new ArJoyHandler;
    myJoyHandler->init();
    Aria::setJoyHandler(myJoyHandler);
  }
  myRobot->addUserTask("driveJoyUserTask", 75, &myJoyUserTaskCB);
}

AREXPORT ArServerModeDrive::~ArServerModeDrive()
{

}

AREXPORT void ArServerModeDrive::activate(void)
{
  driveJoystick(0, 0);
}

AREXPORT void ArServerModeDrive::deactivate(void)
{
  myDriveGroup.deactivate();
  baseDeactivate();
}

AREXPORT void ArServerModeDrive::setSafeDriving(bool safe)
{
  // if this is a change then print it
  if (safe != myDriveSafely)
  {
    if (safe)
    {
      ArLog::log(ArLog::Normal, "Driving safely again");
    }
    else
    {
      ArLog::log(ArLog::Normal, "Driving UNSAFELY");
    }
    myNewDriveSafely = true;
  }
  myDriveSafely = safe;
  if (isActive())
    driveJoystick(myVel, myRotVel);
}

AREXPORT bool ArServerModeDrive::getSafeDriving(void)
{
  return myDriveSafely;
}

AREXPORT void ArServerModeDrive::serverSafeDrivingEnable(void)
{
  myRobot->lock();
  setSafeDriving(true);
  myRobot->unlock();
}

AREXPORT void ArServerModeDrive::serverSafeDrivingDisable(void)
{
  myRobot->lock();
  setSafeDriving(false);
  myRobot->unlock();
}

AREXPORT void ArServerModeDrive::addControlCommands(ArServerHandlerCommands *handlerCommands)
{
  myHandlerCommands = handlerCommands;
  myHandlerCommands->addCommand(
          "safeDrivingEnable",
          "Enables safe driving, which will attempt to prevent collisions (default)",
          &myServerSafeDrivingEnableCB, "UnsafeMovement");
  myHandlerCommands->addCommand(
          "safeDrivingDisable",
          "Disables safe driving, this is UNSAFE and will let you drive your robot into things or down stairs, use at your own risk",
          &myServerSafeDrivingDisableCB, "UnsafeMovement");
}

AREXPORT void ArServerModeDrive::setThrottleParams(int lowSpeed, int highSpeed)
{
  myJoydriveAction.setThrottleParams(lowSpeed, highSpeed);
}
  
AREXPORT void ArServerModeDrive::driveJoystick(double vel, double rotVel)
{
  bool wasActive;
  wasActive = isActive();
  if (!wasActive && !baseActivate())
    return;
  if (!wasActive || myNewDriveSafely)
  {
    myRobot->clearDirectMotion();
    if (myDriveSafely)
    {
      myDriveGroup.activateExclusive();
      myMode = "Drive";
      ArLog::log(ArLog::Verbose, "Driving safely");
    }
    else
    {
      myRobot->deactivateActions();
      myJoydriveAction.activate();
      myInputAction->activate();
      if (myExtraUnsafeAction != NULL)
        myExtraUnsafeAction->activate();
      myMode = "UNSAFE Drive";
      ArLog::log(ArLog::Verbose, "Driving unsafely");
    }
  }
  myNewDriveSafely = false;
  myVel = vel;
  myRotVel = rotVel;
  myDriveGroup.setVel(vel);
  myDriveGroup.setRotVel(rotVel);
  setActivityTimeToNow();
}

AREXPORT void ArServerModeDrive::serverDriveJoystick(ArServerClient *client, 
                                                     ArNetPacket *packet)
{
  double vel;
  double rotVel;

  vel = packet->bufToByte2();
  rotVel = packet->bufToByte2();
  myRobot->lock();
  driveJoystick(vel, rotVel);
  myRobot->unlock();
}

AREXPORT void ArServerModeDrive::joyUserTask(void)
{
  // if we're not active but we should be
  if (myTakeControlOnJoystick && !isActive() && 
      myJoyHandler->haveJoystick() && myJoyHandler->getButton(1))
  {
    if (ArServerMode::getActiveMode() != NULL)
      ArLog::log(ArLog::Normal, 
                 "ArServerModeDrive: Activating instead of %s because of local joystick", ArServerMode::getActiveMode()->getName());
    else
      ArLog::log(ArLog::Normal, 
                 "ArServerModeDrive: Activating because of local joystick");
    activate();
  }
}

AREXPORT void ArServerModeDrive::userTask(void)
{
  // Sets the robot so that we always thing we're trying to move in
  // this mode
  myRobot->forceTryingToMove();

  if (myJoyHandler->haveJoystick() && myJoyHandler->getButton(1))
    setActivityTimeToNow();
  if (!myStatusSetThisCycle)
  {
    if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
      myStatus = "Stalled";
    // this works because if the motors stalled above caught it, if
    // not and more values it means a stall
    else if (myRobot->getStallValue())
      myStatus = "Bumped";
    else if (ArMath::fabs(myRobot->getVel()) < 2 && 
             ArMath::fabs(myRobot->getRotVel()) < 2)
      myStatus = "Stopped";
    else
      myStatus = "Driving";
  }
  myStatusSetThisCycle = false;
}

---