source: level2/competencies/SamgarV1Modules/expressive-behaviours-uh/L2PioneerPhysical.cpp @ 1706

Revision 462, 25.9 KB checked in by KDucasse, 10 years ago (diff)
Line 
1/*
2        PROGRAM:        Level 2 of Pioneer's Physical Expression Module
3        Author :        K . L. Koay
4        Date   :    05 May 2010
5
6*/
7
8#include <Windows.h>
9#include "SamgarMainClass.h"
10
11//int AbsoluteMaxRotAccel = 300;                //Sets the robot's absolute maximum rotational acceleration.
12//int AbsoluteMaxRotDecel = 300;                //Sets the robot's absolute maximum rotational deceleration.
13//int AbsoluteMaxRotVel = 1500;         //Sets the robot's absolute maximum rotational velocity.
14
15//int AbsoluteMaxTransAccel = 2000;     //Sets the robot's absolute maximum translational acceleration.
16//int AbsoluteMaxTransDecel = 2000;//Sets the robot's absolute maximum translational deceleration.
17//int AbsoluteMaxTransVel = 1500; //Sets the robot's absolute maximum translational velocity.
18
19
20int CAM_Init=-99999;
21int ROBOT_Stop=-88888;
22int ROBOT_FinishedTransMovement=-77777;
23int ROBOT_Wait=60000;
24int CAM_Wait=30000;
25
26int ROBOT_Move=17367;
27int ROBOT_DeltaHeading=6986;//
28int ROBOT_AbsoluteMaxTransVel=5096;
29int ROBOT_AbsoluteMaxTransAccel=3086;   //Sets the robot's absolute maximum translational acceleration.
30int ROBOT_AbsoluteMaxTransDecel=1075;   //Sets the robot's absolute maximum translational deceleration.
31int ROBOT_AbsoluteMaxRotVel = 954;              //Sets the robot's absolute maximum rotational velocity.
32int ROBOT_AbsoluteMaxRotAccel = 644;            //Sets the robot's absolute maximum rotational acceleration.
33int ROBOT_AbsoluteMaxRotDecel = 333;            //Sets the robot's absolute maximum rotational deceleration.
34int CAM_Pan=231;        //
35int CAM_Tilt=39;        //
36
37
38
39int camWait(int wait)
40{
41        if (wait>20000){
42                wait=20000;
43                printf("\n Max waiting time is 20sec");
44        }
45        return wait+=CAM_Wait;
46}
47int robotWait(int wait)
48{
49        if (wait>20000){
50                wait=20000;
51                printf("\n Max waiting time is 20sec");
52        }
53        return wait+=ROBOT_Wait;
54}
55
56int robotMove(int moveCommand)
57{
58return moveCommand+=ROBOT_Move;
59}
60
61int robotDeltaHeading(int deltaHeadingCommand)
62{
63return deltaHeadingCommand+=ROBOT_DeltaHeading;
64}
65
66int setMaxTransVel(int MaxTransVel)
67{
68return MaxTransVel+=ROBOT_AbsoluteMaxTransVel;
69}
70
71int setMaxTransAccel(int MaxTransAccel)
72{
73return MaxTransAccel+=ROBOT_AbsoluteMaxTransAccel;
74}
75
76int setMaxTransDecel(int MaxTransDecel)
77{
78return MaxTransDecel+=ROBOT_AbsoluteMaxTransDecel;
79}
80int setMaxRotVel(int MaxRotVel)
81{
82return MaxRotVel+=ROBOT_AbsoluteMaxRotVel;
83}
84int setMaxRotAccel(int MaxRotAccel)
85{
86return MaxRotAccel+=ROBOT_AbsoluteMaxRotAccel;
87}
88int setMaxRotDecel(int MaxRotDecel)
89{
90return MaxRotDecel+=ROBOT_AbsoluteMaxRotDecel;
91}
92int camPan(int pan)
93{
94        return pan = CAM_Pan + pan;
95}
96int camTilt(int tilt)
97{
98        return tilt = CAM_Tilt + tilt;
99}
100
101
102
103
104//2147483648 to +2147483647
105
106
107
108void behaviour_Physical(SamgarModule &PhysicalRef, int behaviour)
109{
110Bottle BehaviourBaseOut;
111Bottle BehaviourCamOut;
112int ii, jj;
113switch (behaviour) {
114    case  0:    //MigrationOut - Done
115                //Camera stats looking down
116                /* for V1 of Vrobot
117                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(0));
118                BehaviourOut.addInt(camWait(18500));
119                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(-30));
120                //send data to virtual robot
121                PhysicalRef.SendBottleData("L2PBOut", BehaviourOut); //all physical movement share port i.e. cam and base movement
122                BehaviourOut.clear();
123                */
124                //New Vrobot - v2
125                BehaviourCamOut.addDouble(0); BehaviourCamOut.addDouble(0);     BehaviourCamOut.addDouble(18500);
126                BehaviourCamOut.addDouble(0); BehaviourCamOut.addDouble(-30);   BehaviourCamOut.addDouble(0);
127                PhysicalRef.SendBottleData("L2PB_COut", BehaviourCamOut);       //send cam data to cam port
128                BehaviourCamOut.clear();
129                printf("Migration Out\n");
130
131                break;
132
133        case  1:        //'MigrationInto - Done
134                /*for V1 of Vrobot
135                BehaviourOut.addInt(camWait(21500));
136                BehaviourOut.addInt(CAM_Init);  //Init the Camera and then start looking forward
137                BehaviourOut.addInt(camWait(5000));
138                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
139                */
140                //New Vrobot -v2
141                BehaviourCamOut.addDouble(-666);        BehaviourCamOut.addDouble(-666);        BehaviourCamOut.addDouble(21500); //do nothing to camera just wait
142                BehaviourCamOut.addDouble(999); BehaviourCamOut.addDouble(999); BehaviourCamOut.addDouble(5000);        //init the came and wait for 5sec
143                BehaviourCamOut.addDouble(0);           BehaviourCamOut.addDouble(20);          BehaviourCamOut.addDouble(0);   //init the came and wait for 5sec
144                printf("Migration Into\n");
145                //send data to virtual robot
146                //for V1 of Vrobot
147                //PhysicalRef.SendBottleData("L2PBOut", BehaviourOut); //all physical movement share port i.e. cam and base movement
148                //BehaviourOut.clear();
149                //for Vrobot - v2
150                PhysicalRef.SendBottleData("L2PB_COut", BehaviourCamOut);       //send cam data to cam port
151                BehaviourCamOut.clear();
152          break;
153
154    case  2:    //'Happy':
155                /*for V1 of Vrobot
156                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
157
158                BehaviourOut.addInt(robotDeltaHeading(-15)); //turn right
159                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
160                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire movemyRobot->lock();
161
162                BehaviourOut.addInt(robotDeltaHeading(30)); //turn left
163                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
164                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire movemyRobot->lock();
165
166                BehaviourOut.addInt(robotDeltaHeading(-30)); //turn right
167                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
168                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire movemyRobot->lock();
169
170                BehaviourOut.addInt(robotDeltaHeading(30)); //turn left
171                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
172                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire movemyRobot->lock();
173
174                BehaviourOut.addInt(robotDeltaHeading(-15)); //turn right
175                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
176                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire movemyRobot->lock();
177
178                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
179                BehaviourOut.addInt(camWait(1000));
180                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(-20));
181                BehaviourOut.addInt(camWait(1000));
182                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
183                BehaviourOut.addInt(camWait(1000));
184                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(-20));
185                BehaviourOut.addInt(camWait(1000));
186                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
187
188                PhysicalRef.SendBottleData("L2PBOut", BehaviourOut);
189                BehaviourOut.clear();
190                */
191                //for Vrobot - v2
192                //Base
193                BehaviourBaseOut.addDouble(-1);         //setMaxTransAccel, setMaxTransDecel
194                BehaviourBaseOut.addDouble(-1);         //setMaxTransVel
195                BehaviourBaseOut.addDouble(-1);         //setMaxRotAccel, setMaxRotDecel
196                BehaviourBaseOut.addDouble(-1);         //setMaxRotVel
197                BehaviourBaseOut.addDouble(0);          //translation
198                BehaviourBaseOut.addDouble(-15);        //rotation - turn right
199               
200                BehaviourBaseOut.addDouble(-1);         //setMaxTransAccel, setMaxTransDecel
201                BehaviourBaseOut.addDouble(-1);         //setMaxTransVel
202                BehaviourBaseOut.addDouble(-1);         //setMaxRotAccel, setMaxRotDecel
203                BehaviourBaseOut.addDouble(-1);         //setMaxRotVel
204                BehaviourBaseOut.addDouble(0);          //translation
205                BehaviourBaseOut.addDouble(30);         //rotation - turn left
206               
207                BehaviourBaseOut.addDouble(-1);         //setMaxTransAccel, setMaxTransDecel
208                BehaviourBaseOut.addDouble(-1);         //setMaxTransVel
209                BehaviourBaseOut.addDouble(-1);         //setMaxRotAccel, setMaxRotDecel
210                BehaviourBaseOut.addDouble(-1);         //setMaxRotVel
211                BehaviourBaseOut.addDouble(0);          //translation
212                BehaviourBaseOut.addDouble(-30);        //turn right
213               
214                BehaviourBaseOut.addDouble(-1);         //setMaxTransAccel, setMaxTransDecel
215                BehaviourBaseOut.addDouble(-1);         //setMaxTransVel
216                BehaviourBaseOut.addDouble(-1);         //setMaxRotAccel, setMaxRotDecel
217                BehaviourBaseOut.addDouble(-1);         //setMaxRotVel
218                BehaviourBaseOut.addDouble(0);          //translation
219                BehaviourBaseOut.addDouble(30);         //turn left
220               
221                BehaviourBaseOut.addDouble(-1);         //setMaxTransAccel, setMaxTransDecel
222                BehaviourBaseOut.addDouble(-1);         //setMaxTransVel
223                BehaviourBaseOut.addDouble(-1);         //setMaxRotAccel, setMaxRotDecel
224                BehaviourBaseOut.addDouble(-1);         //setMaxRotVel
225                BehaviourBaseOut.addDouble(0);          //translation
226                BehaviourBaseOut.addDouble(-15);        //turn right
227               
228                //Cam
229                BehaviourCamOut.addDouble(0);   BehaviourCamOut.addDouble(20);  BehaviourCamOut.addDouble(1000);
230                BehaviourCamOut.addDouble(0);   BehaviourCamOut.addDouble(-20); BehaviourCamOut.addDouble(1000);
231                BehaviourCamOut.addDouble(0);   BehaviourCamOut.addDouble(20);  BehaviourCamOut.addDouble(1000);
232                BehaviourCamOut.addDouble(0);   BehaviourCamOut.addDouble(-20); BehaviourCamOut.addDouble(1000);
233                BehaviourCamOut.addDouble(0);   BehaviourCamOut.addDouble(20);  BehaviourCamOut.addDouble(0);
234               
235               
236                PhysicalRef.SendBottleData("L2PB_BOut", BehaviourBaseOut);      //send Base data to Base port
237                PhysicalRef.SendBottleData("L2PB_COut", BehaviourCamOut);       //send cam data to cam port
238                BehaviourBaseOut.clear();
239                BehaviourCamOut.clear();
240          break;
241
242    case  3:    //Excited':
243                /*for V1 of Vrobot
244                BehaviourOut.addInt(setMaxTransAccel(250));
245                BehaviourOut.addInt(setMaxTransDecel(250));
246
247                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
248
249                BehaviourOut.addInt(robotMove(40)); //move forward
250                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
251                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
252               
253                BehaviourOut.addInt(robotMove(-80)); //move backward
254                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
255                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
256               
257                BehaviourOut.addInt(robotMove(80)); //move forward
258                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
259                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
260                 
261                BehaviourOut.addInt(robotMove(-80)); //move backward
262                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
263                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
264                 
265                BehaviourOut.addInt(robotMove(80)); //move forward
266                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
267                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
268                 
269                BehaviourOut.addInt(robotMove(-80)); //move backwardward
270                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
271                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
272                 
273                BehaviourOut.addInt(robotMove(80)); //move forward
274                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
275                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
276                 
277                BehaviourOut.addInt(robotMove(-40)); //move backward
278                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
279                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
280
281                BehaviourOut.addInt(ROBOT_Stop); //stop the robot if it reach the desire move
282                BehaviourOut.addInt(robotWait(200)); //wait for the robot to stop
283                 
284                BehaviourOut.addInt(camPan(-10)); BehaviourOut.addInt(camTilt(25));
285                BehaviourOut.addInt(camWait(800));
286                BehaviourOut.addInt(camPan(10)); BehaviourOut.addInt(camTilt(25));
287                BehaviourOut.addInt(camWait(800));
288                BehaviourOut.addInt(camPan(-10)); BehaviourOut.addInt(camTilt(25));
289                BehaviourOut.addInt(camWait(800));
290                BehaviourOut.addInt(camPan(10)); BehaviourOut.addInt(camTilt(25));
291                BehaviourOut.addInt(camWait(800));
292                BehaviourOut.addInt(camPan(-10)); BehaviourOut.addInt(camTilt(25));
293                BehaviourOut.addInt(camWait(800));
294                BehaviourOut.addInt(camPan(10)); BehaviourOut.addInt(camTilt(25));
295                BehaviourOut.addInt(camWait(800));
296                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(25));
297                BehaviourOut.addInt(camWait(200));
298
299                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
300                PhysicalRef.SendBottleData("L2PBOut", BehaviourOut);
301                BehaviourOut.clear();
302                */
303                BehaviourBaseOut.addDouble(250); //setMaxTransAccel, setMaxTransDecel
304                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
305                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
306                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
307                BehaviourBaseOut.addDouble(40); //move forward
308                BehaviourBaseOut.addDouble(0);  //delta
309               
310                BehaviourBaseOut.addDouble(250); //setMaxTransAccel, setMaxTransDecel
311                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
312                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
313                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
314                BehaviourBaseOut.addDouble(-80); //move backward
315                BehaviourBaseOut.addDouble(0);  //delta
316               
317                BehaviourBaseOut.addDouble(250); //setMaxTransAccel, setMaxTransDecel
318                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
319                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
320                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
321                BehaviourBaseOut.addDouble(80); //move forward
322                BehaviourBaseOut.addDouble(0);  //rotation - delta heading
323               
324               
325                BehaviourBaseOut.addDouble(250); //setMaxTransAccel, setMaxTransDecel
326                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
327                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
328                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
329                BehaviourBaseOut.addDouble(-80); //move backward
330                BehaviourBaseOut.addDouble(0);  //rotation - delta heading
331               
332                BehaviourBaseOut.addDouble(250); //setMaxTransAccel, setMaxTransDecel
333                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
334                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
335                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
336                BehaviourBaseOut.addDouble(80);         //move forward
337                BehaviourBaseOut.addDouble(0);  //rotation - delta heading
338               
339                BehaviourBaseOut.addDouble(250); //setMaxTransAccel, setMaxTransDecel
340                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
341                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
342                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
343                BehaviourBaseOut.addDouble(-80); //move backwardward
344                BehaviourBaseOut.addDouble(0);  //rotation - delta heading
345               
346                BehaviourBaseOut.addDouble(250); //setMaxTransAccel, setMaxTransDecel
347                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
348                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
349                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
350                BehaviourBaseOut.addDouble(80); //move forward
351                BehaviourBaseOut.addDouble(0);  //rotation - delta heading
352               
353                BehaviourBaseOut.addDouble(250); //setMaxTransAccel, setMaxTransDecel
354                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
355                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
356                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
357                BehaviourBaseOut.addDouble(-40); //move backward
358                BehaviourBaseOut.addDouble(0);  //rotation - delta heading
359               
360                //Cam
361                //BehaviourCamOut.addDouble(0); BehaviourCamOut.addDouble(20);          BehaviourCamOut.addDouble(800);
362                BehaviourCamOut.addDouble(-10); BehaviourCamOut.addDouble(25);          BehaviourCamOut.addDouble(800);
363                BehaviourCamOut.addDouble(10);  BehaviourCamOut.addDouble(25);          BehaviourCamOut.addDouble(800);
364                BehaviourCamOut.addDouble(-10); BehaviourCamOut.addDouble(25);          BehaviourCamOut.addDouble(800);
365                BehaviourCamOut.addDouble(10);  BehaviourCamOut.addDouble(25);          BehaviourCamOut.addDouble(800);
366                BehaviourCamOut.addDouble(-10); BehaviourCamOut.addDouble(25);          BehaviourCamOut.addDouble(800);
367                BehaviourCamOut.addDouble(10);  BehaviourCamOut.addDouble(25);          BehaviourCamOut.addDouble(800);
368                BehaviourCamOut.addDouble(0);   BehaviourCamOut.addDouble(25);          BehaviourCamOut.addDouble(200);
369                BehaviourCamOut.addDouble(0);   BehaviourCamOut.addDouble(20);          BehaviourCamOut.addDouble(0);
370               
371                PhysicalRef.SendBottleData("L2PB_BOut", BehaviourBaseOut);
372                PhysicalRef.SendBottleData("L2PB_COut", BehaviourCamOut);
373                BehaviourBaseOut.clear();
374                BehaviourCamOut.clear();
375               
376          break;
377
378    case  4:    //'Bored':
379    /*for V1 of Vrobot
380                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
381
382                BehaviourOut.addInt(setMaxRotAccel(10));
383                BehaviourOut.addInt(setMaxRotDecel(10));
384
385                BehaviourOut.addInt(robotDeltaHeading(-35)); //turn right
386                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
387                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
388                BehaviourOut.addInt(ROBOT_Stop); //stop the robot if it reach the desire move
389                BehaviourOut.addInt(robotWait(800)); //wait for the robot to stop FinishedRotMovement(myRobot);
390
391                BehaviourOut.addInt(robotDeltaHeading(70)); //turn left
392                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
393                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
394                BehaviourOut.addInt(ROBOT_Stop); //stop the robot if it reach the desire move
395                BehaviourOut.addInt(robotWait(800)); //wait for the robot to stop FinishedRotMovement(myRobot);
396
397                BehaviourOut.addInt(robotDeltaHeading(-35)); //turn right
398                //BehaviourOut.addInt(robotWait(100)); //wait for command to get to robot
399                //BehaviourOut.addInt(ROBOT_FinishedTransMovement); //then activate this to check if robot reach the desire move
400                BehaviourOut.addInt(ROBOT_Stop); //stop the robot if it reach the desire move
401                BehaviourOut.addInt(robotWait(800)); //wait for the robot to stop FinishedRotMovement(myRobot);
402
403                //camera down and up randomly.//moves slowly or try to get attention //boring colour/orange on
404                //restored Max Rot Acceel and Rot Decel
405
406                jj=0; 
407                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(-20));
408                BehaviourOut.addInt(camWait(200));
409
410                /*for (ii=0; ii<10; ii++) {
411                        //random select one
412                        BehaviourOut.addInt(camPan(2*ii)); BehaviourOut.addInt(camTilt(-20));
413                        BehaviourOut.addInt(camWait(500));
414                }
415                BehaviourOut.addInt(camWait(2000));
416
417                for (ii=0; ii<10; ii++) {
418                        BehaviourOut.addInt(camPan(-4*ii)); BehaviourOut.addInt(camTilt(-20));
419                        BehaviourOut.addInt(camWait(500));
420                        //random select one
421                } */
422                /*BehaviourOut.addInt(camPan(45)); BehaviourOut.addInt(camTilt(45));
423                BehaviourOut.addInt(camWait(1000));
424
425                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(-30));
426                BehaviourOut.addInt(camWait(2000));
427
428                BehaviourOut.addInt(camPan(-45)); BehaviourOut.addInt(camTilt(45));
429                BehaviourOut.addInt(camWait(1000));
430
431                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(-30));
432                BehaviourOut.addInt(camWait(2000));
433
434                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
435                BehaviourOut.addInt(camWait(200));
436
437                PhysicalRef.SendBottleData("L2PBOut", BehaviourOut);
438                BehaviourOut.clear();
439               
440                */
441               
442
443                BehaviourBaseOut.addDouble(-1);         //setMaxTransAccel, setMaxTransDecel
444                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
445                BehaviourBaseOut.addDouble(10); //setMaxRotAccel, setMaxRotDecel
446                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
447                BehaviourBaseOut.addDouble(0);  //move backward
448                BehaviourBaseOut.addDouble(-35); //turn right
449               
450                BehaviourBaseOut.addDouble(-1);         //setMaxTransAccel, setMaxTransDecel
451                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
452                BehaviourBaseOut.addDouble(10); //setMaxRotAccel, setMaxRotDecel
453                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
454                BehaviourBaseOut.addDouble(0);  //move backward
455                BehaviourBaseOut.addDouble(70); //turn left
456               
457                BehaviourBaseOut.addDouble(-1);         //setMaxTransAccel, setMaxTransDecel
458                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
459                BehaviourBaseOut.addDouble(10); //setMaxRotAccel, setMaxRotDecel
460                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
461                BehaviourBaseOut.addDouble(0);  //move backward
462                BehaviourBaseOut.addDouble(-35); //turn right
463               
464                //camera down and up randomly.//moves slowly or try to get attention //boring colour/orange on
465                //restored Max Rot Acceel and Rot Decel
466               
467                BehaviourCamOut.addDouble(0); BehaviourCamOut.addDouble(20);    BehaviourCamOut.addDouble(200);
468                BehaviourCamOut.addDouble(0); BehaviourCamOut.addDouble(-20);   BehaviourCamOut.addDouble(2000);
469                BehaviourCamOut.addDouble(45); BehaviourCamOut.addDouble(45);   BehaviourCamOut.addDouble(2000);
470                BehaviourCamOut.addDouble(0); BehaviourCamOut.addDouble(-30);   BehaviourCamOut.addDouble(2000);
471                BehaviourCamOut.addDouble(-45); BehaviourCamOut.addDouble(45);  BehaviourCamOut.addDouble(2000);
472                BehaviourCamOut.addDouble(0); BehaviourCamOut.addDouble(-30);   BehaviourCamOut.addDouble(2000);
473                BehaviourCamOut.addDouble(0); BehaviourCamOut.addDouble(20);    BehaviourCamOut.addDouble(200);
474               
475
476                PhysicalRef.SendBottleData("L2PB_BOut", BehaviourBaseOut);
477                PhysicalRef.SendBottleData("L2PB_COut", BehaviourCamOut);
478                BehaviourBaseOut.clear();
479                BehaviourCamOut.clear();
480               
481          break;
482
483    case  5:    //'Tired'
484                /*
485                //camera down
486                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(-10));
487                //BehaviourOut.addInt(camWait(2000));
488                //Set the Max Trans Vel for tired
489                //BehaviourOut.addInt(setMaxTransVel(50));
490                BehaviourOut.addInt(setMaxTransAccel(10));
491                BehaviourOut.addInt(setMaxTransDecel(10));
492
493               
494                //Performing the forward backward motion
495                BehaviourOut.addInt(robotMove(20)); //move forward
496                BehaviourOut.addInt(ROBOT_Stop); //stop the robot if it reach the desire move
497                BehaviourOut.addInt(robotWait(200)); //wait for the robot to stop       
498               
499                BehaviourOut.addInt(robotMove(-40)); //moves backward
500                BehaviourOut.addInt(ROBOT_Stop); //stop the robot if it reach the desire move
501                BehaviourOut.addInt(robotWait(200)); //wait for the robot to stop
502         
503                BehaviourOut.addInt(robotMove(40)); //moves forward
504                BehaviourOut.addInt(ROBOT_Stop); //stop the robot if it reach the desire move
505                BehaviourOut.addInt(robotWait(200)); //wait for the robot to stop
506         
507                BehaviourOut.addInt(robotMove(-20)); //move forward
508                BehaviourOut.addInt(ROBOT_Stop); //stop the robot if it reach the desire move
509                BehaviourOut.addInt(robotWait(200)); //wait for the robot to stop
510
511                //Restored the Max Trans Vel for tired
512                /*
513                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
514                BehaviourOut.addInt(camWait(2000));
515                for (ii=0; ii<10; ii++) {
516                        BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20-(ii*5)));
517                        //need to set a random select one action at some point
518                        BehaviourOut.addInt(camWait(500-ii*20));
519                }*/
520/*
521                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
522                BehaviourOut.addInt(camWait(1000));
523
524                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(-30));
525                BehaviourOut.addInt(camWait(2000));
526
527                BehaviourOut.addInt(camPan(0)); BehaviourOut.addInt(camTilt(20));
528                BehaviourOut.addInt(camWait(200));
529
530                PhysicalRef.SendBottleData("L2PBOut", BehaviourOut);
531                BehaviourOut.clear();
532                */
533               
534                //camera down
535
536               
537                //Performing the forward backward motion
538                BehaviourBaseOut.addDouble(10);         //setMaxTransAccel, setMaxTransDecel -1: use vrobot value
539                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
540                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
541                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
542                BehaviourBaseOut.addDouble(20);         //move backward
543                BehaviourBaseOut.addDouble(0);  //delta rot
544
545                BehaviourBaseOut.addDouble(10);         //setMaxTransAccel, setMaxTransDecel
546                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
547                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
548                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
549                BehaviourBaseOut.addDouble(-40);        //move backward
550                BehaviourBaseOut.addDouble(0);  //delta rot
551
552                BehaviourBaseOut.addDouble(10);         //setMaxTransAccel, setMaxTransDecel
553                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
554                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
555                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
556                BehaviourBaseOut.addDouble(40);         //move backward
557                BehaviourBaseOut.addDouble(0);  //delta rot
558         
559                BehaviourBaseOut.addDouble(10);         //setMaxTransAccel, setMaxTransDecel
560                BehaviourBaseOut.addDouble(-1); //setMaxTransVel
561                BehaviourBaseOut.addDouble(-1); //setMaxRotAccel, setMaxRotDecel
562                BehaviourBaseOut.addDouble(-1); //setMaxRotVel
563                BehaviourBaseOut.addDouble(-20); //move backward
564                BehaviourBaseOut.addDouble(0);  //delta rot
565         
566                BehaviourCamOut.addDouble(0);           BehaviourCamOut.addDouble(-10);         BehaviourCamOut.addDouble(2000); //might need bigger delay so it only do the below movement later - check with video
567                BehaviourCamOut.addDouble(0);           BehaviourCamOut.addDouble(20);          BehaviourCamOut.addDouble(1000);
568                BehaviourCamOut.addDouble(0);           BehaviourCamOut.addDouble(-30);         BehaviourCamOut.addDouble(2000);
569                BehaviourCamOut.addDouble(0);           BehaviourCamOut.addDouble(20);          BehaviourCamOut.addDouble(0);
570               
571
572                PhysicalRef.SendBottleData("L2PB_BOut", BehaviourBaseOut);
573                PhysicalRef.SendBottleData("L2PB_COut", BehaviourCamOut);
574                BehaviourBaseOut.clear();
575                BehaviourCamOut.clear();
576               
577          break;
578         
579         
580        default:
581                printf("\nNo Such Behaviour, I haven't learn that behaviour yet");
582      break;
583
584
585  }
586}
587int main(void)
588{
589        Network yarp;           //name                  //Category //subcategory       
590        SamgarModule Physical("PioneerPhysical", "Physical", "BaseAndCam", SamgarModule::interupt);
591        Physical.AddPortS("L2PBIn");
592        Physical.AddPortS("L2PB_BOut");
593        Physical.AddPortS("L2PB_COut");
594       
595        Bottle BehaviourIn;
596        while (1){
597                if (Physical.GetBottleData("L2PBIn", &BehaviourIn, SamgarModule::NoStep)==true)
598                {
599                        //printf("\n ---- I got something %d", BehaviourIn.get(0).asInt());
600                        behaviour_Physical(Physical, BehaviourIn.get(0).asInt());
601                        //printf("\n ---- Done sending Behaviour %d", BehaviourIn.get(0).asInt());
602                        BehaviourIn.clear();
603                }
604                Physical.SucceedFail(true,100);
605        }
606}
Note: See TracBrowser for help on using the repository browser.