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_HIGH);
40
  SetEvent(R, RSENSOR, EVENT_TYPE_HIGH);
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
    }
61
    catch (EVENT_MASK(L))
62
    {
63
      line=L;    // zaznamenej, kdes videl caru
64
      n=-dira;
65
      AddToDatalog(n);
66
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
67
    }
68
    catch (EVENT_MASK(R))
69
    {
70
      line=R;    // zaznamenej, kdes videl caru
71
      n=dira;
72
      AddToDatalog(n);
73
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
74
    }
75
 
76
    // oba senzory mimo caru
77
 
78
    if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat
79
                 // v ostatnich cyklech nedelame nic (pro urychleni snimani)
80
    {
81
       switch(line)  // kdyz nevidis caru, tak jed tam, kdes ji videl naposled
82
       {
83
         case L:
84
              Off(ML);       // doleva
85
              movement=L;    // poznamenej kam jedes
86
           break;
87
         case R:
88
              Off(MR);       // doprava
89
              movement=R;    // poznamenej kam jedes
90
           break;
91
       };
92
       dira++;   // zvets pocitadlo diry, aby to pristi cyklus tudy neslo
93
       continue; // co nejrychleji se vrat na snimani cary
94
    }
95
 
96
    if (MEZERA < dira++)  // nejedeme uz moc dlouho bez cary?
97
    {
98
      dira=0;
99
      PlaySound (SOUND_FAST_UP);
100
      switch(movement)  // kam se jelo ?
101
      {
102
       case L:
103
            Off(ML);  // couvej doprava
104
            BR;
105
            Wait(COUVEJ+10);
106
            movement=R;
107
         break;
108
       case R:
109
            BL;       // couvej doleva
110
            Off(MR);
111
            Wait(COUVEJ);
112
            movement=L;
113
         break;
114
      }
115
      PlaySound(SOUND_CLICK);
116
      STOP;                    // konec couvani
117
      FL; FR;                  // rovne pres diru
118
      Wait(PRES_DIRU);
119
      STOP;
120
      cikcak();                // najdi caru
121
      FR; FL;                  // vpred
122
    } // dira
123
 
124
  } // while(true)
125
}
126
 
127
 
128
void cikcak()   // Hledani cary
129
{
130
  n=CIKCAK/2;       // poprve hned zatoc opacne, nez se jelo pred detekci diry
131
  switch(movement)  // zmenime smer zataceni
132
  {
133
   case L:
134
        FL; BR;       // doprava
135
        movement=R;        // poznamenej kam jedem
136
        line=L;            // kdyz prejedem, tak bude cara vlevo
137
     break;
138
   case R:
139
        FR; BL;       // doleva
140
        movement=L;        // poznamenej kam jedem
141
        line=R;            // kdyz prejedem, tak bude cara vpravo
142
     break;
143
  }
144
 
145
  while(true)   // jed cik-cak, dokud nenajdes caru
146
  {
147
    if ((THRESHOLD < RSENSOR) || (THRESHOLD < RSENSOR))   // je cara ?
148
    {
149
         STOP;
150
         break;
151
    };
152
    if (CIKCAK < n++)  // Jedeme uz dost dlouho cik? Pak jed cak.
153
    {
154
       n=0;
155
       STOP;
156
       switch(movement)     // zmenime smer zataceni
157
       {
158
         case L:
159
              FL; BR;       // doprava
160
              movement=R;        // poznamenej kam jedem
161
              line=L;            // kdyz prejedem, tak bude cara vlevo
162
           break;
163
         case R:
164
              FR; BL;       // doleva
165
              movement=L;        // poznamenej kam jedem
166
              line=R;            // kdyz prejedem, tak bude cara vpravo
167
           break;
168
       }
169
    }
170
  }
171
}
172
 
173