void Joydrive::drive(void)
{
int trans, rot;
// print out some data about the robot
printf("\rx %6.1f y %6.1f tth %6.1f vel %7.1f mpacs %3d ",
myRobot->getX(), myRobot->getY(), myRobot->getTh(),
myRobot->getVel(), myRobot->getMotorPacCount());
fflush(stdout);
// see if a joystick butotn is pushed, if so drive
if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
myJoyHandler.getButton(2)))
{
// get the values out of the joystick handler
myJoyHandler.getAdjusted(&rot, &trans);
// drive the robot
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
// if a button isn't pushed, stop the robot
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
}
// this is the function called in the new thread
void *Joydrive::runThread(void *arg)
{
threadStarted();
int trans, rot;
// only run while running, ie play nice and pay attention to the thread
//being shutdown
while (myRunning)
{
// lock the robot before touching it
myRobot->lock();
if (!myRobot->isConnected())
{
myRobot->unlock();
break;
}
// print out some information about the robot
printf("\rx %6.1f y %6.1f tth %6.1f vel %7.1f mpacs %3d ",
myRobot->getX(), myRobot->getY(), myRobot->getTh(),
myRobot->getVel(), myRobot->getMotorPacCount());
fflush(stdout);
// if one of the joystick buttons is pushed, drive the robot
if (myJoyHandler.haveJoystick() && (myJoyHandler.getButton(1) ||
myJoyHandler.getButton(2)))
{
// get out the values from the joystick
myJoyHandler.getAdjusted(&rot, &trans);
// drive the robot
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
// if no buttons are pushed stop the robot
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
// unlock the robot, so everything else can run
myRobot->unlock();
// now take a little nap
ArUtil::sleep(50);
}
// return out here, means the thread is done
return NULL;
}