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 
// $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