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; }