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_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