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