source: libs/SamgarWRUTed/tags/1.0/libSamgar/examples/SamgarMotion.cpp @ 447

Revision 447, 4.7 KB checked in by lmalek, 10 years ago (diff)

tree structure reorganization

Line 
1//----------------------------------------------
2// Heriot-Watt University
3// MACS
4// www.lirec.eu
5// author: Amol Deshmukh
6// Date: 10/03/2010
7//-----------------------------------------------
8
9#define ARIA 0
10#if ARIA
11#include "Aria.h"
12#else
13typedef void* ArRobot;
14#endif
15#include <Samgar.h>
16
17/*
18 * This is a connection handler class, to demonstrate how to run code in
19 * response to events such as the program connecting an disconnecting
20 * from the robot.
21 */
22class ConnHandler
23{
24public:
25  // Constructor
26  ConnHandler(ArRobot *robot);
27  // Destructor, its just empty
28  ~ConnHandler(void) {}
29  // to be called if the connection was made
30  void connected(void);
31  // to call if the connection failed
32  void connFail(void);
33  // to be called if the connection was lost
34  void disconnected(void);
35protected:
36#if ARIA
37  // robot pointer
38  ArRobot *myRobot;
39  // the functor callbacks
40  ArFunctorC<ConnHandler> myConnectedCB;
41  ArFunctorC<ConnHandler> myConnFailCB;
42  ArFunctorC<ConnHandler> myDisconnectedCB;
43#endif
44};
45
46ConnHandler::ConnHandler(ArRobot *robot)
47#if ARIA
48:
49  myConnectedCB(this, &ConnHandler::connected), 
50  myConnFailCB(this, &ConnHandler::connFail),
51  myDisconnectedCB(this, &ConnHandler::disconnected)
52#endif
53
54{
55#if ARIA
56  myRobot = robot;
57  myRobot->addConnectCB(&myConnectedCB, ArListPos::FIRST);
58  myRobot->addFailedConnectCB(&myConnFailCB, ArListPos::FIRST);
59  myRobot->addDisconnectNormallyCB(&myDisconnectedCB, ArListPos::FIRST);
60  myRobot->addDisconnectOnErrorCB(&myDisconnectedCB, ArListPos::FIRST);
61#endif
62}
63
64// just exit if the connection failed
65void ConnHandler::connFail(void)
66{
67  printf("directMotionDemo connection handler: Failed to connect.\n");
68#if ARIA
69  myRobot->stopRunning();
70  Aria::shutdown();
71#endif
72  return;
73}
74
75// turn on motors, and off sonar, and off amigobot sounds, when connected
76void ConnHandler::connected(void)
77{
78  printf("directMotionDemo connection handler: Connected\n");
79#if ARIA
80  myRobot->comInt(ArCommands::SONAR, 0);
81  myRobot->comInt(ArCommands::ENABLE, 1);
82  myRobot->comInt(ArCommands::SOUNDTOG, 1);
83#endif
84}
85
86// lost connection, so just exit
87void ConnHandler::disconnected(void)
88{
89  printf("directMotionDemo connection handler: Lost connection, exiting program.\n");
90  exit(0);
91}
92
93Bottle bot;
94
95
96int main(int argc, char** argv)
97{
98  //setting yarp connection
99  Network yarp;
100  Samgar::SamgarModule IntReciver("Motion","Motion","Motion",Samgar::ModeInterupt);
101  IntReciver.AddPortS("In"); //adding in port for recieving motion commands from CMION
102
103  //Please not there is no obstacle avoidance in this program so be careful
104
105#if ARIA
106  //Aria configuration
107  Aria::init();
108 
109  ArArgumentParser argParser(&argc, argv);
110
111  ArSimpleConnector con(&argParser);
112
113  argParser.loadDefaultArguments();
114
115  ArRobot robot;
116
117  // the connection handler from class
118  ConnHandler ch(&robot);
119
120  if(!Aria::parseArgs())
121    {
122      Aria::logOptions();
123      Aria::shutdown();
124      return 1;
125    }
126
127  if(!con.connectRobot(&robot))
128    {
129      ArLog::log(ArLog::Normal, "MotionExample: Could not connect to the robot. Exiting.");
130      return 1;
131    }
132
133  yarp::os::Time::delay(2);
134  ArLog::log(ArLog::Normal, "MotionExample: Connected.");
135
136 
137  robot.setAbsoluteMaxRotVel(10);
138  robot.setAbsoluteMaxTransVel(200);
139  robot.runAsync(false);
140
141#endif
142
143  while( 1 )
144    {
145         
146      //robot.lock();
147
148      if(IntReciver.GetBottleData("In",&bot)==true)
149        {
150          int vel = bot.get(0).asInt();
151          double turn = bot.get(1).asInt()/5; //divide by 5 to avoid large turning angle
152          printf("data %s \n",bot.toString().c_str());
153          std::cout<< "turn " << turn << std::endl;
154#if ARIA
155          robot.setVel(vel); //set velocity
156          robot.setDeltaHeading(turn);
157#endif
158        }
159      else
160        {
161          // this will never be printed proving the module sleeps after each new input
162          //printf("no data \n");
163          //robot.setVel(0);
164          //robot.setDeltaHeading(0);
165        }
166
167      // Make sure you unlock before any sleep() call or any other
168      // code that will take some time; if the robot remains locked
169      // during that time, then ArRobot's background thread will be
170      // blocked and unable to communicate with the robot, call
171      // tasks, etc.
172                 
173      //robot.unlock();
174
175      // in an interrupt module this function call has added
176      // significance, the primary being the true or false whether
177      // it has accomplished its task,  and secondly a variable
178      // which could be how well it has acheived the task. This data
179      // is sent on to the GUI so the success rate of the modules
180      // can be deduced
181                 
182      //IntReciver.SucceedFail(true,myint);
183
184      bot.clear();
185      yarp::os::Time::delay(0.2);
186    }
187
188#if ARIA
189  robot.stopRunning();
190
191  // wait for the thread to stop
192  robot.waitForRunExit();
193
194  Aria::shutdown();
195#endif
196  return 0;
197}
198
Note: See TracBrowser for help on using the repository browser.