source: level2/competencies/RobotHouse/VirtualRobot.h @ 527

Revision 527, 3.8 KB checked in by KDucasse, 10 years ago (diff)
RevLine 
[527]1/*
2
3        Vrobot
4
5        The idea here is to send the priority then in couples of three the time wanted,liner vel and rot vel,
6        so if we send 1,1,500,90;
7        it'll be priority 1, set forward vel to 500mm and rot vel to 90 and stay that way for one secound
8        it sends the odo data out at the same time
9        also has a clause that if any of the front bumpers are hit it will stop until the bumper is no longer in contact
10
11
12*/
13
14
15//#include "SamClass.h"
16#include <mrpt/slam.h>
17#include <mrpt/hwdrivers/CHokuyoURG.h>
18#include <mrpt/hwdrivers/CSerialPort.h>
19#include <mrpt/hwdrivers/CActivMediaRobotBase.h>
20#include <mrpt/gui.h>
21
22using namespace mrpt::hwdrivers;
23using namespace mrpt::utils;
24using namespace mrpt::math;
25using namespace mrpt::slam;
26using namespace mrpt::poses;
27using namespace std;
28
29
30
31
32
33
34
35class VRobot: public SamClass
36{
37private:
38BufferedPort<Bottle> PortIn;                           
39BufferedPort<Bottle> OdoOut;
40BufferedPort<Bottle> StarIn;
41double TimeNeedid;
42CActivMediaRobotBase::TRobotDescription  robInfo;
43CActivMediaRobotBase    *robot;
44Bottle ListOfMoves;
45int WhereInList;
46Network yarp;
47int currentpriority;
48public:
49
50        void SamInit(void)
51        {
52                robot = new CActivMediaRobotBase;
53                robot->setSerialPortConfig("COM1",9600);
54                robot->enableSonars();
55                robot->initialize();
56               
57               
58                robot->getRobotInformation(robInfo);
59                cout << "Robot # front bumpers : " << robInfo.nFrontBumpers << endl;
60                cout << "Robot # rear bumpers  : " << robInfo.nRearBumpers << endl;
61                cout << "Robot # sonars        : " << robInfo.nSonars << endl;
62
63                ListOfMoves.addInt(0); // set the current priority to nothing
64                WhereInList=1;
65
66
67                RecognisePort("CMDin");
68                RecognisePort("ODOout");
69                RecognisePort("StarIn");
70                StartModule("/Robot");
71
72
73               
74                PortIn.open("/Robot_CMDin");                           
75                OdoOut.open("/Robot_ODOout");
76                StarIn.open("/Robot_StarIn");
77
78                PortIn.setReporter(myPortStatus);                               
79                OdoOut.setReporter(myPortStatus);
80                StarIn.setReporter(myPortStatus);
81               
82        }
83       
84       
85
86
87       
88        void SamIter(void)
89        {
90                       
91                //robot->setVelocities(200,0);
92                robot->doProcess();
93
94                Bottle *b = PortIn.read(false);
95                if(b!=NULL) // if the new bottle has higher priority then
96                {
97                        puts("robot got new commands");
98                        if(b->get(0).asInt()>=currentpriority){ListOfMoves.clear();ListOfMoves = *b;currentpriority = b->get(0).asInt();WhereInList=1;}
99                }
100
101                Bottle *c = StarIn.read(false);
102                if(c!=NULL)
103                {
104                        robot->changeOdometry(CPose2D(c->get(0).asDouble(),c->get(1).asDouble(),c->get(2).asDouble()));
105                }
106
107                if(yarp::os::Time::now()>TimeNeedid && WhereInList<ListOfMoves.size()) // if its reached the time and theres still more commands
108                {
109                TimeNeedid = yarp::os::Time::now() + ListOfMoves.get(WhereInList).asDouble(); // set the amount of time
110                robot->setVelocities(ListOfMoves.get(WhereInList+1).asDouble(),DEG2RAD(ListOfMoves.get(WhereInList+2).asDouble()));////(WhereInList+1,WhereInList+2);
111        //      printf("command recived speed %f rot %f \n",ListOfMoves.get(WhereInList+1).asDouble(),ListOfMoves.get(WhereInList+2).asDouble());
112                WhereInList+=3;
113                }
114                else if(currentpriority!=0)//WhereInList>=ListOfMoves.size()) // gone through the whole list then stop
115                {
116                        puts("stopping robot");
117                        robot->setVelocities(0,0);
118                        currentpriority=0;
119                        //ListOfMoves.clear();
120                        //ListOfMoves.addInt(0);
121                }
122               
123                //mrpt::system::sleep(20); // has to sleep a little bit so the onboard interupts doesn't keep on interupting itself.
124                yarp::os::Time::delay(1);
125                CPose2D odopose;
126                robot->getOdometry(odopose);
127
128                Bottle &odo = OdoOut.prepare();
129                odo.clear();
130                odo.addDouble(odopose.x());
131                odo.addDouble(odopose.y());
132                odo.addDouble(odopose.phi());
133                OdoOut.write();
134                vector_bool bumps;
135                robot->getBumpers(bumps);
136                for(int y=0;y<robInfo.nFrontBumpers;y++)
137                {
138                        if(bumps[y]==1){robot->setVelocities(0,0);break;}
139                }
140        }
141};
Note: See TracBrowser for help on using the repository browser.