|
I’m having problems with a simple program that was
intended to move a robot through 4 waypoints, avoiding any obstacles that may
be in the way. Everything works well, except that the robot does not go to
the last waypoint (last side of the square path). I’ve tried changing the last waypoint from (0, 0) to (6000,
6000) or (6000, 0) with no success. However, if the last waypoint is
changed to (0, 9000), the robot moves to that position as expected. Any suggestions to what the problem might be? I’ve
tried this with MobileSim and a P3AT with identical results. Here is the code: #include "Aria.h" /* This program will have the robot move in a square, with a
24 meters perimeter, using object avoidance routines. When no objects are
present, the robot will move at a constant velocity. You can press escape while it is running to cause the
program to stop and exit. WARNING: as it is, the robot is not completing the last side of
the square. */ int main(int argc, char **argv) { // robot ArRobot robot; // sonar, must be added to the robot ArSonarDevice sonar; // the actions we'll use ArActionStallRecover
recover; ArActionBumpers
bumpers; ArActionAvoidFront
avoidFrontNear("Avoid Front Near", 1000, 100, 15); // goto the goal ArActionGoto
gotoPose1("goto", ArPose(6000, 0)); ArActionGoto
gotoPose2("goto", ArPose(6000, 6000)); ArActionGoto
gotoPose3("goto", ArPose(0, 6000)); ArActionGoto
gotoPose4("goto", ArPose(0, 0)); // Make a key handler, so that escape will shut down the
program // cleanly ArKeyHandler
keyHandler; // mandatory init Aria::init(); ArSimpleConnector
connector(&argc, argv); if (!connector.parseArgs() || argc > 1) {
connector.logOptions(); exit(1); } printf("This
program will make the robot wander around, avoiding obstacles.\nPress escape to
exit.\n"); // Add the key handler to Aria so other things can find it
Aria::setKeyHandler(&keyHandler); // Attach the key handler to a robot now, so that it
actually gets // some processing time so it can work, this will also make
escape // exit robot.attachKeyHandler(&keyHandler); // add the sonar to the robot
robot.addRangeDevice(&sonar); // try to connect, if we fail exit if (!connector.connectRobot(&robot)) {
printf("Could not connect to robot... exiting\n");
Aria::shutdown(); return 1; } // turn on the motors, turn off amigobot sounds
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0); // add the actions
robot.addAction(&recover, 100);
robot.addAction(&bumpers, 75);
robot.addAction(&avoidFrontNear, 51);
robot.addAction(&gotoPose1, 50);
robot.addAction(&gotoPose2, 49);
robot.addAction(&gotoPose3, 48);
robot.addAction(&gotoPose4, 47); // start the robot running, true so that if we lose
connection the run stops robot.run(true); // now exit Aria::shutdown(); return 0; } Thanks, Mark |