Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
1 kaklik 1
// Soutezni robot pro Istrobot 2004
2
// $Archive: /Lego/Callis/Callis_log.nqc $
3
// $Date: 9.05.04 6:34 $
4
// $Revision: 1 $
5
 
6
#define THRESHOLD  50         // rozhodovaci uroven mezi cernou a bilou
7
#define ML         OUT_A      // Motory
8
#define MR         OUT_C
9
#define FL         OnFwd(ML)  // Vpred
10
#define FR         OnRev(MR)
11
#define BL         OnRev(ML)  // Vzad
12
#define BR         OnFwd(MR)
13
#define RSENSOR    SENSOR_3   // Senzory na caru
14
#define LSENSOR    SENSOR_1
15
#define BUMPER     SENSOR_2   // Senzor na cihlu
16
#define MEZERA     38         // jak se muze jet dlouho bez cary
17
#define COUVEJ     70         // kolik se ma couvat po detekci diry
18
#define CIKCAK     40         // kolik se ma jet cik/cak
19
#define PRES_DIRU  35         // jak predpokladame velkou diru
20
 
21
#define L 0  // left
22
#define R 1  // right
23
#define S 2  // straight
24
 
25
#define STOP    Off(ML + MR); Wait(10);    // zastav  8
26
 
27
int movement;     // smer minuleho pohybu
28
int line;         // na ktere strane byla detekovana cara
29
int dira;         // pocitadlo pro nalezeni preruseni cary
30
int n;            // pomocna promena pro cyklus FOR
31
 
32
task main()
33
{
34
  PlaySound (SOUND_DOUBLE_BEEP);
35
  Wait(100); // 1s
36
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
37
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
38
  SetSensor(BUMPER,SENSOR_TOUCH);    // sensor na prekazku
39
  SetEvent(L, LSENSOR, EVENT_TYPE_PRESSED);
40
  SetEvent(R, RSENSOR, EVENT_TYPE_PRESSED);
41
 
42
  SetPower (ML,OUT_HALF);     // vykon motoru
43
  SetPower (MR,OUT_HALF);
44
  movement=R;
45
  line=S;
46
  dira=0;
47
 
48
//  cikcak();    // toc se, abys nasel caru
49
//!!!  FL; FR;      // vpred
50
 
51
  CreateDatalog(1000);
52
 
53
  // Sledovani cary
54
  while(true)
55
  {
56
 
57
    monitor( EVENT_MASK(L) | EVENT_MASK(R) )
58
    {
59
//!!!      FL; FR;    // rovne
60
Wait(1);
61
    }
62
    catch (EVENT_MASK(L))
63
    {
64
SetUserDisplay(2, 1);
65
PlaySound(SOUND_CLICK);
66
Wait(50);
67
      line=L;    // zaznamenej, kdes videl caru
68
      n=-dira;
69
//      AddToDatalog(n);
70
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
71
    }
72
    catch (EVENT_MASK(R))
73
    {
74
SetUserDisplay(3, 1);
75
PlaySound(SOUND_LOW_BEEP);
76
Wait(50);
77
      line=R;    // zaznamenej, kdes videl caru
78
      n=dira;
79
//      AddToDatalog(n);
80
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
81
    }
82
SetUserDisplay(1, 1);
83
Wait(50);
84
 
85
/*
86
    // oba senzory mimo caru
87
 
88
    if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat
89
                 // v ostatnich cyklech nedelame nic (pro urychleni snimani)
90
    {
91
       switch(line)  // kdyz nevidis caru, tak jed tam, kdes ji videl naposled
92
       {
93
         case L:
94
              Off(ML);       // doleva
95
              movement=L;    // poznamenej kam jedes
96
           break;
97
         case R:
98
              Off(MR);       // doprava
99
              movement=R;    // poznamenej kam jedes
100
           break;
101
       };
102
       dira++;   // zvets pocitadlo diry, aby to pristi cyklus tudy neslo
103
       continue; // co nejrychleji se vrat na snimani cary
104
    }
105
 
106
    if (MEZERA < dira++)  // nejedeme uz moc dlouho bez cary?
107
    {
108
      dira=0;
109
      PlaySound (SOUND_FAST_UP);
110
      switch(movement)  // kam se jelo ?
111
      {
112
       case L:
113
            Off(ML);  // couvej doprava
114
            BR;
115
            Wait(COUVEJ+10);
116
            movement=R;
117
         break;
118
       case R:
119
            BL;       // couvej doleva
120
            Off(MR);
121
            Wait(COUVEJ);
122
            movement=L;
123
         break;
124
      }
125
      PlaySound(SOUND_CLICK);
126
      STOP;                    // konec couvani
127
      FL; FR;                  // rovne pres diru
128
      Wait(PRES_DIRU);
129
      STOP;
130
      cikcak();                // najdi caru
131
      FR; FL;                  // vpred
132
    } // dira
133
*/
134
 
135
  } // while(true)
136
}
137
 
138
 
139
void cikcak()   // Hledani cary
140
{
141
  n=CIKCAK/2;       // poprve hned zatoc opacne, nez se jelo pred detekci diry
142
  switch(movement)  // zmenime smer zataceni
143
  {
144
   case L:
145
        FL; BR;       // doprava
146
        movement=R;        // poznamenej kam jedem
147
        line=L;            // kdyz prejedem, tak bude cara vlevo
148
     break;
149
   case R:
150
        FR; BL;       // doleva
151
        movement=L;        // poznamenej kam jedem
152
        line=R;            // kdyz prejedem, tak bude cara vpravo
153
     break;
154
  }
155
 
156
  while(true)   // jed cik-cak, dokud nenajdes caru
157
  {
158
    if ((THRESHOLD < RSENSOR) || (THRESHOLD < RSENSOR))   // je cara ?
159
    {
160
         STOP;
161
         break;
162
    };
163
    if (CIKCAK < n++)  // Jedeme uz dost dlouho cik? Pak jed cak.
164
    {
165
       n=0;
166
       STOP;
167
       switch(movement)     // zmenime smer zataceni
168
       {
169
         case L:
170
              FL; BR;       // doprava
171
              movement=R;        // poznamenej kam jedem
172
              line=L;            // kdyz prejedem, tak bude cara vlevo
173
           break;
174
         case R:
175
              FR; BL;       // doleva
176
              movement=L;        // poznamenej kam jedem
177
              line=R;            // kdyz prejedem, tak bude cara vpravo
178
           break;
179
       }
180
    }
181
  }
182
}
183
 
184