WebSVN – svnkaklik – Diff – /roboti/istrobot/callis/Callis.nqc


// Soutezni robot pro Istrobot 2004
// $Archive: /Lego/Callis/callis.nqc $
// $Date: 27.04.04 22:51 $
// $Revision: 24 $
 
#define THRESHOLD  50         // rozhodovaci uroven mezi cernou a bilou
#define ML         OUT_A      // Motory
#define MR         OUT_C
#define FL         OnFwd(ML)  // Vpred
#define FR         OnRev(MR)
#define BL         OnRev(ML)  // Vzad
#define BR         OnFwd(MR)
#define RSENSOR    SENSOR_3   // Senzory na caru
#define LSENSOR    SENSOR_1
#define BUMPER     SENSOR_2   // Senzor na cihlu
#define MEZERA     38         // jak se muze jet dlouho bez cary
#define COUVEJ     70         // kolik se ma couvat po detekci diry
#define CIKCAK     40         // kolik se ma jet cik/cak
#define PRES_DIRU  35         // jak predpokladame velkou diru
 
#define L 0  // left
#define R 1  // right
#define S 2  // straight
 
#define STOP    Off(ML + MR); Wait(10);    // zastav  8
 
int movement;     // smer minuleho pohybu
int line;         // na ktere strane byla detekovana cara
int dira;         // pocitadlo pro nalezeni preruseni cary
int n;            // pomocna promena pro cyklus FOR
 
 
void cikcak()   // Hledani cary
{
  n=CIKCAK/2;       // poprve hned zatoc opacne, nez se jelo pred detekci diry
  switch(movement)  // zmenime smer zataceni
  {
   case L:
        FL; BR;       // doprava
        movement=R;        // poznamenej kam jedem
        line=L;            // kdyz prejedem, tak bude cara vlevo
     break;
   case R:
        FR; BL;       // doleva
        movement=L;        // poznamenej kam jedem
        line=R;            // kdyz prejedem, tak bude cara vpravo
     break;
  }
 
  while(true)   // jed cik-cak, dokud nenajdes caru
  {
    if ((THRESHOLD < RSENSOR) || (THRESHOLD < RSENSOR))   // je cara ?
    {
         STOP;
         break;
    };
    if (CIKCAK < n++)  // Jedeme uz dost dlouho cik? Pak jed cak.
    {
       n=0;
       STOP;
       switch(movement)     // zmenime smer zataceni
       {
         case L:
              FL; BR;       // doprava
              movement=R;        // poznamenej kam jedem
              line=L;            // kdyz prejedem, tak bude cara vlevo
           break;
         case R:
              FR; BL;       // doleva
              movement=L;        // poznamenej kam jedem
              line=R;            // kdyz prejedem, tak bude cara vpravo
           break;
       }
    }
  }
}
 
task main()
{
  PlaySound (SOUND_DOUBLE_BEEP);
  Wait(100); // 1s
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
  SetSensor(BUMPER,SENSOR_TOUCH);    // sensor na prekazku
 
  SetPower (ML,OUT_FULL);     // vykon motoru
  SetPower (MR,OUT_FULL);
  movement=R;
  line=S;
  dira=0;
  
  cikcak();    // toc se, abys nasel caru
  FL; FR;      // vpred
 
  // Sledovani cary
  while(true)
  {
    if(BUMPER)  // je cihla?
    {
      PlaySound(SOUND_DOWN);
      objizdka();     // objed cihlu
    }
 
    if (THRESHOLD < RSENSOR)    // Cara pod pravym senzorem
    {
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
      line=R;    // zaznamenej, kdes videl caru
      FL; FR;    // rovne
      continue;
    }
 
    if (THRESHOLD < LSENSOR)    // Cara pod levym senzorem
    {
      dira=0;    // nuluj pocitadlo diry, protoze jsme videli caru
      line=L;    // zaznamenej, kdes videl caru
      FL; FR;    // rovne
      continue;
    }
 
    // oba senzory mimo caru
 
    if (0==dira) // v prvnim cyklu po ztrate cary zacneme zahybat
                 // v ostatnich cyklech nedelame nic (pro urychleni snimani)
    {
       switch(line)  // kdyz nevidis caru, tak jed tam, kdes ji videl naposled
       {
         case L:
              Off(ML);       // doleva
              movement=L;    // poznamenej kam jedes
           break;
         case R:
              Off(MR);       // doprava
              movement=R;    // poznamenej kam jedes
           break;
       };
       dira++;   // zvets pocitadlo diry, aby to pristi cyklus tudy neslo
       continue; // co nejrychleji se vrat na snimani cary
    }
 
    if (MEZERA < dira++)  // nejedeme uz moc dlouho bez cary?
    {
      dira=0;
      PlaySound (SOUND_FAST_UP);
      switch(movement)  // kam se jelo ?
      {
       case L:
            Off(ML);  // couvej doprava
            BR;
            Wait(COUVEJ+10);
            movement=R;
         break;
       case R:
            BL;       // couvej doleva
            Off(MR);
            Wait(COUVEJ);
            movement=L;
         break;
      }
      PlaySound(SOUND_CLICK);
      STOP;                    // konec couvani
      FL; FR;                  // rovne pres diru
      Wait(PRES_DIRU);
      STOP;
      cikcak();                // najdi caru
      FR; FL;                  // vpred
    } // dira
    
  } // while(true)
}
 
void objizdka()
{
  BL; BR;                 // zacouvej
  Wait(15);
  STOP;
 
  cikcak();               // vyrovnej se na caru
  cikcak();               // radeji 3x, aby se vyrovnaly setrvacne sily
  cikcak();
 
  BL; FR; Wait(22);               // vlevo
  STOP;
  FL; FR; Wait(65);               // rovne
  STOP;
  FL; BR; Wait(22);               // vpravo
  STOP;
  FL; FR; Wait(77);               // rovne
  STOP;
  FL; BR; Wait(21);               // vpravo
  STOP;
  FL; FR;
  Wait(20);                       // kousek rovne
  // rovne, dokud nenarazis na caru
  while(THRESHOLD > RSENSOR);     // pravej senzor, protoze cihla je vpravo
  STOP;
  BL; FR;                         // toc se doleva, dokud nenarazis na caru
  while(THRESHOLD > LSENSOR);     // levej senzor, protoze cara je urcite vlevo
  STOP;
  line=R;      // caru predpokladame vpravo, kdybysme ji prejeli
  dira=0;
  FL; FR;                         // vpred
}

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

Subversion Repositories svnkaklik

(root)/roboti/istrobot/callis/Callis.nqc @ 360 – Rev 1 → 5