458 lines
10 KiB
C++
458 lines
10 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
|
|
*/
|
|
|
|
// Includes
|
|
#include "Aria.h"
|
|
|
|
// A class that just wraps the robot so that people don't forget
|
|
// to unlock and lock the robot when using direct motion commands
|
|
class ArDirectMotion
|
|
{
|
|
public:
|
|
// Constructor
|
|
ArDirectMotion(ArRobot *robot)
|
|
{myRobot = robot;}
|
|
|
|
// Destructor
|
|
~ArDirectMotion() {}
|
|
|
|
// Wait until the turn times out or has been completed
|
|
// within the required range in degrees
|
|
void finishTurn(double timeOut, double withinDeg)
|
|
{
|
|
ArTime start;
|
|
while (1)
|
|
{
|
|
myRobot->lock();
|
|
if (myRobot->isHeadingDone(withinDeg))
|
|
{
|
|
printf("Finished turn\n");
|
|
myRobot->unlock();
|
|
break;
|
|
}
|
|
if (start.mSecSince() > timeOut)
|
|
{
|
|
printf("Turn timed out\n");
|
|
myRobot->unlock();
|
|
break;
|
|
}
|
|
myRobot->unlock();
|
|
}
|
|
}
|
|
|
|
// Wait until the move times out or has been completed
|
|
// within the required range in mm
|
|
void finishMove(double timeOut, double withinDist)
|
|
{
|
|
ArTime start;
|
|
while (1)
|
|
{
|
|
myRobot->lock();
|
|
if (myRobot->isMoveDone(withinDist))
|
|
{
|
|
printf("Finished move\n");
|
|
myRobot->unlock();
|
|
break;
|
|
}
|
|
if (start.mSecSince() > timeOut)
|
|
{
|
|
printf("Turn timed out\n");
|
|
myRobot->unlock();
|
|
break;
|
|
}
|
|
myRobot->unlock();
|
|
}
|
|
}
|
|
|
|
// Move a distance if that distance + the buffer space
|
|
// is clear, otherwise do nothing
|
|
void move(double distance, double bufferSpace)
|
|
{
|
|
myRobot->lock();
|
|
|
|
if(myRobot->checkRangeDevicesCurrentBox(0, -myRobot->getRobotRadius(),
|
|
distance + bufferSpace, myRobot->getRobotRadius())
|
|
>= distance + myRobot->getRobotRadius() + bufferSpace)
|
|
{
|
|
myRobot->move(distance);
|
|
}
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Change heading by
|
|
void setDeltaHeading(double heading)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setDeltaHeading(heading);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set to absolute heading
|
|
void setHeading(double degree)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setHeading(degree);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set the maximum translational velocity
|
|
void setTransVelMax(double max)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setTransVelMax(max);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set the translational acceleration
|
|
void setTransAccel(double acc)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setTransAccel(acc);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set the translational deceleration
|
|
void setTransDecel(double decel)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setTransDecel(decel);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set the rotational acceleration
|
|
void setRotAccel(double acc)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setRotAccel(acc);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set the rotational deceleration
|
|
void setRotDecel(double decel)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setRotDecel(decel);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set the maximum rotational velocity
|
|
void setRotVelMax(double max)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setRotVelMax(max);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set the rotational velocity
|
|
void setRotVel(double velocity)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setRotVel(velocity);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set the translational veocity
|
|
void setVel(double velocity)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setVel(velocity);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Set the velocities of each wheel
|
|
void setVel2(double left, double right)
|
|
{
|
|
myRobot->lock();
|
|
myRobot->setVel2(left, right);
|
|
myRobot->unlock();
|
|
}
|
|
|
|
// Stop the wheels
|
|
void stop()
|
|
{
|
|
myRobot->lock();
|
|
myRobot->stop();
|
|
myRobot->unlock();
|
|
}
|
|
|
|
|
|
protected:
|
|
ArRobot *myRobot;
|
|
};
|
|
|
|
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
// To simply connect
|
|
ArSimpleConnector simpleConnector(&argc, argv);
|
|
|
|
// The robot
|
|
ArRobot robot;
|
|
|
|
// The key handler
|
|
ArKeyHandler keyHandler;
|
|
|
|
// Sonar
|
|
ArSonarDevice sonarDev;
|
|
// Laser
|
|
//ArSick laserDev;
|
|
|
|
//Direct Motion Commands
|
|
ArDirectMotion motion(&robot);
|
|
|
|
// Parse the arguments from the simple connector
|
|
simpleConnector.parseArgs();
|
|
|
|
// Some arguments did not parse....
|
|
// The program fails to understand and shuts down.
|
|
if (argc > 1)
|
|
{
|
|
simpleConnector.logOptions();
|
|
keyHandler.restore();
|
|
exit(1);
|
|
}
|
|
|
|
// Initialize Aria
|
|
Aria::init();
|
|
|
|
// Give Aria the key handler
|
|
Aria::setKeyHandler(&keyHandler);
|
|
|
|
// Attach the key handler to the robot
|
|
robot.attachKeyHandler(&keyHandler);
|
|
|
|
// Add the sonar to the robot
|
|
robot.addRangeDevice(&sonarDev);
|
|
// Add the laser (if we have it) to the robot
|
|
//robot.addRangeDevice(&laserDev);
|
|
|
|
// Connect to the robot
|
|
if (!simpleConnector.connectRobot(&robot))
|
|
{
|
|
printf("Could not connect to robot... exiting\n");
|
|
Aria::shutdown();
|
|
keyHandler.restore();
|
|
return 1;
|
|
}
|
|
|
|
|
|
// Run the robot in its own thread
|
|
robot.runAsync(false);
|
|
|
|
//simpleConnector.setupLaser(&laserDev);
|
|
//laserDev.runAsync();
|
|
/*
|
|
if (!laserDev.blockingConnect())
|
|
{
|
|
printf("Could not connect to SICK laser... exiting\n");
|
|
Aria::shutdown();
|
|
return 1;
|
|
}
|
|
*/
|
|
|
|
// turn on the motors
|
|
robot.comInt(ArCommands::ENABLE, 1);
|
|
robot.comInt(ArCommands::JOYDRIVE, 1);
|
|
|
|
|
|
|
|
|
|
|
|
//----------------------------------------------------------------
|
|
// The robot's settings for this run (feel free to change these)
|
|
//----------------------------------------------------------------
|
|
|
|
// Set the Robot's PIDs
|
|
robot.comInt(82, 50); // rotkp
|
|
robot.comInt(83, 300); // rotkv
|
|
robot.comInt(84, 10); // rotki
|
|
robot.comInt(85, 25); // transkp
|
|
robot.comInt(86, 600); // transkv
|
|
robot.comInt(87, 10); // transki
|
|
|
|
// Set the robot's velocities and accelerations
|
|
motion.setTransVelMax(2999);
|
|
motion.setRotVelMax(2999);
|
|
motion.setTransAccel(2999);
|
|
motion.setTransDecel(2999);
|
|
motion.setRotAccel(2999);
|
|
motion.setRotDecel(2999);
|
|
|
|
|
|
|
|
//----------------------------------------------------------------
|
|
// The robot's test pattern for this run (feel free to change this)
|
|
//----------------------------------------------------------------
|
|
|
|
// The test pattern described below
|
|
printf("Executing random test pattern\n");
|
|
|
|
// Some constants for this run
|
|
const double TIMOUT_TIME = 5000; // Time(msec) before a movement times out
|
|
const double WITHIN_DIST = 50; // If within this(mm) of target, good enough
|
|
const double WITHIN_DEG = 10; // If within this(deg) of target, good enough
|
|
const double SPACE_BUFFER = 600; // Extra space to give robot
|
|
|
|
// Distances and angles for pattern
|
|
double distance = 100;
|
|
double angle = 30;
|
|
|
|
/*
|
|
A figure 8 test pattern
|
|
|
|
// The initial move
|
|
motion.move(distance, SPACE_BUFFER);
|
|
motion.finishMove(TIMOUT_TIME, WITHIN_DIST);
|
|
|
|
while(robot.isRunning())
|
|
{
|
|
// Turn right
|
|
if(!robot.isRunning())break;
|
|
motion.setDeltaHeading(-angle);
|
|
motion.finishTurn(TIMOUT_TIME, WITHIN_DEG);
|
|
|
|
// Move forward
|
|
if(!robot.isRunning())break;
|
|
motion.move(distance, SPACE_BUFFER);
|
|
motion.finishMove(TIMOUT_TIME, WITHIN_DIST);
|
|
|
|
// Turn right
|
|
if(!robot.isRunning())break;
|
|
motion.setDeltaHeading(-angle);
|
|
motion.finishTurn(TIMOUT_TIME, WITHIN_DEG);
|
|
|
|
|
|
// Move forward
|
|
if(!robot.isRunning())break;
|
|
motion.move(distance, SPACE_BUFFER);
|
|
motion.finishMove(TIMOUT_TIME, WITHIN_DIST);
|
|
|
|
// Turn right
|
|
if(!robot.isRunning())break;
|
|
motion.setDeltaHeading(-angle);
|
|
motion.finishTurn(TIMOUT_TIME, WITHIN_DEG);
|
|
|
|
|
|
// Move forward
|
|
if(!robot.isRunning())break;
|
|
motion.move(2 * distance, SPACE_BUFFER);
|
|
motion.finishMove(TIMOUT_TIME, WITHIN_DIST);
|
|
|
|
// Turn left
|
|
if(!robot.isRunning())break;
|
|
motion.setDeltaHeading(angle);
|
|
motion.finishTurn(TIMOUT_TIME, WITHIN_DEG);
|
|
|
|
|
|
// Move forward
|
|
if(!robot.isRunning())break;
|
|
motion.move(distance, SPACE_BUFFER);
|
|
motion.finishMove(TIMOUT_TIME, WITHIN_DIST);
|
|
|
|
// Turn left
|
|
if(!robot.isRunning())break;
|
|
motion.setDeltaHeading(angle);
|
|
motion.finishTurn(TIMOUT_TIME, WITHIN_DEG);
|
|
|
|
|
|
// Move forward
|
|
if(!robot.isRunning())break;
|
|
motion.move(distance, SPACE_BUFFER);
|
|
motion.finishMove(TIMOUT_TIME, WITHIN_DIST);
|
|
|
|
// Turn left
|
|
if(!robot.isRunning())break;
|
|
motion.setDeltaHeading(angle);
|
|
motion.finishTurn(TIMOUT_TIME, WITHIN_DEG);
|
|
|
|
|
|
// Move forward
|
|
if(!robot.isRunning())break;
|
|
motion.move(2 * distance, SPACE_BUFFER);
|
|
motion.finishMove(TIMOUT_TIME, WITHIN_DIST);
|
|
}
|
|
*/
|
|
|
|
|
|
while(robot.isRunning())
|
|
{
|
|
double randomNumber = rand();
|
|
|
|
int choice = (int)randomNumber % 3;
|
|
|
|
switch(choice)
|
|
{
|
|
case 0:
|
|
// Turn right
|
|
motion.setDeltaHeading(-angle);
|
|
motion.finishTurn(TIMOUT_TIME, WITHIN_DEG);
|
|
break;
|
|
case 1:
|
|
// Turn left
|
|
motion.setDeltaHeading(angle);
|
|
motion.finishTurn(TIMOUT_TIME, WITHIN_DEG);
|
|
break;
|
|
case 2:
|
|
// Move forward
|
|
motion.move(distance, SPACE_BUFFER);
|
|
motion.finishMove(TIMOUT_TIME, WITHIN_DIST);
|
|
default:
|
|
motion.stop();
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
// now exit
|
|
Aria::shutdown();
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|