source: libs/libSamgar/examples/SamgarMove.cpp @ 445

Revision 445, 5.0 KB checked in by lmalek, 10 years ago (diff)

Samgar edition by WRUT. Based on Qt. Initial import.

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("Move","MoveRobot","Motion",Samgar::ModeInterupt);
101  IntReciver.AddPortS("In"); //adding in port for recieving motion commands from CMION
102  int myint = 0;
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      if (IntReciver.getCurrentState() == Samgar::StateRunning)
146      {
147      //robot.lock();
148
149          if(IntReciver.GetBottleData("In",&bot)==true)
150          {
151              int vel = bot.get(0).asInt();
152              double turn = bot.get(1).asInt()/5; //divide by 5 to avoid large turning angle
153              printf("data %s \n",bot.toString().c_str());
154              std::cout<< "turn " << turn << std::endl;
155#if ARIA
156              robot.setVel(vel); //set velocity
157              robot.setDeltaHeading(turn);
158#endif
159          }
160          else
161          {
162              // this will never be printed proving the module sleeps after each new input
163              //printf("no data \n");
164              //robot.setVel(0);
165              //robot.setDeltaHeading(0);
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      }
183      IntReciver.SucceedFail(true,myint);
184
185      //    bot.clear();
186      //    yarp::os::Time::delay(0.2);
187    }
188
189#if ARIA
190  robot.stopRunning();
191
192  // wait for the thread to stop
193  robot.waitForRunExit();
194
195  Aria::shutdown();
196#endif
197  return 0;
198}
199
Note: See TracBrowser for help on using the repository browser.