void Joydrive::drive(void)
{
int trans, rot;
int pan, tilt;
int zoom, nothing;
// if both buttons are pushed, zoom the joystick
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1) &&
myJoyHandler.getButton(2))
{
// set its speed so we get desired value range, we only care about y
myJoyHandler.setSpeeds(0, myZoomValCamera);
// get the values
myJoyHandler.getAdjusted(¬hing, &zoom);
// zoom the camera
myCam.zoomRel(zoom);
}
// if both buttons aren't pushed, see if one button is pushed
else
{
// if button one is pushed, drive the robot
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
{
// set the speed on the joystick so we get the values we want
myJoyHandler.setSpeeds(myRotValRobot, myTransValRobot);
// get the values
myJoyHandler.getAdjusted(&rot, &trans);
// set the robots speed
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
// if button one isn't pushed, stop the robot
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
// if button two is pushed, move the camera
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2))
{
// set the speeds on the joystick so we get desired value range
myJoyHandler.setSpeeds(myPanValCamera, myTiltValCamera);
// get the values
myJoyHandler.getAdjusted(&pan, &tilt);
// drive the camera
myCam.panTiltRel(pan, tilt);
}
}
}
int main(int argc, char **argv)
{
std::string str;
int ret;
time_t lastTime;
int trans, rot;
ArJoyHandler joyHandler;
ArSerialConnection con;
ArRobot robot(NULL, false);
joyHandler.init();
joyHandler.setSpeeds(100, 700);
if (joyHandler.haveJoystick())
{
printf("Have a joystick\n\n");
}
else
{
printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
// exit(0);
}
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
exit(0);
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot.comInt(ArCommands::ENCODER, 1);
//robot.comStrN(ArCommands::SAY, "\1\6\2\105", 4);
lastTime = time(NULL);
while (1)
{
if (!robot.isConnected())
{
printf("No longer connected to robot, exiting.\n");
exit(0);
}
robot.loopOnce();
if (lastTime != time(NULL))
{
printf("\rx %6.1f y %6.1f tth %6.1f vel %7.1f mpacs %3d", robot.getX(),
robot.getY(), robot.getTh(), robot.getVel(),
robot.getMotorPacCount());
fflush(stdout);
lastTime = time(NULL);
}
if (joyHandler.haveJoystick() && (joyHandler.getButton(1) ||
joyHandler.getButton(2)))
{
if (ArMath::fabs(robot.getVel()) < 10.0)
robot.comInt(ArCommands::ENABLE, 1);
joyHandler.getAdjusted(&rot, &trans);
robot.comInt(ArCommands::VEL, trans);
robot.comInt(ArCommands::RVEL, -rot);
}
else
{
robot.comInt(ArCommands::VEL, 0);
robot.comInt(ArCommands::RVEL, 0);
}
ArUtil::sleep(100);
}
}
