251 lines
6.6 KiB
C++
251 lines
6.6 KiB
C++
/*
|
|
Adept MobileRobots Robotics Interface for Applications (ARIA)
|
|
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
|
|
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
|
|
Copyright (C) 2011, 2012, 2013 Adept Technology
|
|
|
|
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
|
|
Adept MobileRobots for information about a commercial version of ARIA at
|
|
robots@mobilerobots.com or
|
|
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
|
|
*/
|
|
#include "Aria.h"
|
|
|
|
/*
|
|
This demo starts up the robot in its own thread, then runs a bunch of the
|
|
direct motion commands... note that if you want to stop direct motion
|
|
commands and let actions take over, you should use clearDirectMotion, see
|
|
the docs for details. Also note how it locks the robot before doing anything
|
|
with the robot, then unlocks it when done, this is vital for threaded code.
|
|
You shouldn't really use threads if you don't understand them or don't need
|
|
them.
|
|
*/
|
|
|
|
/*
|
|
This is a connection handler, fairly simple, but quite useful, esp when
|
|
the robot is running in another thread. Its not really needed here
|
|
since blockingConnect is used. But it'd still be useful if there is an
|
|
error after connected
|
|
*/
|
|
class ConnHandler
|
|
{
|
|
public:
|
|
// Constructor
|
|
ConnHandler(ArRobot *robot);
|
|
// Destructor, its just empty
|
|
~ConnHandler(void) {}
|
|
// to be called if the connection was made
|
|
void connected(void);
|
|
// to call if the connection failed
|
|
void connFail(void);
|
|
// to be called if the connection was lost
|
|
void disconnected(void);
|
|
protected:
|
|
// robot pointer
|
|
ArRobot *myRobot;
|
|
// the functor callbacks
|
|
ArFunctorC<ConnHandler> myConnectedCB;
|
|
ArFunctorC<ConnHandler> myConnFailCB;
|
|
ArFunctorC<ConnHandler> myDisconnectedCB;
|
|
};
|
|
|
|
/*
|
|
The constructor, note its use of contructor chaining to initalize the
|
|
callbacks.
|
|
*/
|
|
ConnHandler::ConnHandler(ArRobot *robot) :
|
|
myConnectedCB(this, &ConnHandler::connected),
|
|
myConnFailCB(this, &ConnHandler::connFail),
|
|
myDisconnectedCB(this, &ConnHandler::disconnected)
|
|
|
|
{
|
|
// set the robot poitner
|
|
myRobot = robot;
|
|
|
|
// add the callbacks to the robot
|
|
myRobot->addConnectCB(&myConnectedCB, ArListPos::FIRST);
|
|
myRobot->addFailedConnectCB(&myConnFailCB, ArListPos::FIRST);
|
|
myRobot->addDisconnectOnErrorCB(&myDisconnectedCB, ArListPos::FIRST);
|
|
}
|
|
|
|
// just exit if the connection failed
|
|
void ConnHandler::connFail(void)
|
|
{
|
|
printf("Failed to connect.\n");
|
|
myRobot->stopRunning();
|
|
Aria::shutdown();
|
|
return;
|
|
}
|
|
|
|
// turn on motors, and off sonar, and off amigobot sounds, when connected
|
|
void ConnHandler::connected(void)
|
|
{
|
|
printf("Connected\n");
|
|
myRobot->comInt(ArCommands::SONAR, 0);
|
|
myRobot->comInt(ArCommands::ENABLE, 1);
|
|
myRobot->comInt(ArCommands::SOUNDTOG, 0);
|
|
}
|
|
|
|
// lost connection, so just exit
|
|
void ConnHandler::disconnected(void)
|
|
{
|
|
printf("Lost connection\n");
|
|
exit(0);
|
|
}
|
|
|
|
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
std::string str;
|
|
int ret;
|
|
int dist;
|
|
ArTime start;
|
|
ArPose startPose;
|
|
bool vel2 = false;
|
|
|
|
// connection to the robot
|
|
ArSerialConnection con;
|
|
// the robot
|
|
ArRobot robot;
|
|
// the connection handler from above
|
|
ConnHandler ch(&robot);
|
|
|
|
// init area with a dedicated signal handling thread
|
|
Aria::init(Aria::SIGHANDLE_THREAD);
|
|
|
|
if (argc != 2 || (dist = atoi(argv[1])) == 0)
|
|
{
|
|
printf("Usage: %s <distInMM>\n", argv[0]);
|
|
exit(0);
|
|
}
|
|
if (dist < 1000)
|
|
{
|
|
printf("You must go at least a meter\n");
|
|
exit(0);
|
|
}
|
|
// open the connection with the defaults, exit if failed
|
|
if ((ret = con.open()) != 0)
|
|
{
|
|
str = con.getOpenMessage(ret);
|
|
printf("Open failed: %s\n", str.c_str());
|
|
Aria::shutdown();
|
|
return 1;
|
|
}
|
|
|
|
// set the robots connection
|
|
robot.setDeviceConnection(&con);
|
|
// try to connect, if we fail, the connection handler should bail
|
|
if (!robot.blockingConnect())
|
|
{
|
|
// this should have been taken care of by the connection handler
|
|
// but just in case
|
|
printf(
|
|
"asyncConnect failed because robot is not running in its own thread.\n");
|
|
Aria::shutdown();
|
|
return 1;
|
|
}
|
|
// run the robot in its own thread, so it gets and processes packets and such
|
|
robot.runAsync(false);
|
|
|
|
// just a big long set of printfs, direct motion commands and sleeps,
|
|
// it should be self-explanatory
|
|
|
|
robot.lock();
|
|
|
|
/*
|
|
robot.setAbsoluteMaxTransVel(2000);
|
|
robot.setTransVelMax(2000);
|
|
robot.setTransAccel(1000);
|
|
robot.setTransDecel(1000);
|
|
robot.comInt(82, 30); // rotkp
|
|
robot.comInt(83, 200); // rotkv
|
|
robot.comInt(84, 0); // rotki
|
|
robot.comInt(85, 30); // transkp
|
|
robot.comInt(86, 450); // transkv
|
|
robot.comInt(87, 4); // transki
|
|
|
|
*/
|
|
printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
|
|
if (vel2)
|
|
robot.setVel2(2200, 2200);
|
|
else
|
|
robot.setVel(2200);
|
|
robot.unlock();
|
|
start.setToNow();
|
|
startPose = robot.getPose();
|
|
while (1)
|
|
{
|
|
robot.lock();
|
|
printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
|
|
robot.getVel(), robot.getX(), robot.getY(),
|
|
startPose.findDistanceTo(robot.getPose()),
|
|
robot.getTh());
|
|
if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
|
|
{
|
|
printf("\nFinished distance\n");
|
|
robot.setVel(0);
|
|
robot.unlock();
|
|
break;
|
|
}
|
|
if (start.mSecSince() > 10000)
|
|
{
|
|
printf("\nDistance timed out\n");
|
|
robot.setVel(0);
|
|
robot.unlock();
|
|
break;
|
|
}
|
|
robot.unlock();
|
|
ArUtil::sleep(50);
|
|
}
|
|
|
|
if (vel2)
|
|
robot.setVel2(0, 0);
|
|
else
|
|
robot.setVel(0);
|
|
start.setToNow();
|
|
while (1)
|
|
{
|
|
robot.lock();
|
|
if (vel2)
|
|
robot.setVel2(0, 0);
|
|
else
|
|
robot.setVel(0);
|
|
if (fabs(robot.getVel()) < 20)
|
|
{
|
|
printf("Stopped\n");
|
|
robot.unlock();
|
|
break;
|
|
}
|
|
if (start.mSecSince() > 2000)
|
|
{
|
|
printf("\nStop timed out\n");
|
|
robot.unlock();
|
|
break;
|
|
}
|
|
robot.unlock();
|
|
ArUtil::sleep(50);
|
|
}
|
|
robot.lock();
|
|
robot.disconnect();
|
|
robot.unlock();
|
|
// shutdown and ge tout
|
|
Aria::shutdown();
|
|
return 0;
|
|
}
|
|
|