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); } } }
// whether the joystick is there or not bool JoydriveAction::joystickInited(void) { return myJoyHandler.haveJoystick(); }
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); } }
void Joydrive::drive(void) { int trans, rot; ArPose pose; ArPose rpose; ArTransform transform; ArRangeDevice *dev; ArSensorReading *son; if (!myRobot->isConnected()) { printf("Lost connection to the robot, exiting\n"); exit(0); } printf("\rx %6.1f y %6.1f th %6.1f", myRobot->getX(), myRobot->getY(), myRobot->getTh()); fflush(stdout); if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1)) { if (ArMath::fabs(myRobot->getVel()) < 10.0) myRobot->comInt(ArCommands::ENABLE, 1); myJoyHandler.getAdjusted(&rot, &trans); myRobot->setVel(trans); myRobot->setRotVel(-rot); } else { myRobot->setVel(0); myRobot->setRotVel(0); } if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) && time(NULL) - myLastPress > 1) { myLastPress = time(NULL); printf("\n"); switch (myTest) { case 1: printf("Moving back to the origin.\n"); pose.setPose(0, 0, 0); myRobot->moveTo(pose); break; case 2: printf("Moving over a meter.\n"); pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0); myRobot->moveTo(pose); break; case 3: printf("Doing a transform test....\n"); printf("\nOrigin should be transformed to the robots coords.\n"); transform = myRobot->getToGlobalTransform(); pose.setPose(0, 0, 0); pose = transform.doTransform(pose); rpose = myRobot->getPose(); printf("Pos: "); pose.log(); printf("Robot: "); rpose.log(); if (pose.findDistanceTo(rpose) < .1) printf("Success\n"); else printf("#### FAILURE\n"); printf("\nRobot coords should be transformed to the origin.\n"); transform = myRobot->getToLocalTransform(); pose = myRobot->getPose(); pose = transform.doTransform(pose); rpose.setPose(0, 0, 0); printf("Pos: "); pose.log(); printf("Robot: "); rpose.log(); if (pose.findDistanceTo(rpose) < .1) printf("Success\n"); else printf("#### FAILURE\n"); break; case 4: printf("Doing a tranform test...\n"); printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n"); transform = myRobot->getToGlobalTransform(); pose.setPose(-1000, 0, 0); pose = transform.doTransform(pose); rpose = myRobot->getPose(); printf("Pos: "); pose.log(); printf("Robot: "); rpose.log(); if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1) printf("Probable Success\n"); else printf("#### FAILURE\n"); break; case 5: printf("Doing a transform test on range devices..\n"); printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n"); dev = myRobot->findRangeDevice("sonar"); if (dev == NULL) { printf("No sonar on the robot, can't do the test.\n"); break; } printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0)); printf("Sonar 0 reading is at "); son = myRobot->getSonarReading(0); if (son != NULL) { pose = son->getPose(); pose.log(); } pose = myRobot->getPose(); pose.setX(pose.getX() + 4000); pose.setY(pose.getY() + 4000); myRobot->moveTo(pose); printf("Moved robot.\n"); printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0)); printf("Sonar 0 reading is at "); son = myRobot->getSonarReading(0); if (son != NULL) { pose = son->getPose(); pose.log(); } break; case 6: printf("Robot position now is:\n"); pose = myRobot->getPose(); pose.log(); printf("Disconnecting from the robot, then reconnecting.\n"); myRobot->disconnect(); myRobot->blockingConnect(); printf("Robot position now is:\n"); pose = myRobot->getPose(); pose.log(); break; default: printf("No test for second button.\n"); break; } } }