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


// Robot na MiniSumo pro Model Hobby 2004
// $Archive: /Lego/ModelHobby2004.nqc $
// $Date: 2.10.04 10:45 $
// $Revision: 4 $
 
#define THRESHOLD  65      // rozhodovaci uroven mezi cernou a bilou
#define FTHRESHOLD 100     // Rozhodovaci uroven dalkoveho sensoru
#define POHON      OUT_A   // Motory
#define MULETA     OUT_C
#define VPRED      OnFwd(POHON)
#define VZAD       OnRev(POHON)
#define NAHORU     OnRev(MULETA)
#define DOLU       OnFwd(MULETA)
#define PREDNI     SENSOR_1   // Sensory na okraj
#define ZADNI      SENSOR_3
#define BUMPER     SENSOR_2   // Dalkovy sensor
 
task main()
{
  PlaySound (SOUND_DOUBLE_BEEP);
  Wait(400); // 5s podle pravidel - prvni prodleva procedury zapas
  SetSensor(PREDNI,SENSOR_LIGHT);   // senzor na caru predni
  SetSensor(ZADNI,SENSOR_LIGHT);   // senzor na caru zadni
  // sensor na prekazku
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));
 
  SetTxPower(TX_POWER_HI);    // aby daleko videl
 
  SetPower (POHON,OUT_FULL);     // vykon motoru
  SetPower (MULETA,OUT_HALF);
 
  start radar;
  start zapas;
  start nevypadni;
  start toro;
}
 
task toro()
{
  int i=1;
 
  while(true)
  {
     if (i++ & 1) {DOLU;} else {NAHORU;};
     Wait(50);
     Off(MULETA);
     Wait(200);
  }
}
 
task zapas()
{
  Wait(100);       // napodruhe pojedeme dele dozadu
  while(true)
  {
     VZAD;         // mateni telem
     Wait(50);
     Off(POHON);
     Wait(100);
     VPRED;
     Wait(50);
     Off(POHON);
     Wait(100);
     Off(POHON);
     Wait(100);
  }
}
 
task nevypadni()
{
 
  while(true)
  {
     if (THRESHOLD > PREDNI)    // Cara vpredu
     {
        stop zapas;
        stop radar;
        PlaySound (SOUND_FAST_UP);
        VZAD;
        Wait(50);
        start radar;
        start zapas;
     }
     else
     if (THRESHOLD > ZADNI)    // Cara vzadu
     {
        stop zapas;
        stop radar;
        PlaySound (SOUND_FAST_UP);
        VPRED;
        Wait(50);
        start radar;
        start zapas;
     }
  } // while(true)
}
 
task radar() // dalkovy sensor
{
  int lastlevel;
 
  lastlevel = 0;
  while(true)
  {
    SendMessage(0);
    if(lastlevel > BUMPER)  // vidime robota?
    {
      stop zapas;
      PlaySound(SOUND_DOWN);
      VPRED;
      Wait(50);
    }
    lastlevel = BUMPER;
    lastlevel -= FTHRESHOLD;
  }
}
 
 

Rev 1	Rev 5
1	`// Robot na MiniSumo pro Model Hobby 2004`	1	`// Robot na MiniSumo pro Model Hobby 2004`
2	`// $Archive: /Lego/ModelHobby2004.nqc $`	2	`// $Archive: /Lego/ModelHobby2004.nqc $`
3	`// $Date: 2.10.04 10:45 $`	3	`// $Date: 2.10.04 10:45 $`
4	`// $Revision: 4 $`	4	`// $Revision: 4 $`
5		5
6	`#define THRESHOLD 65 // rozhodovaci uroven mezi cernou a bilou`	6	`#define THRESHOLD 65 // rozhodovaci uroven mezi cernou a bilou`
7	`#define FTHRESHOLD 100 // Rozhodovaci uroven dalkoveho sensoru`	7	`#define FTHRESHOLD 100 // Rozhodovaci uroven dalkoveho sensoru`
8	`#define POHON OUT_A // Motory`	8	`#define POHON OUT_A // Motory`
9	`#define MULETA OUT_C`	9	`#define MULETA OUT_C`
10	`#define VPRED OnFwd(POHON)`	10	`#define VPRED OnFwd(POHON)`
11	`#define VZAD OnRev(POHON)`	11	`#define VZAD OnRev(POHON)`
12	`#define NAHORU OnRev(MULETA)`	12	`#define NAHORU OnRev(MULETA)`
13	`#define DOLU OnFwd(MULETA)`	13	`#define DOLU OnFwd(MULETA)`
14	`#define PREDNI SENSOR_1 // Sensory na okraj`	14	`#define PREDNI SENSOR_1 // Sensory na okraj`
15	`#define ZADNI SENSOR_3`	15	`#define ZADNI SENSOR_3`
16	`#define BUMPER SENSOR_2 // Dalkovy sensor`	16	`#define BUMPER SENSOR_2 // Dalkovy sensor`
17		17
18	`task main()`	18	`task main()`
19	`{`	19	`{`
20	`PlaySound (SOUND_DOUBLE_BEEP);`	20	`PlaySound (SOUND_DOUBLE_BEEP);`
21	`Wait(400); // 5s podle pravidel - prvni prodleva procedury zapas`	21	`Wait(400); // 5s podle pravidel - prvni prodleva procedury zapas`
22	`SetSensor(PREDNI,SENSOR_LIGHT); // senzor na caru predni`	22	`SetSensor(PREDNI,SENSOR_LIGHT); // senzor na caru predni`
23	`SetSensor(ZADNI,SENSOR_LIGHT); // senzor na caru zadni`	23	`SetSensor(ZADNI,SENSOR_LIGHT); // senzor na caru zadni`
24	`// sensor na prekazku`	24	`// sensor na prekazku`
25	`SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));`	25	`SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));`
26		26
27	`SetTxPower(TX_POWER_HI); // aby daleko videl`	27	`SetTxPower(TX_POWER_HI); // aby daleko videl`
28		28
29	`SetPower (POHON,OUT_FULL); // vykon motoru`	29	`SetPower (POHON,OUT_FULL); // vykon motoru`
30	`SetPower (MULETA,OUT_HALF);`	30	`SetPower (MULETA,OUT_HALF);`
31		31
32	`start radar;`	32	`start radar;`
33	`start zapas;`	33	`start zapas;`
34	`start nevypadni;`	34	`start nevypadni;`
35	`start toro;`	35	`start toro;`
36	`}`	36	`}`
37		37
38	`task toro()`	38	`task toro()`
39	`{`	39	`{`
40	`int i=1;`	40	`int i=1;`
41		41
42	`while(true)`	42	`while(true)`
43	`{`	43	`{`
44	`if (i++ & 1) {DOLU;} else {NAHORU;};`	44	`if (i++ & 1) {DOLU;} else {NAHORU;};`
45	`Wait(50);`	45	`Wait(50);`
46	`Off(MULETA);`	46	`Off(MULETA);`
47	`Wait(200);`	47	`Wait(200);`
48	`}`	48	`}`
49	`}`	49	`}`
50		50
51	`task zapas()`	51	`task zapas()`
52	`{`	52	`{`
53	`Wait(100); // napodruhe pojedeme dele dozadu`	53	`Wait(100); // napodruhe pojedeme dele dozadu`
54	`while(true)`	54	`while(true)`
55	`{`	55	`{`
56	`VZAD; // mateni telem`	56	`VZAD; // mateni telem`
57	`Wait(50);`	57	`Wait(50);`
58	`Off(POHON);`	58	`Off(POHON);`
59	`Wait(100);`	59	`Wait(100);`
60	`VPRED;`	60	`VPRED;`
61	`Wait(50);`	61	`Wait(50);`
62	`Off(POHON);`	62	`Off(POHON);`
63	`Wait(100);`	63	`Wait(100);`
64	`Off(POHON);`	64	`Off(POHON);`
65	`Wait(100);`	65	`Wait(100);`
66	`}`	66	`}`
67	`}`	67	`}`
68		68
69	`task nevypadni()`	69	`task nevypadni()`
70	`{`	70	`{`
71		71
72	`while(true)`	72	`while(true)`
73	`{`	73	`{`
74	`if (THRESHOLD > PREDNI) // Cara vpredu`	74	`if (THRESHOLD > PREDNI) // Cara vpredu`
75	`{`	75	`{`
76	`stop zapas;`	76	`stop zapas;`
77	`stop radar;`	77	`stop radar;`
78	`PlaySound (SOUND_FAST_UP);`	78	`PlaySound (SOUND_FAST_UP);`
79	`VZAD;`	79	`VZAD;`
80	`Wait(50);`	80	`Wait(50);`
81	`start radar;`	81	`start radar;`
82	`start zapas;`	82	`start zapas;`
83	`}`	83	`}`
84	`else`	84	`else`
85	`if (THRESHOLD > ZADNI) // Cara vzadu`	85	`if (THRESHOLD > ZADNI) // Cara vzadu`
86	`{`	86	`{`
87	`stop zapas;`	87	`stop zapas;`
88	`stop radar;`	88	`stop radar;`
89	`PlaySound (SOUND_FAST_UP);`	89	`PlaySound (SOUND_FAST_UP);`
90	`VPRED;`	90	`VPRED;`
91	`Wait(50);`	91	`Wait(50);`
92	`start radar;`	92	`start radar;`
93	`start zapas;`	93	`start zapas;`
94	`}`	94	`}`
95	`} // while(true)`	95	`} // while(true)`
96	`}`	96	`}`
97		97
98	`task radar() // dalkovy sensor`	98	`task radar() // dalkovy sensor`
99	`{`	99	`{`
100	`int lastlevel;`	100	`int lastlevel;`
101		101
102	`lastlevel = 0;`	102	`lastlevel = 0;`
103	`while(true)`	103	`while(true)`
104	`{`	104	`{`
105	`SendMessage(0);`	105	`SendMessage(0);`
106	`if(lastlevel > BUMPER) // vidime robota?`	106	`if(lastlevel > BUMPER) // vidime robota?`
107	`{`	107	`{`
108	`stop zapas;`	108	`stop zapas;`
109	`PlaySound(SOUND_DOWN);`	109	`PlaySound(SOUND_DOWN);`
110	`VPRED;`	110	`VPRED;`
111	`Wait(50);`	111	`Wait(50);`
112	`}`	112	`}`
113	`lastlevel = BUMPER;`	113	`lastlevel = BUMPER;`
114	`lastlevel -= FTHRESHOLD;`	114	`lastlevel -= FTHRESHOLD;`
115	`}`	115	`}`
116	`}`	116	`}`
117		117
118		118

Subversion Repositories svnkaklik

(root)/roboti/istrobot/callis/ModelHobby2004.nqc @ 141 – Rev 1 → 5