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

Revision 461, 9.1 KB checked in by KDucasse, 10 years ago (diff)
Line 
1/*
2        PROGRAM:        Level 2 of Pioneer's Display Expression Module
3        Author :        K . L. Koay
4        Date   :    05 May 2010
5
6*/
7
8#include <Windows.h>
9#include "SamgarMainClass.h"
10
11using namespace std;
12using namespace yarp;
13
14void behaviour_Display(SamgarModule &DisplayRef,        int behaviour)
15{
16int ii, jj, kk; 
17
18Bottle BehaviourOut;
19
20  switch (behaviour){
21
22        case 0:  //Migration Out
23                printf("Migration Out - 0");
24                for (jj=0; jj<10; jj++){
25                        for (ii=0; ii<4; ii++){
26                                if (ii==0){
27                                        BehaviourOut.addInt(100);BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);
28                                        //sprintf(data1,"#12H #13L #14L #15L \r");
29                                }
30                                else if (ii==1){
31                                                BehaviourOut.addInt(0);BehaviourOut.addInt(100);BehaviourOut.addInt(0);BehaviourOut.addInt(0);
32                                                //sprintf(data1,"#12L #13H #14L #15L \r");
33                                        }
34                                        else if (ii==2){
35                                                        BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(100);BehaviourOut.addInt(0);
36                                                        //sprintf(data1,"#12L #13L #14H #15L \r");
37                                                }
38                                                else if (ii==3){
39                                                                BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(100);
40                                                                //sprintf(data1,"#12L #13L #14L #15H \r");
41                                                        }
42                                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
43                                BehaviourOut.clear();
44                                //write(*fd,data1,sizeof(data1));
45                                Sleep(500);
46                                //ArUtil::sleep(500);
47                        }
48                }
49                BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);
50                //sprintf(data1,"#12H #13H #14H #15H \r");
51                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
52                BehaviourOut.clear();
53                //write(*fd,data1,sizeof(data1));
54                Sleep(1000);
55                //ArUtil::sleep(1000);
56                BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);
57                //sprintf(data1,"#12L #13L #14L #15L \r");
58                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
59                BehaviourOut.clear();
60                //write(*fd,data1,sizeof(data1));
61                Sleep(500);
62                //ArUtil::sleep(500);
63                BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);
64                //sprintf(data1,"#12H #13H #14H #15H \r");
65                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
66                BehaviourOut.clear();
67                //write(*fd,data1,sizeof(data1));
68                Sleep(1000);
69                //ArUtil::sleep(1000);
70                BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);
71                //sprintf(data1,"#12L #13L #14L \r");
72                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
73                BehaviourOut.clear();
74                Sleep(1000);
75                //DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
76                //BehaviourOut.clear();
77                //write(*fd,data1,sizeof(data1));
78                break;
79
80    case 1:  //Migration in
81                printf("Migration in -1");
82                for (jj=0; jj<10; jj++)
83                { 
84                  for (ii=0; ii<4; ii++){
85                        if (ii==0){
86                                BehaviourOut.addInt(100);BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);
87                                //sprintf(data1,"#12H #15L #14L #13L \r");
88                         }
89                        else if (ii==1){
90                                        BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(100);
91                                        //sprintf(data1,"#12L #15H #14L #13L \r");
92                                 }     
93                                else if (ii==2){
94                                                BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(100);BehaviourOut.addInt(0);
95                                                //sprintf(data1,"#12L #15L #14H #13L \r");
96                                        }
97                                        else if (ii==3){
98                                                        BehaviourOut.addInt(0);BehaviourOut.addInt(100);BehaviourOut.addInt(0);BehaviourOut.addInt(0);
99                                                        //sprintf(data1,"#12L #15L #14L #13H \r");
100                                                }
101                        DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
102                        BehaviourOut.clear();
103                        Sleep(500);
104                        }
105                }
106                BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);
107                //sprintf(data1,"#12H #13H #14H #15H \r");
108                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
109                BehaviourOut.clear();
110                Sleep(1000);
111                BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);BehaviourOut.addInt(0);
112                //sprintf(data1,"#12L #13L #14L #15L \r");
113                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
114                BehaviourOut.clear();
115                Sleep(500);
116                BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);     BehaviourOut.addInt(100);
117                //sprintf(data1,"#12H #13H #14H #15H \r");
118                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
119                BehaviourOut.clear();
120                //write(*fd,data1,sizeof(data1)); 
121                       
122      /*
123      x[6]=2400;x[5]=1600;x[4]=800;x[3]=0;
124      x[2]=2400;x[1]=1600;x[0]=800;
125      sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r", 2400,    0,    0,    0);
126      write(*fd,data1,sizeof(data1));
127      ArUtil::sleep(500);
128      sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r", 1600, 2400,    0,    0);
129      write(*fd,data1,sizeof(data1));
130      ArUtil::sleep(500);
131      for (jj=0; jj<5; jj++)
132      {
133        for (ii=0; ii<4; ii++){
134          sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r", x[3-ii], x[4-ii], x[5-ii], x[6-ii]);
135          write(*fd,data1,sizeof(data1));
136          ArUtil::sleep(500);
137        }
138      }
139      */
140/*     
141        sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r",  800, 1600, 2400,    0);
142        sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r",    0,  800, 1600, 2400);
143        sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r", 2400,    0,  800, 1600);
144        sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r", 1600, 2400,    0,  800);
145        sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r",  800, 1600, 2400,    0);
146        P12=     
147      #12 P2400 #13 P1600 #14 P800 #15 P0
148*/       
149                break;
150
151        case 2:  //Happy
152                printf(" Happy -2 \n");
153                for (ii=0; ii<10; ii++){
154                        BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);
155                        //sprintf(data1,"#12H #13H #14H #15H \r");
156                        DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
157                        BehaviourOut.clear();
158                        Sleep(500);     
159                        BehaviourOut.addInt(100);BehaviourOut.addInt(0);BehaviourOut.addInt(100);BehaviourOut.addInt(0);
160                        //sprintf(data1,"#12H #13L #14H #15L \r");
161                        DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
162                        BehaviourOut.clear();
163                        Sleep(500);
164                }
165                BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);
166                //sprintf(data1,"#12H #13H #14H #15H \r");
167                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
168                BehaviourOut.clear();
169                Sleep(500);     
170                break;
171
172        case 3:  //Excited
173                for (ii=0; ii<20; ii++){
174                        BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);
175                        //sprintf(data1,"#12 P%d #13 P2500 #14 P2500 #15 P2500 T100\r",2500);
176                        DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
177                        BehaviourOut.clear();
178                        Sleep(200);
179                        BehaviourOut.addInt(0);BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);
180                        //sprintf(data1,"#12 P%d #13 P2500 #14 P2500 #15 P2500 T100\r",0);
181                        DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
182                        BehaviourOut.clear();
183                        Sleep(200);
184                }
185                BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);BehaviourOut.addInt(100);
186                //sprintf(data1,"#12H #13H #14H #15H \r");
187                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
188                BehaviourOut.clear();
189                Sleep(500);     
190                break;   
191        case 4: //Bored
192                        jj=100;
193                        BehaviourOut.addInt(jj);BehaviourOut.addInt(jj);BehaviourOut.addInt(jj);BehaviourOut.addInt(jj);
194                        //sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r", jj, jj, jj, jj );
195                        //F100,L100,B100, R100, T100,\r,
196                        DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
197                        BehaviourOut.clear();
198                break;
199
200    case 5:  //Tired
201                for (kk=0; kk<6; kk++){
202                        jj=50;
203                        for (ii=0; ii<20; ii++){
204                                jj+=120;
205                                BehaviourOut.addInt((int)(jj/25));BehaviourOut.addInt((int)(jj/25));BehaviourOut.addInt((int)(jj/25));BehaviourOut.addInt((int)(jj/25));
206                                //sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r", jj, jj, jj, jj );
207                                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
208                                BehaviourOut.clear();
209                                //printf("%d going bright\n", jj);
210                                Sleep(200);
211                        }
212                        Sleep(1000);
213                           
214                        for (ii=0; ii<20; ii++){
215                                jj-=120;
216                                BehaviourOut.addInt((int)(jj/25));BehaviourOut.addInt((int)(jj/25));BehaviourOut.addInt((int)(jj/25));BehaviourOut.addInt((int)(jj/25));
217                                //sprintf(data1,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r", jj, jj, jj, jj );
218                                DisplayRef.SendBottleData("L2DBOut", BehaviourOut);
219                                BehaviourOut.clear();
220                                //printf("%d going dim\n", jj);
221                                Sleep(200);
222                                }
223                        Sleep(1000);
224                }
225                break;
226        default:
227                        printf("\nNo Such Behaviour, I haven't learn that behaviour yet");
228                break;
229
230   }
231}
232
233
234int main(void)
235{
236        Network yarp;            //name                   //Category //subcategory     
237        SamgarModule Display("PioneerDisplay", "Display", "BaseLights", SamgarModule::interupt);
238        Display.AddPortS("L2DBIn");
239        Display.AddPortS("L2DBOut");
240       
241        Bottle BehaviourIn;
242        while (1){
243               
244                        if (Display.GetBottleData("L2DBIn", &BehaviourIn, SamgarModule::NoStep)==true)
245                        {
246                                //printf("\n ---- I got something %d", BehaviourIn.get(0).asInt());
247                                behaviour_Display(Display, BehaviourIn.get(0).asInt());
248                                //printf("\n ---- Done sending Behaviour %d", BehaviourIn.get(0).asInt());
249                                BehaviourIn.clear();
250                        }
251                        Display.SucceedFail(true,100);
252                        printf(".");
253                }
254}
Note: See TracBrowser for help on using the repository browser.