source: level2/competencies/SamgarV1Modules/Navi-Robot-uh/O_VirtualRobot.cpp @ 469

Revision 469, 15.0 KB checked in by KDucasse, 9 years ago (diff)
Line 
1
2
3#include <Aria.h>
4#include <time.h>
5#include <string>
6#include <ArBumpers.h>
7#include <ArVCC4.h>
8#include <ariaUtil.h>
9#include <stdio.h>
10#include <stdlib.h>
11#include <iostream>
12#include <string>
13#include <conio.h>
14#include "SamgarMainClass.h"
15
16
17/*
18        Remember to put some stuff in so it waits for stargazer to update it before it starts pumping out data
19
20
21
22*/
23
24#define usestar false
25
26
27using namespace std;
28using namespace yarp;
29
30/*** Robot Defs ***/
31
32ArRobot robot(NULL,false);
33ArVCC4 ptz(&robot, false, ArVCC4::COMM_UNKNOWN,true, false, ArVCC4::CAMERA_C50I);
34ArSerialConnection RobotConnectoin;
35ArSerialConnection CameraConnectoin;
36ArSonarDevice sonarDev;
37ArBumpers bumpers;
38
39
40ArKeyHandler keyHandler;
41unsigned int camTimeRef;
42
43bool InTransit = false;
44bool Emergency = false;
45bool BeenCorrectedByStar = false;
46
47
48
49class ActionEmergencyControl : public ArAction
50{
51public :
52        double SonarThreshold;
53        ArRangeDevice *mysonar;
54        // constructor
55        ActionEmergencyControl() : ArAction("EmergencyControl") {       SonarThreshold = 500;   }
56        virtual void setRobot(ArRobot *robot)                                   {       ArAction::setRobot(robot);      mysonar = robot->findRangeDevice("sonar");      }
57        virtual ~ActionEmergencyControl(void){}
58        virtual ArActionDesired *fire (ArActionDesired currentDesired)
59        {
60                static bool BangBang = false;
61                double LeftFrontSonar  = mysonar->currentReadingPolar(0,90);
62                double LeftBackSonar   = mysonar->currentReadingPolar(90,180);
63                double RightBackSonar  = mysonar->currentReadingPolar(180,270);
64                double RightFrontSonar = mysonar->currentReadingPolar(270,360);//- myRobot->getRobotRadius();// - robot.getRobotRadius();
65                double leftwheel =0,rightwheel =0;
66                double turnleft=0,turnright=0;
67                double forward=0,backward=0;   
68
69                Emergency=false;
70//              if(LeftFrontSonar<SonarThreshold)       {turnleft-=2;backward-=100;Emergency=true;}
71//              if(RightFrontSonar<SonarThreshold)      {turnright+=2;backward-=100;Emergency=true;}
72//              if(LeftBackSonar<SonarThreshold)        {turnright+=2;forward+=100;Emergency=true;}
73//              if(RightBackSonar<SonarThreshold)       {turnleft-=2;forward+=100;Emergency=true;}
74       
75                Emergency=false;
76                if (kbhit())
77                {
78                        char mychar = getch();
79                        if(mychar=='q'){myRobot->disconnect();Aria::shutdown();}
80                        if(mychar=='w'){myDesired.setVel(+200);}
81                        if(mychar=='s'){myDesired.setVel(-200);}
82                        if(mychar=='a'){myDesired.setDeltaHeading(+10);}
83                        if(mychar=='d'){myDesired.setDeltaHeading(-10);}
84                        BangBang=true;
85                }
86                else if(Emergency==true)
87                {
88                //      myDesired.setDeltaHeading(turnleft+turnright,1);
89                //      myDesired.setVel(forward+backward);
90                //      myDesired.setVel(0);
91                }
92                else
93                {
94                        if(BangBang==true)
95                        {
96                                puts("resetting to zero");
97                        myDesired.setDeltaHeading(0,1);
98                        myDesired.setVel(0);
99                        myDesired.reset();
100                        myRobot->unlock();
101                        }
102                        BangBang=false;
103                }
104               
105               
106        return &myDesired;
107        }
108protected :
109        ArActionDesired myDesired;
110};
111
112class UpdateMap : public ArAction
113{
114public :
115       
116
117        UpdateMap(SamgarModule *copyofmodule) : ArAction("UpdateMap")
118        {
119        Mycopyofmodule = copyofmodule;
120        Mycopyofmodule->AddPortS("MAPout");
121        }
122        virtual void setRobot(ArRobot *robot)                                   {       ArAction::setRobot(robot);}
123        virtual ~UpdateMap(void){}
124        virtual ArActionDesired *fire (ArActionDesired currentDesired)
125        {
126                if(BeenCorrectedByStar || !usestar) // if we have updated odo from star or is star isn't being used
127                {
128                 ArPose MyPose;
129                 MyPose =       myRobot->getPose();
130                Bottle PoseBottle;
131               
132                PoseBottle.addDouble(MyPose.getX()/1000);
133                PoseBottle.addDouble(MyPose.getY()/1000);
134                PoseBottle.addDouble(MyPose.getTh());
135                Mycopyofmodule->SendBottleData("MAPout",PoseBottle);
136                //printf("my x:%f y:%f rot:%f \n",MyPose.getX()/1000,MyPose.getY()/1000,MyPose.getTh());
137                }
138        return &myDesired;
139        }
140        protected :
141        SamgarModule *Mycopyofmodule;
142        ArActionDesired myDesired;
143};
144class PlaySounder : public ArAction
145{
146public :
147
148        PlaySounder(SamgarModule *copyofmodule) : ArAction("PlaySounder")
149        {
150        Mycopyofmodule = copyofmodule;
151        Mycopyofmodule->AddPortS("SOUNDin");
152        }
153        virtual void setRobot(ArRobot *robot)                                   {       ArAction::setRobot(robot);}
154        virtual ~PlaySounder(void){}
155        virtual ArActionDesired *fire (ArActionDesired currentDesired)
156        {
157        Bottle SoundBottle;
158        if(Mycopyofmodule->GetBottleData("SOUNDin",&SoundBottle,SamgarModule::NoStep)==true)
159                {
160                        char tune[40];
161                        for(int uu = 0;uu<SoundBottle.size();uu++)
162                        {
163                                tune[uu]=SoundBottle.get(uu).asInt();
164                        }
165                        robot.comStrN(ArCommands::SAY,tune,SoundBottle.size());
166                }
167        return &myDesired;
168        }
169        protected :
170        SamgarModule *Mycopyofmodule;
171        ArActionDesired myDesired;
172};
173class BehaveMove : public ArAction
174{
175public :
176
177        BehaveMove(SamgarModule *copyofmodule) : ArAction("BehaveMove")
178        {
179        Mycopyofmodule = copyofmodule;
180        Mycopyofmodule->AddPortS("BEHAVEin");
181        NewData = false;
182        WhereInList =0;
183        }
184        virtual void setRobot(ArRobot *robot)                                   {       ArAction::setRobot(robot);}
185        virtual ~BehaveMove(void){}
186
187        virtual ArActionDesired *fire (ArActionDesired currentDesired)
188        {
189        Bottle MoveBottle;
190        NewData = Mycopyofmodule->GetBottleData("BEHAVEin",&MoveBottle,SamgarModule::NoStep);
191                //happy
192/*              PrecBottle.addDouble(myRobot->getTransVelMax());
193                PrecBottle.addDouble(myRobot->getRotVelMax());
194                PrecBottle.addDouble(0); //linear vel = 0
195                PrecBottle.addDouble(-15); //turn right
196               
197                PrecBottle.addDouble(myRobot->getTransVelMax());
198                PrecBottle.addDouble(myRobot->getRotVelMax());
199                PrecBottle.addDouble(0); //linear vel = 0
200                PrecBottle.addDouble(30); //turn left
201               
202                PrecBottle.addDouble(myRobot->getTransVelMax());
203                PrecBottle.addDouble(myRobot->getRotVelMax());
204                PrecBottle.addDouble(0); //linear vel = 0
205                PrecBottle.addDouble(-30); //turn right
206               
207                PrecBottle.addDouble(myRobot->getTransVelMax());
208                PrecBottle.addDouble(myRobot->getRotVelMax());
209                PrecBottle.addDouble(0); //linear vel = 0
210                PrecBottle.addDouble(30); //turn left
211               
212                PrecBottle.addDouble(myRobot->getTransVelMax());
213                PrecBottle.addDouble(myRobot->getRotVelMax());
214                PrecBottle.addDouble(-0); //linear vel = 0
215                PrecBottle.addDouble(-15); //turn right
216*/     
217        if(NewData){WhereInList=0;PrecBottle=MoveBottle;}
218
219
220        //I change the order from the original now they work as below
221        //PrecBottle.clear();
222        //PrecBottle.addDouble(0); // maxtrans
223        //PrecBottle.addDouble(0); // max rot
224        //PrecBottle.addDouble(0); // distance
225        //PrecBottle.addDouble(0); // rot
226                       
227                        if(myRobot->isHeadingDone(1) && WhereInList<PrecBottle.size() && InTransit == false)
228                        {
229                                printf("heading done,loading new");
230                                //myRobot->setAbsoluteMaxLatVel(PrecBottle.get(WhereInList).asDouble());
231                                myRobot->setAbsoluteMaxTransVel(PrecBottle.get(WhereInList).asDouble());
232                                WhereInList++;
233                                myRobot->setAbsoluteMaxRotVel(PrecBottle.get(WhereInList).asDouble());
234                                WhereInList++;
235                                myRobot->move(PrecBottle.get(WhereInList).asDouble());
236                                WhereInList++;
237                                myRobot->setDeltaHeading(PrecBottle.get(WhereInList).asDouble());
238                                WhereInList++;
239                        }
240
241                        if(WhereInList>=PrecBottle.size())
242                        {
243                //              myDesired.reset();
244                        }
245                //      myDesired.setHeading(0,1);
246        return &myDesired;
247        }
248
249        protected :
250                Bottle PrecBottle;
251                int WhereInList;
252                bool NewData;
253                SamgarModule *Mycopyofmodule;
254                ArActionDesired myDesired;
255};
256
257class MoveCAM : public ArAction
258{
259public :
260
261        MoveCAM(SamgarModule *copyofmodule) : ArAction("MoveCAM")
262        {
263        Mycopyofmodule = copyofmodule;
264        Mycopyofmodule->AddPortS("CAMin");
265        NewData = false;
266        WhereInList =0;
267        pan =0;
268        tilt=0;
269        time=0;
270        }
271
272        virtual void setRobot(ArRobot *robot)                                   {       ArAction::setRobot(robot);}
273        virtual ~MoveCAM(void){}
274
275        virtual ArActionDesired *fire (ArActionDesired currentDesired)
276        {
277        Bottle CAMBottle;
278        NewData = Mycopyofmodule->GetBottleData("CAMin",&CAMBottle,SamgarModule::NoStep);
279
280               
281        if(NewData){WhereInList=0;PrecBottle=CAMBottle;} // if theres new data reset the system
282
283        //PrecBottle.clear();
284        //PrecBottle.addDouble(0); // pan
285        //PrecBottle.addDouble(0); // tilt
286        //PrecBottle.addDouble(100); // wait (iteration)
287
288        PrecBottle.addDouble(0); PrecBottle.addDouble(20);      PrecBottle.addDouble(1000);
289        PrecBottle.addDouble(0); PrecBottle.addDouble(-20);     PrecBottle.addDouble(1000);
290        PrecBottle.addDouble(0); PrecBottle.addDouble(20);      PrecBottle.addDouble(1000);
291        PrecBottle.addDouble(0); PrecBottle.addDouble(-20);     PrecBottle.addDouble(1000);
292        PrecBottle.addDouble(0); PrecBottle.addDouble(20);      PrecBottle.addDouble(100);
293        time=-1;
294
295        // if its reached the desired tilt/pan get the next lot of data if there is more data in the list
296        if(ptz.getPan()==pan && ptz.getTilt() == tilt && WhereInList<PrecBottle.size()/*&& ((time==-1) || ((time!=-1)&& (time < (ArUtil::getTime()-camTimeRef))) )*/)   
297                {
298                pan  = PrecBottle.get(WhereInList).asDouble();WhereInList++;
299                tilt = PrecBottle.get(WhereInList).asDouble();WhereInList++;                   
300                time = PrecBottle.get(WhereInList).asDouble();WhereInList++;
301                printf("if - Pan %d=%d, Tilt %d=%d, Timer %d=%d\n",pan,ptz.getPan(), tilt,ptz.getTilt(), time, (ArUtil::getTime()-camTimeRef));
302                }
303        else  // if its not reached the desired pan/tilt send the command again.
304                {
305                ptz.pan(pan);
306                ptz.tilt(tilt);
307                camTimeRef=ArUtil::getTime();
308                printf("else - Pan %d=%d, Tilt %d=%d, Timer %d=%d\n",pan,ptz.getPan(), tilt,ptz.getTilt(), time, (ArUtil::getTime()-camTimeRef));
309               
310                }
311        //time--;
312                return &myDesired;
313        }
314
315        protected :
316                Bottle PrecBottle;
317                int WhereInList;
318                bool NewData;
319                double pan,tilt,time;
320                SamgarModule *Mycopyofmodule;
321                ArActionDesired myDesired;
322};
323
324class UpOdo : public ArAction
325{
326public :
327                // corrects odo from stargazer
328        UpOdo(SamgarModule *copyofmodule) : ArAction("UpOdo")
329        {
330        Mycopyofmodule = copyofmodule;
331        Mycopyofmodule->AddPortS("STARin");
332        }
333        virtual void setRobot(ArRobot *robot)                                   {       ArAction::setRobot(robot);}
334        virtual ~UpOdo(void){}
335
336        virtual ArActionDesired *fire (ArActionDesired currentDesired)
337        {
338        Bottle OdoBottle;
339       
340        if(Mycopyofmodule->GetBottleData("STARin",&OdoBottle,SamgarModule::NoStep))
341        {
342                ArPose MyPose;
343                MyPose.setX (OdoBottle.get(0).asDouble());
344                MyPose.setY (OdoBottle.get(1).asDouble());
345                MyPose.setTh(OdoBottle.get(2).asDouble());
346                myRobot->moveTo(MyPose,true);//update in mm
347                BeenCorrectedByStar=true;
348        }
349       
350        return &myDesired;
351        }
352
353        protected :
354                SamgarModule *Mycopyofmodule;
355                ArActionDesired myDesired;
356};
357
358class Transit : public ArAction
359{
360public :
361
362        Transit(SamgarModule *copyofmodule) : ArAction("TransitIn")
363        {
364        Mycopyofmodule = copyofmodule;
365       
366        PI= 3.14159265;
367        }
368        virtual void setRobot(ArRobot *robot)                                   {       ArAction::setRobot(robot);}
369        virtual ~Transit(void){}
370
371        virtual ArActionDesired *fire (ArActionDesired currentDesired)
372        {
373        Bottle TransitBottle;
374        static bool wasIntransit=false;
375        //puts("in transit method");
376        if(Mycopyofmodule->GetBottleData("TransitIn",&TransitBottle,SamgarModule::NoStep))
377        {
378                puts("got data");
379                //printf("recived data %s \n",TransitBottle.toString().c_str());
380                if(TransitBottle.get(0).asDouble()==1)
381                {
382
383               
384               
385                speed    = TransitBottle.get(1).asDouble()*1000;
386                angle    = TransitBottle.get(2).asDouble();
387                accuracy = TransitBottle.get(3).asDouble()*1000;
388
389
390
391
392
393                if(InTransit==false){myDesired.setHeading(angle);
394                yarp::os::Time::delay(0.1);}
395                InTransit=true;
396        //      speed=speed/2;
397                if(speed<20){speed=0;}   // its in mm
398                }
399
400                else
401                {
402                speed    = 0;//TransitBottle.get(1).asDouble()*1000;
403                angle    = 0;//TransitBottle.get(2).asDouble();
404                accuracy = 0;//TransitBottle.get(3).asDouble()*1000;
405
406
407                        InTransit=false;
408                        myDesired.reset();
409               
410                }
411        }
412
413
414                if(InTransit==true)
415                {
416                                               
417                        //      float currentheadingchange = abs(myDesired.getHeading()-angle);
418                                printf("angle wanted %f \n",angle);
419                        //      myDesired.setDeltaHeading(currentheadingchange/4);
420                                myDesired.setHeading(angle,1);
421                                myDesired.setVel(speed,1);
422                }
423                else
424                {
425                        myDesired.reset();
426                }
427                       
428
429       
430        return &myDesired;
431        }
432
433        void FigureoutWheelsKyronsAlgorythem(void)
434        {
435                // all in mm and in one timestep (sec)
436               
437                double RobotCircum = (myRobot->getRobotRadius()*2)*PI;
438                double WheelRobotRatio = RobotCircum / 360 ;// Very important, the wheel move needid to move 1 deg
439                double errortospeedratio = speed/accuracy;
440                double angleneedid = angle - myRobot->getPose().getTh();
441
442               
443                // degree a secound myDesired.setRotVel
444        //      RotLeftWheel = (WheelRobotRatio*angleneedid)*errortospeedratio;
445        //      RotRightWheel = RotLeftWheel*-1;
446        //      myDesired.setRotVel(angle/2,1);
447                //or
448        //      myDesired.setRotVel(RotLeftWheel,1);
449
450        //      myDesired.setVel(speed,1);
451
452//              puts("working out");
453//              printf("wheel ratio %f angleneedid %f errortospeedration %f \n",WheelRobotRatio,angleneedid,errortospeedratio);
454//              int hh;
455        //      cin >> hh;
456                       
457//              if(abs(RotLeftWheel)>=speed)
458//              {
459//                      if(RotLeftWheel>0)      {RotLeftWheel=   speed; RotRightWheel = - speed;}
460//                      else                            {RotLeftWheel= - speed; RotRightWheel =   speed;}
461//              }
462//              else
463//              {
464                //double leftoverspeed = speed - abs(RotLeftWheel);
465//              RotLeftWheel+=leftoverspeed;
466//              RotRightWheel+=leftoverspeed;
467//              }
468
469               
470        }
471
472        protected :
473                double speed,angle, accuracy;
474                 SamgarModule *Mycopyofmodule;
475                ArActionDesired myDesired;
476                double PI;
477                double RotLeftWheel,RotRightWheel;
478};
479
480void SetupRobot(void);
481
482int main()
483{
484
485/*
486        TODO
487        Check that the Starclass is updating the global pose right      - not done
488        Check that the MapClass is giving the right pose                        - not done
489        check khenglee can use the behaviours                                           - not done
490        check emergency control                                                                         - not done
491
492*/
493        Network yarp;
494
495        SamgarModule VR("Vrobot","Loco","wheel",SamgarModule::run); // Cant have spaces or underscores
496        VR.AddPortS("TransitIn");
497        SetupRobot();
498
499
500        ActionEmergencyControl EmergencyControl;
501        UpdateMap               UpdMap          (&VR);
502        UpOdo           OdoUp       (&VR);
503        Transit         TransitIn   (&VR);
504        PlaySounder             SoundPlayer     (&VR);
505        BehaveMove              MoveBehave      (&VR);
506        MoveCAM                 CAMMove         (&VR);
507
508        // lowest priority might actully be highest becouse coms direct to robot and not through desiredaction.
509        robot.addAction(&EmergencyControl,99); // need to check this works
510        robot.addAction(&UpdMap,99);
511        robot.addAction(&OdoUp,99);
512        robot.addAction(&TransitIn,70);
513        robot.addAction(&SoundPlayer,100);
514        robot.addAction(&MoveBehave,10);
515        robot.addAction(&CAMMove,100);
516        robot.run(true);
517
518        robot.disconnect();
519        Aria::shutdown();
520        return 0;
521}
522
523void SetupRobot(void)
524{
525
526        puts("attempting to connect to robot");
527        RobotConnectoin.setPort("COM2");
528        RobotConnectoin.setBaud(9600);
529        robot.setDeviceConnection(&RobotConnectoin);
530        CameraConnectoin.open("COM9");
531        ptz.setDeviceConnection(&CameraConnectoin);
532        ptz.init();
533        ArUtil::sleep(15000);
534        printf("done Init");
535        Aria::setKeyHandler(&keyHandler);
536        robot.attachKeyHandler(&keyHandler);
537
538        if(!robot.blockingConnect()){puts("not connected to robot");Aria::shutdown();}
539        robot.addRangeDevice(&sonarDev);
540        robot.addRangeDevice(&bumpers);
541        robot.enableMotors();
542        robot.enableSonar();
543        robot.requestEncoderPackets();
544        robot.setCycleChained(false);
545        robot.setAbsoluteMaxRotVel(robot.getAbsoluteMaxRotVel()/10);
546//      robot.setRotVelMax(robot.getRotVelMax());
547}
548
549
550
551
552
Note: See TracBrowser for help on using the repository browser.