This is basically just a demo of how to use the sony, but made fancy so you can drive around the robot and look at stuff with the camera. Press down button 1 to drive the robot, button 2 to move the camera, and both buttons to zoom the camera.
#include "Aria.h"
class Joydrive
{
public:
Joydrive(
ArRobot *robot,
int LRAmount = 15,
int UDAmount = 10,
int zoomAmount = 50);
~Joydrive(void) {}
void drive(void);
void up(void);
void down(void);
void left(void);
void right(void);
void in(void);
void out(void);
void center(void);
protected:
int myRotValRobot;
int myTransValRobot;
int myPanValCamera;
int myTiltValCamera;
int myZoomValCamera;
int myLRAmount;
int myUDAmount;
int myZoomAmount;
};
Joydrive::Joydrive(
ArRobot *robot,
int LRAmount,
int UDAmount,
int zoomAmount) :
myCam(robot),
myDriveCB(this, &Joydrive::drive),
myUpCB(this, &Joydrive::up),
myDownCB(this, &Joydrive::down),
myLeftCB(this, &Joydrive::left),
myRightCB(this, &Joydrive::right),
myInCB(this, &Joydrive::in),
myOutCB(this, &Joydrive::out),
myCenterCB(this, &Joydrive::center)
{
myRobot = robot;
myRotValRobot = 100;
myTransValRobot = 700;
myPanValCamera = 8;
myTiltValCamera = 3;
myZoomValCamera = 50;
myLRAmount = LRAmount;
myUDAmount = UDAmount;
myZoomAmount = zoomAmount;
myRobot = robot;
{
myRobot->attachKeyHandler(keyHandler);
}
"The key handler already has a key for left, keydrive will not work correctly.");
"The key handler already has a key for right, keydrive will not work correctly.");
"The key handler already has a key for 'z', keydrive will not work correctly.");
"The key handler already has a key for 'Z', keydrive will not work correctly.");
"The key handler already has a key for 'x', keydrive will not work correctly.");
"The key handler already has a key for 'X', keydrive will not work correctly.");
"The key handler already has a key for 'c', keydrive will not work correctly.");
"The key handler already has a key for 'C', keydrive will not work correctly.");
myJoyHandler.init();
if (myJoyHandler.haveJoystick())
{
printf("Have a joystick\n\n");
}
else
{
printf("Do not have a joystick, only the keyboard will work.\n");
}
}
void Joydrive::left(void)
{
myCam.panTiltRel(-myLRAmount, 0);
}
void Joydrive::right(void)
{
myCam.panTiltRel(myLRAmount, 0);
}
void Joydrive::up(void)
{
myCam.panTiltRel(0, myUDAmount);
}
void Joydrive::down(void)
{
myCam.panTiltRel(0, -myUDAmount);
}
void Joydrive::in(void)
{
myCam.zoomRel(myZoomAmount);
}
void Joydrive::out(void)
{
myCam.zoomRel(-myZoomAmount);
}
void Joydrive::center(void)
{
myCam.panTilt(0,0);
myCam.zoom(0);
}
void Joydrive::drive(void)
{
int trans, rot;
int pan, tilt;
int zoom, nothing;
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1) &&
myJoyHandler.getButton(2))
{
myJoyHandler.setSpeeds(0, myZoomValCamera);
myJoyHandler.getAdjusted(¬hing, &zoom);
myCam.zoomRel(zoom);
}
else
{
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
{
myJoyHandler.setSpeeds(myRotValRobot, myTransValRobot);
myJoyHandler.getAdjusted(&rot, &trans);
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2))
{
myJoyHandler.setSpeeds(myPanValCamera, myTiltValCamera);
myJoyHandler.getAdjusted(&pan, &tilt);
myCam.panTiltRel(pan, tilt);
}
}
}
int main(int argc, char **argv)
{
std::string str;
int ret;
Joydrive joyd(&robot);
if ((ret = con.
open()) != 0)
{
str = con.
getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
return 1;
}
{
printf("Could not connect to robot... exiting\n");
return 1;
}
return 0;
}