Subversion Repositories svnkaklik

Rev

Go to most recent revision | Details | Last modification | View Log

Rev Author Line No. Line
1 kaklik 1 
#define THRESHOLD  44     // rozhodovaci uroven mezi cernou a bilou
2 
#define FTHRESHOLD 150    // Rozhodovaci uroven dalkoveho sensoru
3 
#define RED_WHITE  60     // maximum bile na cervenem senzoru
4 
#define MLEFT     OUT_C
5 
#define MRIGHT    OUT_A
6 
#define RSENSOR   SENSOR_1
7 
#define LSENSOR   SENSOR_3
8 
#define BUMPER    SENSOR_2
9 
 
10 
#define L 0
11 
#define R 1
12 
#define S 2
13 
 
14 
int line;
15 
 
16 
task main()
17 
{
18 
  PlaySound (SOUND_DOUBLE_BEEP);
19 
  Wait(100); // 1s
20 
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
21 
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
22 
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW)); // senzor na prekazku
23 
  SetTxPower(TX_POWER_HI);    // aby daleko videl
24 
 
25 
//  start cihla;
26 
//  start cara;
27 
    SetPower (MLEFT,OUT_HALF);
28 
    SetPower (MRIGHT,OUT_HALF);
29 
    OnRev(MLEFT);         // rozjed se vpred
30 
    OnFwd(MRIGHT);
31 
 
32 
    line=S;
33 
 
34 
  while(true)
35 
  {
36 
    if (THRESHOLD < RSENSOR)
37 
    {
38 
      line=R;
39 
    SetPower (MRIGHT,OUT_LOW);
40 
    OnRev(MLEFT);
41 
    };
42 
    if (THRESHOLD < LSENSOR)
43 
    {
44 
      line=L;
45 
    SetPower (MLEFT,OUT_LOW);
46 
    OnFwd(MRIGHT);
47 
    };
48 
 
49 
    if ((THRESHOLD > LSENSOR)&&(THRESHOLD > RSENSOR))
50 
    {
51 
       if(L==line)
52 
       {
53 
         Off(MLEFT);
54 
         OnFwd(MRIGHT);
55 
         while(THRESHOLD > LSENSOR);
56 
         while(THRESHOLD < LSENSOR);
57 
       };
58 
       if(R==line)
59 
       {
60 
         Off(MRIGHT);
61 
         OnRev(MLEFT);
62 
         while(THRESHOLD > RSENSOR);
63 
         while(THRESHOLD < RSENSOR);
64 
       };
65 
       line=S;
66 
       SetPower (MLEFT,OUT_HALF);
67 
       SetPower (MRIGHT,OUT_HALF);
68 
       OnRev(MLEFT);         // rozjed se vpred
69 
       OnFwd(MRIGHT);
70 
    }
71 
  }
72 
}
73 
 
74 
task cara()
75 
{
76 
  while(true)
77 
  {
78 
    OnRev(MLEFT);         // rozjed se vpred
79 
    OnFwd(MRIGHT);
80 
    if (THRESHOLD < RSENSOR)
81 
    {
82 
       OnRev(MRIGHT);
83 
       Wait(1);
84 
       Off(MRIGHT);
85 
    };
86 
    if (THRESHOLD < LSENSOR)
87 
    {
88 
       OnFwd(MLEFT);
89 
       Wait(1);
90 
       Off(MLEFT);
91 
    };
92 
    while((THRESHOLD < RSENSOR)||(THRESHOLD < LSENSOR));
93 
  }
94 
}
95 
 
96 
task cihla() // dalkovy sensor
97 
{
98 
  int lastlevel;
99 
 
100 
  lastlevel = 0;
101 
  while(true)
102 
  {
103 
    SendMessage(0);
104 
    if(lastlevel > BUMPER)
105 
    {
106 
      PlaySound(SOUND_FAST_UP);
107 
      stop cara;
108 
      Off(MLEFT);
109 
      Off(MRIGHT);
110 
      Wait(100);
111 
      start cara;
112 
    }
113 
    lastlevel = BUMPER;
114 
    lastlevel -= FTHRESHOLD;
115 
  }
116 
}
117 
 
118