Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

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