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.nqc $
3
// $Date: 27.04.04 22:51 $
4
// $Revision: 24 $
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
 
33
void cikcak()   // Hledani cary
34
{
35
  n=CIKCAK/2;       // poprve hned zatoc opacne, nez se jelo pred detekci diry
36
  switch(movement)  // zmenime smer zataceni
37
  {
38
   case L:
39
        FL; BR;       // doprava
40
        movement=R;        // poznamenej kam jedem
41
        line=L;            // kdyz prejedem, tak bude cara vlevo
42
     break;
43
   case R:
44
        FR; BL;       // doleva
45
        movement=L;        // poznamenej kam jedem
46
        line=R;            // kdyz prejedem, tak bude cara vpravo
47
     break;
48
  }
49
 
50
  while(true)   // jed cik-cak, dokud nenajdes caru
51
  {
52
    if ((THRESHOLD < RSENSOR) || (THRESHOLD < RSENSOR))   // je cara ?
53
    {
54
         STOP;
55
         break;
56
    };
57
    if (CIKCAK < n++)  // Jedeme uz dost dlouho cik? Pak jed cak.
58
    {
59
       n=0;
60
       STOP;
61
       switch(movement)     // zmenime smer zataceni
62
       {
63
         case L:
64
              FL; BR;       // doprava
65
              movement=R;        // poznamenej kam jedem
66
              line=L;            // kdyz prejedem, tak bude cara vlevo
67
           break;
68
         case R:
69
              FR; BL;       // doleva
70
              movement=L;        // poznamenej kam jedem
71
              line=R;            // kdyz prejedem, tak bude cara vpravo
72
           break;
73
       }
74
    }
75
  }
76
}
77
 
78
task main()
79
{
80
  PlaySound (SOUND_DOUBLE_BEEP);
81
  Wait(100); // 1s
82
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
83
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
84
  SetSensor(BUMPER,SENSOR_TOUCH);    // sensor na prekazku
85
 
86
  SetPower (ML,OUT_FULL);     // vykon motoru
87
  SetPower (MR,OUT_FULL);
88
  movement=R;
89
  line=S;
90
  dira=0;
91
 
92
  cikcak();    // toc se, abys nasel caru
93
  FL; FR;      // vpred
94
 
95
  // Sledovani cary
96
  while(true)
97
  {
98
    if(BUMPER)  // je cihla?
99
    {
100
      PlaySound(SOUND_DOWN);
101
      objizdka();     // objed cihlu
102
    }
103
 
104
    if (THRESHOLD < RSENSOR)    // Cara pod pravym senzorem
105
    {
106
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
107
      line=R;    // zaznamenej, kdes videl caru
108
      FL; FR;    // rovne
109
      continue;
110
    }
111
 
112
    if (THRESHOLD < LSENSOR)    // Cara pod levym senzorem
113
    {
114
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
115
      line=L;    // zaznamenej, kdes videl caru
116
      FL; FR;    // rovne
117
      continue;
118
    }
119
 
120
    // oba senzory mimo caru
121
 
122
    if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat
123
                 // v ostatnich cyklech nedelame nic (pro urychleni snimani)
124
    {
125
       switch(line)  // kdyz nevidis caru, tak jed tam, kdes ji videl naposled
126
       {
127
         case L:
128
              Off(ML);       // doleva
129
              movement=L;    // poznamenej kam jedes
130
           break;
131
         case R:
132
              Off(MR);       // doprava
133
              movement=R;    // poznamenej kam jedes
134
           break;
135
       };
136
       dira++;   // zvets pocitadlo diry, aby to pristi cyklus tudy neslo
137
       continue; // co nejrychleji se vrat na snimani cary
138
    }
139
 
140
    if (MEZERA < dira++)  // nejedeme uz moc dlouho bez cary?
141
    {
142
      dira=0;
143
      PlaySound (SOUND_FAST_UP);
144
      switch(movement)  // kam se jelo ?
145
      {
146
       case L:
147
            Off(ML);  // couvej doprava
148
            BR;
149
            Wait(COUVEJ+10);
150
            movement=R;
151
         break;
152
       case R:
153
            BL;       // couvej doleva
154
            Off(MR);
155
            Wait(COUVEJ);
156
            movement=L;
157
         break;
158
      }
159
      PlaySound(SOUND_CLICK);
160
      STOP;                    // konec couvani
161
      FL; FR;                  // rovne pres diru
162
      Wait(PRES_DIRU);
163
      STOP;
164
      cikcak();                // najdi caru
165
      FR; FL;                  // vpred
166
    } // dira
167
 
168
  } // while(true)
169
}
170
 
171
void objizdka()
172
{
173
  BL; BR;                 // zacouvej
174
  Wait(15);
175
  STOP;
176
 
177
  cikcak();               // vyrovnej se na caru
178
  cikcak();               // radeji 3x, aby se vyrovnaly setrvacne sily
179
  cikcak();
180
 
181
  BL; FR; Wait(22);               // vlevo
182
  STOP;
183
  FL; FR; Wait(65);               // rovne
184
  STOP;
185
  FL; BR; Wait(22);               // vpravo
186
  STOP;
187
  FL; FR; Wait(77);               // rovne
188
  STOP;
189
  FL; BR; Wait(21);               // vpravo
190
  STOP;
191
  FL; FR;
192
  Wait(20);                       // kousek rovne
193
  // rovne, dokud nenarazis na caru
194
  while(THRESHOLD > RSENSOR);     // pravej senzor, protoze cihla je vpravo
195
  STOP;
196
  BL; FR;                         // toc se doleva, dokud nenarazis na caru
197
  while(THRESHOLD > LSENSOR);     // levej senzor, protoze cara je urcite vlevo
198
  STOP;
199
  line=R;      // caru predpokladame vpravo, kdybysme ji prejeli
200
  dira=0;
201
  FL; FR;                         // vpred
202
}