Subversion Repositories svnkaklik

Rev

Go to most recent revision | Details | Last modification | View Log

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