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 100    // Rozhodovaci uroven dalkoveho sensoru
3
#define MLEFT     OUT_C   // Motory
4
#define MRIGHT    OUT_A
5
#define RSENSOR   SENSOR_1   // Sensory na caru
6
#define LSENSOR   SENSOR_3
7
#define BUMPER    SENSOR_2   // Dalkovy sensor
8
 
9
#define L 0  // left
10
#define R 1  // right
11
#define S 2  // straight
12
 
13
int movement;     // smer minuleho pohybu
14
int line;         // na ktere strane byla detekovana cara
15
int dira;         // pocitadlo pro nalezeni preruseni cary
16
int derivace;     // jak moc se bude zachovavat predesly smer
17
 
18
void Straight()   // rovne
19
{
20
    SetPower (MLEFT,OUT_FULL);
21
    SetPower (MRIGHT,OUT_FULL);
22
    OnRev(MLEFT);
23
    OnFwd(MRIGHT);
24
    derivace=5;
25
    movement=S;
26
}
27
 
28
void Left()    // doleva
29
{
30
    SetPower (MLEFT,OUT_LOW);
31
    SetPower (MRIGHT,OUT_FULL);
32
    Off(MLEFT);
33
    OnFwd(MRIGHT);
34
    derivace=3;
35
    movement=L;
36
}
37
 
38
void Right()   // doprava
39
{
40
    SetPower (MLEFT,OUT_FULL);
41
    SetPower (MRIGHT,OUT_LOW);
42
    OnRev(MLEFT);
43
    Off(MRIGHT);
44
    derivace=3;
45
    movement=R;
46
}
47
 
48
task main()
49
{
50
  PlaySound (SOUND_DOUBLE_BEEP);
51
  Wait(100); // 1s
52
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
53
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
54
  // sensor na prekazku
55
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));
56
  SetTxPower(TX_POWER_HI);    // aby daleko videl
57
 
58
  movement=S;
59
  line=S;
60
  dira=0;
61
 
62
//  start cara;
63
  start cihla;
64
 
65
  Straight();
66
  while(true)
67
  {
68
       if (THRESHOLD < RSENSOR)
69
       {
70
           line=R;
71
           dira=0;
72
           Right();
73
           if (THRESHOLD < RSENSOR)
74
           {
75
             while(THRESHOLD < RSENSOR);
76
           }
77
           else
78
 
79
       };
80
       if (THRESHOLD < LSENSOR)
81
       {
82
         if (L==movement)
83
         {
84
            Straight();
85
         }
86
         else
87
         {
88
           line=L;
89
           dira=0;
90
           Left();
91
         };
92
       };
93
  }
94
}
95
 
96
 
97
task cara()
98
{
99
Left();
100
 while(true)
101
 {
102
          if (8 < dira++)
103
          {
104
            PlaySound(SOUND_DOUBLE_BEEP);
105
            if (L==line) line=R; else line=L;
106
            Off(MLEFT);
107
            Off(MRIGHT);
108
            Wait(500);
109
            dira=0;
110
          };
111
 
112
   switch(line)
113
   {
114
      case S:
115
          break;
116
      case L:
117
            Left();
118
          break;
119
 
120
      case R:
121
            Right();
122
          break;
123
   };
124
//   line=S;
125
   Wait(5);
126
   Straight();
127
   Wait(5);
128
 }
129
}
130
 
131
task cihla() // dalkovy sensor
132
{
133
  int lastlevel;
134
 
135
  lastlevel = 0;
136
  while(true)
137
  {
138
    SendMessage(0);
139
    if(lastlevel > BUMPER)
140
    {
141
      PlaySound(SOUND_FAST_UP);
142
      stop cara;
143
      Off(MLEFT);
144
      Off(MRIGHT);
145
      Wait(300);
146
      start cara;
147
    }
148
    lastlevel = BUMPER;
149
    lastlevel -= FTHRESHOLD;
150
  }
151
}
152
 
153