From df36b163cab7dfbdf3f7278f739007b22473cd7c Mon Sep 17 00:00:00 2001 From: Hadiya Date: Mon, 2 Apr 2018 16:26:47 -0600 Subject: [PATCH] Added demofile --- bettybot/demo.cpp | 278 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 bettybot/demo.cpp diff --git a/bettybot/demo.cpp b/bettybot/demo.cpp new file mode 100644 index 0000000..82affa3 --- /dev/null +++ b/bettybot/demo.cpp @@ -0,0 +1,278 @@ +/* +Adept MobileRobots Robotics Interface for Applications (ARIA) +Copyright (C) 2004-2005 ActivMedia Robotics LLC +Copyright (C) 2006-2010 MobileRobots Inc. +Copyright (C) 2011-2015 Adept Technology, Inc. +Copyright (C) 2016-2018 Omron Adept Technologies, Inc. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +If you wish to redistribute ARIA under different terms, contact +Adept MobileRobots for information about a commercial version of ARIA at +robots@mobilerobots.com or +Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960 +*/ +#include "Aria.h" +#include "ArSoundPlayer.h" +#include "ArSoundsQueue.h"; +#include "ariaUtil.h"; +#include "ariaTypedefs.h" +#include ; +#include ; + + +/** @example demo.cpp General purpose testing and demo program, using ArMode + * classes to provide keyboard control of various robot functions. + * + * demo uses ArMode subclasses from ARIA. These modes + * provide keyboard control of various aspects and accessories of + * the robot, and can be re-used in your programs if you wish. + * The ArMode classes are defined in %ArModes.cpp. + * + * "demo" is a useful program for testing out the operation of the robot + * for diagnostic or demonstration purposes. Other example programs + * focus on individual areas. + */ + +bool handleDebugMessage(ArRobotPacket *pkt) +{ + + if(pkt->getID() != ArCommands::MARCDEBUG) return false; + char msg[256]; + pkt->bufToStr(msg, sizeof(msg)); + msg[255] = 0; + ArLog::log(ArLog::Terse, "Controller Firmware Debug: %s", msg); + return true; +} + +void horseMove(ArRobot robot) { +} +void beepTones(ArRobot *robot, const std::vector< std::pair >& tune) +{ + char buf[40]; + size_t p = 0; + for (std::vector< std::pair >::const_iterator i = tune.begin(); i != tune.end() && p < 39; ++i) + { + if ((*i).first > 0) + { + buf[p++] = (char)((*i).first / 20.0); + buf[p++] = (char)(*i).second; + } + } + buf[p++] = 0; + buf[p++] = 0; + robot->comStrN(ArCommands::SAY, buf, p); +} +int main(int argc, char** argv) +{ + // Initialize some global data + Aria::init(); + ArSoundsQueue soundq; + //ArSoundsQueue::Item item = soundq.createDefaultFileItem("C:\\Program Files\\MobileRobots\\Aria\\bettybot\\atmosphere_jungle.wav"); + // If you want ArLog to print "Verbose" level messages uncomment this: + //ArLog::init(ArLog::StdOut, ArLog::Verbose); + + // This object parses program options from the command line + ArArgumentParser parser(&argc, argv); + + // Load some default values for command line arguments from /etc/Aria.args + // (Linux) or the ARIAARGS environment variable. + parser.loadDefaultArguments(); + + // Central object that is an interface to the robot and its integrated + // devices, and which manages control of the robot by the rest of the program. + ArRobot robot; + + // Object that connects to the robot or simulator using program options + ArRobotConnector robotConnector(&parser, &robot); + + // If the robot has an Analog Gyro, this object will activate it, and + // if the robot does not automatically use the gyro to correct heading, + // this object reads data from it and corrects the pose in ArRobot + ArAnalogGyro gyro(&robot); + + robot.addPacketHandler(new ArGlobalRetFunctor1(&handleDebugMessage)); + + // Connect to the robot, get some initial data from it such as type and name, + // and then load parameter files for this robot. + if (!robotConnector.connectRobot()) + { + // Error connecting: + // if the user gave the -help argumentp, then just print out what happened, + // and continue so options can be displayed later. + if (!parser.checkHelpAndWarnUnparsed()) + { + ArLog::log(ArLog::Terse, "Could not connect to robot, will not have parameter file so options displayed later may not include everything"); + } + // otherwise abort + else + { + ArLog::log(ArLog::Terse, "Error, could not connect to robot."); + Aria::logOptions(); + Aria::exit(1); + } + } + + if(!robot.isConnected()) + { + ArLog::log(ArLog::Terse, "Internal error: robot connector succeeded but ArRobot::isConnected() is false!"); + } + + // Connector for laser rangefinders + //ArLaserConnector laserConnector(&parser, &robot, &robotConnector); + + // Connector for compasses + ArCompassConnector compassConnector(&parser); + + // Parse the command line options. Fail and print the help message if the parsing fails + // or if the help was requested with the -help option + if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) + { + Aria::logOptions(); + Aria::exit(1); + return 1; + } + + // Used to access and process sonar range data + ArSonarDevice sonarDev; + + // Used to perform actions when keyboard keys are pressed + ArKeyHandler keyHandler; + Aria::setKeyHandler(&keyHandler); + + // ArRobot contains an exit action for the Escape key. It also + // stores a pointer to the keyhandler so that other parts of the program can + // use the same keyhandler. + robot.attachKeyHandler(&keyHandler); + printf("You may press escape to exit\n"); + + // Attach sonarDev to the robot so it gets data from it. + robot.addRangeDevice(&sonarDev); + + + // Start the robot task loop running in a new background thread. The 'true' argument means if it loses + // connection the task loop stops and the thread exits. + robot.runAsync(true); + + // Connect to the laser(s) if lasers were configured in this robot's parameter + // file or on the command line, and run laser processing thread if applicable + // for that laser class. For the purposes of this demo, add all + // possible lasers to ArRobot's list rather than just the ones that were + // connected by this call so when you enter laser mode, you + // can then interactively choose which laser to use from that list of all + // lasers mentioned in robot parameters and on command line. Normally, + // only connected lasers are put in ArRobot's list. + // if (!laserConnector.connectLasers( + // false, // continue after connection failures + // false, // add only connected lasers to ArRobot + // true // add all lasers to ArRobot + //)) + //{ + // printf("Warning: Could not connect to laser(s). Set LaserAutoConnect to false in this robot's individual parameter file to disable laser connection.\n"); + //} + +/* not needed, robot connector will do it by default + if (!sonarConnector.connectSonars( + false, // continue after connection failures + false, // add only connected lasers to ArRobot + true // add all lasers to ArRobot + )) + { + printf("Could not connect to sonars... exiting\n"); + Aria::exit(2); + } +*/ + + // Create and connect to the compass if the robot has one. + ArTCM2 *compass = compassConnector.create(&robot); + if(compass && !compass->blockingConnect()) { + compass = NULL; + } + + // Sleep for a second so some messages from the initial responses + // from robots and cameras and such can catch up + ArUtil::sleep(1000); + + // We need to lock the robot since we'll be setting up these modes + // while the robot task loop thread is already running, and they + // need to access some shared data in ArRobot. + robot.lock(); + + // now add all the modes for this demo + // these classes are defined in ArModes.cpp in ARIA's source code. + + if(robot.getOrigRobotConfig()->getHasGripper()) + new ArModeGripper(&robot, "gripper", 'g', 'G'); + else + ArLog::log(ArLog::Normal, "Robot does not indicate that it has a gripper."); + ArModeActs actsMode(&robot, "acts", 'a', 'A'); + ArModeTCM2 tcm2(&robot, "tcm2", 'm', 'M', compass); + ArModeIO io(&robot, "io", 'i', 'I'); + ArModeRobotStatus stat(&robot, "detailed status/error flags", 'f', 'F'); + ArModeConfig cfg(&robot, "report robot config", 'o' , 'O'); + ArModeCommand command(&robot, "command", 'd', 'D'); + ArModeCamera camera(&robot, "camera", 'c', 'C'); + ArModePosition position(&robot, "position", 'p', 'P', &gyro); + ArModeSonar sonar(&robot, "sonar", 's', 'S'); + ArModeBumps bumps(&robot, "bumps", 'b', 'B'); + ArModeLaser laser(&robot, "laser", 'l', 'L'); + ArModeWander wander(&robot, "wander", 'w', 'W'); + ArModeUnguardedTeleop unguardedTeleop(&robot, "unguarded teleop", 'u', 'U'); + ArModeTeleop teleop(&robot, "teleop", 't', 'T'); + + + // activate the default mode + teleop.activate(); + + // turn on the motors + robot.comInt(ArCommands::ENABLE, 1); + + robot.unlock(); +// + robot.move(100); + while (!robot.isMoveDone()); + robot.move(-100); + while (!robot.isMoveDone()); + robot.setDeltaHeading(-90); + while (!robot.isHeadingDone()); + robot.move(100); + while (!robot.isMoveDone()); + robot.move(-100); + while (!robot.isMoveDone()); + robot.setDeltaHeading(-90); + while (!robot.isHeadingDone()); + robot.move(100); + while (!robot.isMoveDone()); + robot.move(-100); + while (!robot.isMoveDone()); + robot.setDeltaHeading(-90); + while (!robot.isHeadingDone()); + robot.move(100); + while (!robot.isMoveDone()); + robot.move(-100); + while (!robot.isMoveDone()); + robot.setDeltaHeading(-90); + while (!robot.isHeadingDone()); + robot.move(100); + while (!robot.isMoveDone()); + robot.move(-100); + while (!robot.isMoveDone()); + robot.waitForRunExit(); + + Aria::exit(0); + return 0; + +} +