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); } }
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); } } }
// 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; }
void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg) { veltime = ros::Time::now(); #ifdef SPEW ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() ); #endif robot->lock(); robot->setVel(msg->linear.x*1e3); robot->setRotVel(msg->angular.z*180/M_PI); robot->unlock(); ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(), (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI); }
void RosAriaNode::spin() { //ros::spin(); while(ros::ok()){ if(ros::WallTime::now()-last_command_time_ > ros::WallDuration(1)){ robot->lock(); robot->setVel(0.0); if(robot->hasLatVel()) robot->setLatVel(0.0); robot->setRotVel(0.0); robot->unlock(); } ros::spinOnce(); } }
void setMotors(ArRobot& robot){ TiObj G_motor; G_motor.loadFile("../motors/motors"); if ( G_motor.has("speed") || G_motor.has("rotation") ){ cout << G_motor; robot.lock(); if ( G_motor.has("speed") ) robot.setVel( G_motor.atInt("speed") ); if ( G_motor.has("rotation") ) robot.setRotVel( G_motor.atInt("rotation") ); robot.unlock(); FILE* fd = fopen("../motors/motors","w"); fclose(fd); } // }
void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg) { veltime = ros::WallTime::now(); last_command_time_=veltime; ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() ); twistMsg.linear.x=msg->linear.x; twistMsg.angular.z=msg->angular.z; twist_pub.publish(twistMsg); robot->lock(); robot->setVel(msg->linear.x*1e3); if(robot->hasLatVel()) robot->setLatVel(msg->linear.y*1e3); robot->setRotVel(msg->angular.z*180/M_PI); robot->unlock(); ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(), (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI); }
void manualControlHandler(){ if(firstCall){ firstCall=false; showMenu(); } if(keyPressedBefore && !driver->estaEjecutando()){ keyPressedBefore=false; showMenu(); } if(mrpt::system::os::kbhit() ){ char c = mrpt::system::os::getch(); keyPressedBefore=true; switch(c){ case '\033': c=mrpt::system::os::getch(); // skip the [ cout << "Siguiente caracter" << (int)c << endl; double v; switch(mrpt::system::os::getch()) { // the real value case 'A': // code for arrow up v=robot->getVel(); robot->setVel(v+50); break; case 'B': // code for arrow down v=robot->getVel(); robot->setVel(v-50); break; case 'C': // code for arrow right v=robot->getRotVel(); robot->setRotVel(v-5); break; case 'D': // code for arrow left v=robot->getRotVel(); robot->setRotVel(v+5); break; } break; case ' ': robot->stop(); break; case 'x': //Aria::shutdown(); driver->stopRunning(); robot->stopRunning(); break; case 'c': guardarContinuo(); break; case 'p': start(); break; case 'g': guardar(); break; case 'w': guardarTrayectoria(); break; case 't': testParado(); break; case 's': stop(); break; } } }
void PeoplebotTest::userTask(void) { switch (myState) { case IDLE: // start wandering printf("Starting to wander for the first time\n"); myStateTime.setToNow(); myState = WANDERING; myRobot->addAction(myConstantVelocity, 25); printf("Opening up ACTS\n"); myCmd = "DISPLAY="; myCmd += myHostname.c_str(); myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &"; system(myCmd.c_str()); break; case WANDERING: if (timeout(myWanderingTimeout)) { myRobot->comInt(ArCommands::SONAR,0); myRobot->remAction(myConstantVelocity); myRobot->setVel(0); myRobot->setRotVel(0); myState = RESTING; mySonar = 0; printf("Going to rest now\n"); printf("Killing ACTS\n"); system("killall -9 acts &> /dev/null"); myStateTime.setToNow(); myTotalWanderTime += myWanderingTimeout; } else if (myRobot->getVel() > 0 && mySonar != 1) { // ping front sonar //printf("Enabling front sonar\n"); mySonar = 1; myRobot->comInt(ArCommands::SONAR, 0); myRobot->comInt(ArCommands::SONAR, 1); myRobot->comInt(ArCommands::SONAR, 4); } else if (myRobot->getVel() < 0 && mySonar != -1) { // ping rear sonar //printf("Enabling rear sonar\n"); mySonar = -1; myRobot->comInt(ArCommands::SONAR, 0); myRobot->comInt(ArCommands::SONAR, 5); } break; case RESTING: if (timeout(myRestingTimeout)) { printf("Going to wander now\n"); myState = WANDERING; myStateTime.setToNow(); myTotalRestTime += myRestingTimeout; myRobot->clearDirectMotion(); myRobot->addAction(myConstantVelocity, 25); printf("Opening up ACTS\n"); myCmd = "DISPLAY="; myCmd += myHostname.c_str(); myCmd += ":0; /usr/local/acts/bin/acts -G bttv -n 0 &> /dev/null &"; system(myCmd.c_str()); } break; case OTHER: default: break; }; }
int main(int argc, char **argv) { Aria::init(); ArRobot robot; ArArgumentParser parser(&argc, argv); parser.loadDefaultArguments(); ArLog::log(ArLog::Terse, "WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has approximately 3 meters of free space on all sides."); // ArRobotConnector connects to the robot, get some initial data from it such as type and name, // and then loads parameter files for this robot. ArRobotConnector robotConnector(&parser, &robot); if(!robotConnector.connectRobot()) { ArLog::log(ArLog::Terse, "simpleMotionCommands: Could not connect to the robot."); if(parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(1); return 1; } } if (!Aria::parseArgs()) { Aria::logOptions(); Aria::exit(1); return 1; } ArLog::log(ArLog::Normal, "simpleMotionCommands: Connected."); // Start the robot processing cycle running in the background. // True parameter means that if the connection is lost, then the // run loop ends. robot.runAsync(true); // Print out some data from the SIP. // We must "lock" the ArRobot object // before calling its methods, and "unlock" when done, to prevent conflicts // with the background thread started by the call to robot.runAsync() above. // See the section on threading in the manual for more about this. // Make sure you unlock before any sleep() call or any other code that will // take some time; if the robot remains locked during that time, then // ArRobot's background thread will be blocked and unable to communicate with // the robot, call tasks, etc. robot.lock(); ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage()); robot.unlock(); // Sleep for 3 seconds. ArLog::log(ArLog::Normal, "simpleMotionCommands: Will start driving in 3 seconds..."); ArUtil::sleep(3000); // Set forward velocity to 50 mm/s ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 250 mm/s for 5 sec..."); robot.lock(); robot.enableMotors(); robot.setVel(250); robot.unlock(); ArUtil::sleep(5000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping."); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(1000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at 10 deg/s for 5 sec..."); robot.lock(); robot.setRotVel(10); robot.unlock(); ArUtil::sleep(5000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Rotating at -10 deg/s for 10 sec..."); robot.lock(); robot.setRotVel(-10); robot.unlock(); ArUtil::sleep(10000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Driving forward at 150 mm/s for 5 sec..."); robot.lock(); robot.setRotVel(0); robot.setVel(150); robot.unlock(); ArUtil::sleep(5000); ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping."); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(1000); // Other motion command functions include move(), setHeading(), // setDeltaHeading(). You can also adjust acceleration and deceleration // values used by the robot with setAccel(), setDecel(), setRotAccel(), // setRotDecel(). See the ArRobot class documentation for more. robot.lock(); ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV", robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage()); robot.unlock(); ArLog::log(ArLog::Normal, "simpleMotionCommands: Ending robot thread..."); robot.stopRunning(); // wait for the thread to stop robot.waitForRunExit(); // exit ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting."); Aria::exit(0); return 0; }
int main(int argc, char **argv) { bool done; double distToTravel = 2300; // whether to use the sim for the laser or not, if you use the sim // for hte laser, you have to use the sim for the robot too bool useSim = false; // the laser ArSick sick; // connection ArDeviceConnection *con; // Laser connection ArSerialConnection laserCon; // robot ArRobot robot; // set a default filename //std::string filename = "c:\\log\\1scans.2d"; std::string filename = "1scans.2d"; // see if we want to use a different filename //if (argc > 1) //Lfilename = argv[1]; printf("Logging to file %s\n", filename.c_str()); // start the logger with good values sick.configureShort(useSim, ArSick::BAUD38400, ArSick::DEGREES180, ArSick::INCREMENT_HALF); ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str()); // mandatory init Aria::init(); // add it to the robot robot.addRangeDevice(&sick); //ArAnalogGyro gyro(&robot); // if we're not using the sim, make a serial connection and set it up if (!useSim) { ArSerialConnection *serCon; serCon = new ArSerialConnection; serCon->setPort(); //serCon->setBaud(38400); con = serCon; } // if we are using the sim, set up a tcp connection else { ArTcpConnection *tcpCon; tcpCon = new ArTcpConnection; tcpCon->setPort(); con = tcpCon; } // set the connection on the robot robot.setDeviceConnection(con); // try to connect, if we fail exit if (!robot.blockingConnect()) { printf("Could not connect to robot... exiting\n"); Aria::shutdown(); return 1; } // set up a key handler so escape exits and attach to the robot ArKeyHandler keyHandler; robot.attachKeyHandler(&keyHandler); // run the robot, true here so that the run will exit if connection lost robot.runAsync(true); // if we're not using the sim, set up the port for the laser if (!useSim) { laserCon.setPort(ArUtil::COM3); sick.setDeviceConnection(&laserCon); } // now that we're connected to the robot, connect to the laser sick.runAsync(); if (!sick.blockingConnect()) { printf("Could not connect to SICK laser... exiting\n"); robot.disconnect(); Aria::shutdown(); return 1; } #ifdef WIN32 // wait until someone pushes the motor button to go while (1) { robot.lock(); if (!robot.isRunning()) exit(0); if (robot.areMotorsEnabled()) { robot.unlock(); break; } robot.unlock(); ArUtil::sleep(100); } #endif // basically from here on down the robot just cruises around a bit robot.lock(); // enable the motors, disable amigobot sounds robot.comInt(ArCommands::ENABLE, 1); // move a couple meters robot.setRotVel(3000); robot.unlock(); ArUtil::sleep(15 * 1000); robot.lock(); robot.disconnect(); robot.unlock(); keyHandler.restore(); exit(1); // now exit return 0; }
int main(int argc, char **argv) { Aria::init(); ArArgumentParser argParser(&argc, argv); ArSimpleConnector con(&argParser); ArRobot robot; // the connection handler from above ConnHandler ch(&robot); if(!Aria::parseArgs()) { Aria::logOptions(); Aria::shutdown(); return 1; } if(!con.connectRobot(&robot)) { ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting."); return 1; } ArLog::log(ArLog::Normal, "directMotionExample: Connected."); // Run the robot processing cycle in its own thread. Note that after starting this // thread, we must lock and unlock the ArRobot object before and after // accessing it. robot.runAsync(false); // Send the robot a series of motion commands directly, sleeping for a // few seconds afterwards to give the robot time to execute them. printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n"); robot.lock(); robot.setRotVel(100); robot.unlock(); ArUtil::sleep(3*1000); printf("Stopping\n"); robot.lock(); robot.setRotVel(0); robot.unlock(); ArUtil::sleep(200); printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n"); robot.lock(); robot.setVel2(300, 100); robot.unlock(); ArTime start; start.setToNow(); while (1) { robot.lock(); if (start.mSecSince() > 5000) { robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n"); robot.lock(); robot.move(1000); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isMoveDone()) { printf("directMotionExample: Finished distance\n"); robot.unlock(); break; } if (start.mSecSince() > 5000) { printf("directMotionExample: Distance timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n"); robot.lock(); robot.move(-1000); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isMoveDone()) { printf("directMotionExample: Finished distance\n"); robot.unlock(); break; } if (start.mSecSince() > 10000) { printf("directMotionExample: Distance timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(50); } printf("directMotionExample: Telling the robot to turn to 180, then sleeping 4 seconds\n"); robot.lock(); robot.setHeading(180); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isHeadingDone(5)) { printf("directMotionExample: Finished turn\n"); robot.unlock(); break; } if (start.mSecSince() > 5000) { printf("directMotionExample: Turn timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(100); } printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n"); robot.lock(); robot.setHeading(90); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (robot.isHeadingDone(5)) { printf("directMotionExample: Finished turn\n"); robot.unlock(); break; } if (start.mSecSince() > 5000) { printf("directMotionExample: turn timed out\n"); robot.unlock(); break; } robot.unlock(); ArUtil::sleep(100); } printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n"); robot.lock(); robot.setVel2(200, 200); robot.unlock(); ArUtil::sleep(3000); printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n"); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(2000); printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n"); robot.lock(); robot.setVel(200); robot.unlock(); ArUtil::sleep(3000); printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n"); robot.lock(); robot.stop(); robot.unlock(); ArUtil::sleep(2000); printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n"); robot.lock(); robot.setVel2(0, 200); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n"); robot.lock(); robot.setRotVel(50); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n"); robot.lock(); robot.setRotVel(-50); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n"); robot.lock(); robot.setVel2(0, 0); robot.unlock(); ArUtil::sleep(3000); printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n"); robot.lock(); robot.setDeltaHeading(-125); robot.unlock(); ArUtil::sleep(6000); printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n"); robot.lock(); robot.setDeltaHeading(45); robot.unlock(); ArUtil::sleep(6000); printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n"); robot.lock(); robot.setVel2(200, 0); robot.unlock(); ArUtil::sleep(5000); printf("directMotionExample: Done, shutting down Aria and exiting.\n"); Aria::shutdown(); return 0; }
int main(int argc, char **argv) { std::string str; int ret; ArTime start; // connection to the robot ArSerialConnection con; // the robot ArRobot robot; // the connection handler from above ConnHandler ch(&robot); // init area with a dedicated signal handling thread Aria::init(Aria::SIGHANDLE_THREAD); // open the connection with the defaults, exit if failed if ((ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); Aria::shutdown(); return 1; } // set the robots connection robot.setDeviceConnection(&con); // try to connect, if we fail, the connection handler should bail if (!robot.blockingConnect()) { // this should have been taken care of by the connection handler // but just in case printf( "asyncConnect failed because robot is not running in its own thread.\n"); Aria::shutdown(); return 1; } // run the robot in its own thread, so it gets and processes packets and such robot.runAsync(false); // just a big long set of printfs, direct motion commands and sleeps, // it should be self-explanatory printf("Telling the robot to go 300 mm for 5 seconds\n"); robot.lock(); robot.setVel(500); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (start.mSecSince() > 5000) { robot.unlock(); break; } printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel()); robot.unlock(); ArUtil::sleep(100); } printf("Telling the robot to turn at 50 deg/sec for 10 seconds\n"); robot.lock(); robot.setVel(0); robot.setRotVel(50); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (start.mSecSince() > 10000) { robot.unlock(); break; } printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel()); robot.unlock(); ArUtil::sleep(100); } printf("Telling the robot to turn at 100 deg/sec for 10 seconds\n"); robot.lock(); robot.setVel(0); robot.setRotVel(100); robot.unlock(); start.setToNow(); while (1) { robot.lock(); if (start.mSecSince() > 10000) { robot.unlock(); break; } printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel()); robot.unlock(); ArUtil::sleep(100); } printf("Done with tests, exiting\n"); robot.disconnect(); // shutdown and ge tout Aria::shutdown(); return 0; }
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; } } }