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