601 lines
19 KiB
C++
601 lines
19 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"
|
|
|
|
/** @example dpptuExample.cpp Shows how to control the Directed Perception
|
|
* pan-tilt unit using ArDPPTU class directly.
|
|
*
|
|
* This program lets you use the keyboard to control the DPPTU. It uses the same acceleration and slew rates for the pan and tilt axes.
|
|
*
|
|
* It is also possible to specify the type of PTU in program configuration
|
|
* (in the ARIA robot parameter files or program command-line arguments)
|
|
* instead. For an example of that, see cameraPTZExample.cpp instead.
|
|
|
|
Commands:
|
|
_________________
|
|
|
|
UP,DOWN -- tilt up/down by one positional increment
|
|
LEFT,RIGHT -- pan left/right by one positional increment
|
|
SPACE -- perform reset calibration
|
|
I -- initialize PTU to default settings
|
|
<,> -- increase/decrease the posIncrement by 0.5 degree
|
|
+,- -- increase/decrease the speed by 1 degree/sec
|
|
A -- awaits the completion of last issued positional command
|
|
R -- change pan/tilt movements to relative or absolute movements
|
|
Z -- move pan and tilt axes to zero
|
|
1 -- move to stored position 1 (-90, 0)
|
|
2 -- move to stored position 2 (90, 0)
|
|
3 -- move to stored position 3 (0, -45)
|
|
4 -- move to stored position 4 (0, 30)
|
|
M -- Enter or Exit monitor (continuous scan) mode
|
|
H -- Halt all motion
|
|
S -- print current variable values
|
|
ESC -- quit
|
|
*/
|
|
|
|
// If defined, use this computer serial port. If not defined, use first robot
|
|
// aux. serial port. Most robots have the DPPTU on COM2 if on Linux and COM4 on
|
|
// Windows, if not equipped with other accessories which might require those
|
|
// ports (e.g. GPS or Laser).
|
|
#define SERIAL_PORT ArUtil::COM4
|
|
|
|
|
|
// Determines type of DPPTU to set internal conversion factors. See enum of
|
|
// types in ArDPPTU class for possible values.
|
|
//#define PTU_TYPE ArDPPTU::PANTILT_PTUD46
|
|
|
|
// by how much the < and > keys change the position command increment in this
|
|
// example
|
|
#define POS_INC_ADJUSTMENT 1
|
|
|
|
/*
|
|
This class is the core of this demo, it adds itself to the robot given
|
|
as a user task, and contains key handler callbacks to control the PTU.
|
|
*/
|
|
class KeyPTU
|
|
{
|
|
public:
|
|
// constructor
|
|
KeyPTU(ArRobot *robot);
|
|
~KeyPTU(void);
|
|
|
|
void up(void);
|
|
void down(void);
|
|
void left(void);
|
|
void right(void);
|
|
void space(void);
|
|
void i(void);
|
|
void plus(void);
|
|
void minus(void);
|
|
void greater(void);
|
|
void less(void);
|
|
void question(void);
|
|
void status(void);
|
|
void a(void);
|
|
void m(void);
|
|
void h(void);
|
|
void r(void);
|
|
void gotoPos(double p, double t);
|
|
|
|
// the callback function
|
|
void drive(void);
|
|
|
|
protected:
|
|
int myPanValPTU;
|
|
int myTiltValPTU;
|
|
|
|
int myDesiredPanPos;
|
|
int myDesiredTiltPos;
|
|
int mySlew;
|
|
int myPosIncrement;
|
|
int mySlewIncrement;
|
|
|
|
int POS_INCREMENT_ADJUSTMENT;
|
|
|
|
bool myMonitor;
|
|
bool myReset;
|
|
bool myInit;
|
|
bool myAbsolute;
|
|
|
|
ArFunctorC<KeyPTU> myUpCB;
|
|
ArFunctorC<KeyPTU> myDownCB;
|
|
ArFunctorC<KeyPTU> myLeftCB;
|
|
ArFunctorC<KeyPTU> myRightCB;
|
|
ArFunctorC<KeyPTU> mySpaceCB;
|
|
ArFunctorC<KeyPTU> myICB;
|
|
ArFunctorC<KeyPTU> myPlusCB;
|
|
ArFunctorC<KeyPTU> myMinusCB;
|
|
ArFunctorC<KeyPTU> myGreaterCB;
|
|
ArFunctorC<KeyPTU> myLessCB;
|
|
ArFunctorC<KeyPTU> myQuestionCB;
|
|
ArFunctorC<KeyPTU> mySCB;
|
|
ArFunctorC<KeyPTU> myACB;
|
|
ArFunctorC<KeyPTU> myMCB;
|
|
ArFunctorC<KeyPTU> myHCB;
|
|
ArFunctorC<KeyPTU> myRCB;
|
|
ArFunctor2C<KeyPTU, double, double> myPos0CB;
|
|
ArFunctor2C<KeyPTU, double, double> myPos1CB;
|
|
ArFunctor2C<KeyPTU, double, double> myPos2CB;
|
|
ArFunctor2C<KeyPTU, double, double> myPos3CB;
|
|
ArFunctor2C<KeyPTU, double, double> myPos4CB;
|
|
|
|
|
|
// the PTU
|
|
ArDPPTU myPTU;
|
|
|
|
|
|
// whether the PTU has been inited or not
|
|
bool myPTUInited;
|
|
// pointer to the robot
|
|
ArRobot *myRobot;
|
|
// callback for the drive function
|
|
ArFunctorC<KeyPTU> myDriveCB;
|
|
|
|
ArSerialConnection *mySerialConnection;
|
|
};
|
|
|
|
/*
|
|
Constructor, sets the robot pointer, and some initial values, also note the
|
|
use of constructor chaining on myPTU and myDriveCB.
|
|
*/
|
|
KeyPTU::KeyPTU(ArRobot *robot) :
|
|
myUpCB(this, &KeyPTU::up),
|
|
myDownCB(this, &KeyPTU::down),
|
|
myLeftCB(this, &KeyPTU::left),
|
|
myRightCB(this, &KeyPTU::right),
|
|
mySpaceCB(this, &KeyPTU::space),
|
|
myICB(this, &KeyPTU::i),
|
|
myPlusCB(this, &KeyPTU::plus),
|
|
myMinusCB(this, &KeyPTU::minus),
|
|
myGreaterCB(this, &KeyPTU::greater),
|
|
myLessCB(this, &KeyPTU::less),
|
|
myQuestionCB(this, &KeyPTU::question),
|
|
mySCB(this, &KeyPTU::status),
|
|
myACB(this, &KeyPTU::a),
|
|
myMCB(this, &KeyPTU::m),
|
|
myHCB(this, &KeyPTU::h),
|
|
myRCB(this, &KeyPTU::r),
|
|
myPos0CB(this, &KeyPTU::gotoPos, 0, 0),
|
|
myPos1CB(this, &KeyPTU::gotoPos, -90.0, 0.0),
|
|
myPos2CB(this, &KeyPTU::gotoPos, 90.0, 0.0),
|
|
myPos3CB(this, &KeyPTU::gotoPos, 0.0, -45.0),
|
|
myPos4CB(this, &KeyPTU::gotoPos, 0.0, 30.0),
|
|
myPTU(robot),
|
|
myDriveCB(this, &KeyPTU::drive),
|
|
mySerialConnection(NULL)
|
|
{
|
|
#ifdef SERIAL_PORT
|
|
mySerialConnection = new ArSerialConnection;
|
|
ArLog::log(ArLog::Normal, "dpptuExample: connecting to DPPTU over computer serial port %s.", SERIAL_PORT);
|
|
if(mySerialConnection->open(SERIAL_PORT) != 0)
|
|
{
|
|
ArLog::log(ArLog::Terse, "dpptuExample: Error: Could not open computer serial port %s for DPPTU!", SERIAL_PORT);
|
|
Aria::exit(5);
|
|
}
|
|
myPTU.setDeviceConnection(mySerialConnection);
|
|
#endif
|
|
|
|
// set the robot pointer and add the KeyPTU as user task
|
|
ArKeyHandler *keyHandler;
|
|
myRobot = robot;
|
|
myRobot->addSensorInterpTask("KeyPTU", 50, &myDriveCB);
|
|
|
|
if ((keyHandler = Aria::getKeyHandler()) == NULL)
|
|
{
|
|
keyHandler = new ArKeyHandler;
|
|
Aria::setKeyHandler(keyHandler);
|
|
myRobot->attachKeyHandler(keyHandler);
|
|
}
|
|
|
|
if (!keyHandler->addKeyHandler(ArKeyHandler::UP, &myUpCB))
|
|
ArLog::log(ArLog::Terse, "The key handler already has a key for up, keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler(ArKeyHandler::DOWN, &myDownCB))
|
|
ArLog::log(ArLog::Terse, "The key handler already has a key for down, keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler(ArKeyHandler::LEFT, &myLeftCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for left, keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler(ArKeyHandler::RIGHT, &myRightCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for right, keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for space, keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('i', &myICB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for 'i', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('+', &myPlusCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for '+', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('-', &myMinusCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for '-', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('>', &myGreaterCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for '>', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('<', &myLessCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for '<', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('?', &myQuestionCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for '?', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('s', &mySCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for 'S', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('a', &myACB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for 'A', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('z', &myPos0CB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for 'Z', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('m', &myMCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for 'M', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('h', &myHCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for 'H', keydrive will not work correctly.");
|
|
if (!keyHandler->addKeyHandler('r', &myRCB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for 'R', keydrive will not work correctly.");
|
|
|
|
|
|
if(!keyHandler->addKeyHandler('Z', &myPos0CB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for 'Z', keydrive will not work correctly.");
|
|
|
|
if(!keyHandler->addKeyHandler('1', &myPos1CB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for '1', keydrive will not work correctly.");
|
|
|
|
if(!keyHandler->addKeyHandler('2', &myPos2CB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for '2', keydrive will not work correctly.");
|
|
|
|
if(!keyHandler->addKeyHandler('3', &myPos3CB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for '3', keydrive will not work correctly.");
|
|
|
|
if(!keyHandler->addKeyHandler('4', &myPos4CB))
|
|
ArLog::log(ArLog::Terse,
|
|
"The key handler already has a key for '4', keydrive will not work correctly.");
|
|
|
|
// initialize some variables
|
|
myReset = false;
|
|
myInit = true;
|
|
myDesiredPanPos = 0;
|
|
myDesiredTiltPos = 0;
|
|
myPosIncrement = 1;
|
|
mySlewIncrement = 1;
|
|
myPTUInited = false;
|
|
myMonitor = false;
|
|
|
|
}
|
|
|
|
KeyPTU::~KeyPTU()
|
|
{
|
|
if(mySerialConnection)
|
|
{
|
|
myPTU.setDeviceConnection(NULL);
|
|
delete mySerialConnection;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void KeyPTU::left(void)
|
|
{
|
|
myDesiredPanPos += myPosIncrement;
|
|
|
|
if (myDesiredPanPos > myPTU.getMaxPosPan())
|
|
myDesiredPanPos = myPTU.getMaxPosPan();
|
|
}
|
|
|
|
void KeyPTU::right(void)
|
|
{
|
|
myDesiredPanPos -= myPosIncrement;
|
|
|
|
if (myDesiredPanPos < myPTU.getMaxNegPan())
|
|
myDesiredPanPos = myPTU.getMaxNegPan();
|
|
}
|
|
|
|
void KeyPTU::up(void)
|
|
{
|
|
myDesiredTiltPos += myPosIncrement;
|
|
|
|
if (myDesiredTiltPos > myPTU.getMaxPosTilt())
|
|
myDesiredTiltPos = myPTU.getMaxPosTilt();
|
|
}
|
|
|
|
void KeyPTU::down(void)
|
|
{
|
|
myDesiredTiltPos -= myPosIncrement;
|
|
|
|
if (myDesiredTiltPos < myPTU.getMaxNegTilt())
|
|
myDesiredTiltPos = myPTU.getMaxNegTilt();
|
|
}
|
|
|
|
void KeyPTU::space(void)
|
|
{
|
|
myReset = true;
|
|
}
|
|
|
|
void KeyPTU::i(void)
|
|
{
|
|
myInit = true;
|
|
}
|
|
|
|
void KeyPTU::plus(void)
|
|
{
|
|
mySlew += mySlewIncrement;
|
|
|
|
if (mySlew > myPTU.getMaxPanSlew())
|
|
mySlew = myPTU.getMaxPanSlew();
|
|
|
|
status();
|
|
}
|
|
|
|
void KeyPTU::minus(void)
|
|
{
|
|
mySlew -= mySlewIncrement;
|
|
|
|
if (mySlew < myPTU.getMinPanSlew())
|
|
mySlew = myPTU.getMinPanSlew();
|
|
|
|
status();
|
|
}
|
|
|
|
void KeyPTU::greater(void)
|
|
{
|
|
myPosIncrement += POS_INCREMENT_ADJUSTMENT;
|
|
|
|
if (myPosIncrement > myPTU.getMaxPosPan())
|
|
myPosIncrement = myPTU.getMaxPosPan();
|
|
|
|
status();
|
|
}
|
|
|
|
void KeyPTU::less(void)
|
|
{
|
|
myPosIncrement -= POS_INCREMENT_ADJUSTMENT;
|
|
|
|
if (myPosIncrement < 0)
|
|
myPosIncrement = 0;
|
|
|
|
status();
|
|
}
|
|
|
|
void KeyPTU::a(void)
|
|
{
|
|
myPTU.awaitExec();
|
|
ArLog::log(ArLog::Normal, "AwaitExecution command sent");
|
|
}
|
|
|
|
void KeyPTU::gotoPos(double p, double t)
|
|
{
|
|
myDesiredPanPos = p;
|
|
myDesiredTiltPos = t;
|
|
status();
|
|
}
|
|
|
|
void KeyPTU::question(void)
|
|
{
|
|
ArLog::log(ArLog::Normal, "\r\nCommands:\r\n_________________\r\n");
|
|
ArLog::log(ArLog::Normal, "UP,DOWN -- tilt up/down by one positional increment");
|
|
ArLog::log(ArLog::Normal, "LEFT,RIGHT -- pan left/right by one positional increment");
|
|
ArLog::log(ArLog::Normal, "SPACE -- perform reset calibration");
|
|
ArLog::log(ArLog::Normal, "I -- initialize PTU to default settings");
|
|
ArLog::log(ArLog::Normal, "<,> -- increase/decrease the posIncrement by %f degree(s)", POS_INC_ADJUSTMENT);
|
|
ArLog::log(ArLog::Normal, "+,- -- increase/decrease the speed by 1 degree/sec");
|
|
ArLog::log(ArLog::Normal, "A -- awaits the completion of last issued positional command");
|
|
ArLog::log(ArLog::Normal, "R -- change pan/tilt movements to relative or absolute movements");
|
|
ArLog::log(ArLog::Normal, "Z -- move pan and tilt axes to zero");
|
|
ArLog::log(ArLog::Normal, "1 -- move to stored position 1 (-90, 0)");
|
|
ArLog::log(ArLog::Normal, "2 -- move to stored position 2 (90, 0)");
|
|
ArLog::log(ArLog::Normal, "3 -- move to stored position 3 (0, -45)");
|
|
ArLog::log(ArLog::Normal, "4 -- move to stored position 4 (0, 30)");
|
|
ArLog::log(ArLog::Normal, "M -- Enter or Exit monitor (continuous scan) mode");
|
|
ArLog::log(ArLog::Normal, "H -- Halt all motion");
|
|
ArLog::log(ArLog::Normal, "S -- print current variable values");
|
|
ArLog::log(ArLog::Normal, "ESC -- exit program");
|
|
ArLog::log(ArLog::Normal, "\r\n");
|
|
}
|
|
|
|
void KeyPTU::status(void)
|
|
{
|
|
ArLog::log(ArLog::Normal, "\r\nStatus:\r\n_________________\r\n");
|
|
ArLog::log(ArLog::Normal, "Last Pan Command = %.1f deg", myPTU.getLastPanRequest());
|
|
ArLog::log(ArLog::Normal, "Last Tilt Command = %.1f deg", myPTU.getLastTiltRequest());
|
|
ArLog::log(ArLog::Normal, "Current Pan Position = %.1f deg", myPTU.getPan());
|
|
ArLog::log(ArLog::Normal, "Current Tilt Position = %.1f deg", myPTU.getTilt());
|
|
ArLog::log(ArLog::Normal, "Pan Slew Rate = %d deg/sec", myPTU.getPanSlew());
|
|
ArLog::log(ArLog::Normal, "Tilt Slew Rate = %d deg/sec", myPTU.getTiltSlew());
|
|
ArLog::log(ArLog::Normal, "Position Increment = %d deg", myPosIncrement);
|
|
if (myAbsolute)
|
|
ArLog::log(ArLog::Normal, "Positional-movements using absolute commands");
|
|
else
|
|
ArLog::log(ArLog::Normal, "Positional-movements using relative commands");
|
|
ArLog::log(ArLog::Normal, "\r\n");
|
|
}
|
|
|
|
void KeyPTU::m(void)
|
|
{
|
|
if (!myMonitor)
|
|
{
|
|
ArLog::log(ArLog::Normal, "Entering Monitor mode - hit 'M' to disable");
|
|
myMonitor = true;
|
|
myPTU.initMon(-60,60,30,-30);
|
|
}
|
|
else
|
|
{
|
|
myPTU.blank(); //Blank packet exits monitor mode
|
|
myMonitor = false;
|
|
}
|
|
}
|
|
|
|
void KeyPTU::h(void)
|
|
{
|
|
myPTU.haltAll();
|
|
}
|
|
|
|
void KeyPTU::r(void)
|
|
{
|
|
if (!myAbsolute)
|
|
{
|
|
myAbsolute = true;
|
|
}
|
|
else
|
|
{
|
|
myAbsolute = false;
|
|
}
|
|
status();
|
|
}
|
|
|
|
|
|
// the important function
|
|
void KeyPTU::drive(void)
|
|
{
|
|
|
|
// if the PTU isn't initialized, initialize it here... it has to be
|
|
// done here instead of above because it needs to be done when the
|
|
// robot is connected
|
|
if (!myPTUInited && myRobot->isConnected())
|
|
{
|
|
ArLog::log(ArLog::Normal, "Initializing ArDPPTU...");
|
|
myPTU.init();
|
|
ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration...");
|
|
myPTU.resetCalib();
|
|
myPTU.awaitExec(); // DPPTU will wait for self-calibration to end before executing the following commands (though they will still be sent)
|
|
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
|
|
myPTU.awaitExec();
|
|
myPTUInited = true;
|
|
myInit = false;
|
|
myAbsolute = true;
|
|
}
|
|
|
|
if (myInit == true) // User hit initialization key
|
|
{
|
|
ArLog::log(ArLog::Normal, "Initializing PTU...");
|
|
myPTU.init();
|
|
myInit = false;
|
|
myDesiredPanPos = myPTU.getPan();
|
|
myDesiredTiltPos = myPTU.getTilt();
|
|
mySlew = myPTU.getPanSlew(); //uses only pan slew rate
|
|
myReset = false;
|
|
}
|
|
|
|
if (myReset == true) // User hit reset key
|
|
{
|
|
ArLog::log(ArLog::Normal, "Resetting PTU and performing self-calibration...");
|
|
myPTU.resetCalib();
|
|
myPTU.awaitExec();
|
|
myDesiredPanPos = myPTU.getPan();
|
|
myDesiredTiltPos = myPTU.getTilt();
|
|
myReset = false;
|
|
}
|
|
else // User did nothing, or hit a key that changed myDesiredPanPos, myDesiredTiltPos, or mySlew (so request PTU to move if those changed since last request)
|
|
{
|
|
|
|
// Some PTUs can determine their current position (with encoders, etc) and return that.
|
|
// canGetRealPanTilt() will return true in this case, and getPan() and
|
|
// getTilt() will return those received values. Otherwise, getPan() and
|
|
// getTilt() return the last commanded values. getLastPanRequest() and
|
|
// getLastTiltRequest() will always return the last commanded values sent by
|
|
// ArDPPTU (so in the case that canGetRealPanTilt() is false, getPan() and
|
|
// getTilt() return the same pair of values as getLastPanRequest() and
|
|
// getLastTiltRequest(). ArDPPTU::canGetRealPanTilt() is initialally false,
|
|
// but once the first set of pan and tilt positions is read back from the
|
|
// PTU device, it becomes true.
|
|
if(myPTU.canGetRealPanTilt())
|
|
printf("Position (%.1f deg, %.1f deg) [Incr. %d deg] Press ? for help \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement);
|
|
else
|
|
printf("Requested (%.1f deg, %.1f deg) [Incr. %d deg] Press ? for help \r", myPTU.getPan(), myPTU.getTilt(), myPosIncrement);
|
|
|
|
if (myDesiredPanPos != myPTU.getLastPanRequest())
|
|
{
|
|
if (myAbsolute)
|
|
myPTU.pan(myDesiredPanPos);
|
|
else
|
|
myPTU.panRel(myDesiredPanPos - myPTU.getPan());
|
|
}
|
|
|
|
if (myDesiredTiltPos != myPTU.getLastTiltRequest())
|
|
{
|
|
if (myAbsolute)
|
|
myPTU.tilt(myDesiredTiltPos);
|
|
else
|
|
myPTU.tiltRel(myDesiredTiltPos - myPTU.getTilt());
|
|
}
|
|
|
|
if (mySlew != myPTU.getPanSlew())
|
|
{
|
|
myPTU.panSlew(mySlew);
|
|
myPTU.tiltSlew(mySlew);
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
Aria::init();
|
|
ArArgumentParser parser(&argc, argv);
|
|
parser.loadDefaultArguments();
|
|
ArRobot robot;
|
|
ArRobotConnector robotConnector(&parser, &robot);
|
|
if(!robotConnector.connectRobot())
|
|
{
|
|
ArLog::log(ArLog::Terse, "dpptuExample: Could not connect to the robot.");
|
|
if(parser.checkHelpAndWarnUnparsed())
|
|
{
|
|
Aria::logOptions();
|
|
Aria::exit(1);
|
|
}
|
|
}
|
|
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
|
|
{
|
|
Aria::logOptions();
|
|
Aria::exit(1);
|
|
}
|
|
ArLog::log(ArLog::Normal, "dpptuExample: Connected to robot.");
|
|
|
|
robot.runAsync(true);
|
|
|
|
// an object for keyboard control, class defined above, this also adds itself as a user task
|
|
KeyPTU ptu(&robot);
|
|
|
|
|
|
// turn off the sonar
|
|
robot.comInt(ArCommands::SONAR, 0);
|
|
|
|
printf("Press '?' for available commands\r\n");
|
|
|
|
// run, if we lose connection to the robot, exit
|
|
robot.waitForRunExit();
|
|
|
|
Aria::exit(0);
|
|
}
|
|
|