Subversion Repositories svnkaklik

Rev

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
    OnRev(MLEFT);
21
    OnFwd(MRIGHT);
22
    movement=S;
23
}
24
 
25
void Left()    // doleva
26
{
27
    Off(MLEFT);
28
    OnFwd(MRIGHT);
29
    movement=L;
30
}
31
 
32
void Right()   // doprava
33
{
34
    OnRev(MLEFT);
35
    Off(MRIGHT);
36
    movement=R;
37
}
38
 
39
task main()
40
{
41
  PlaySound (SOUND_DOUBLE_BEEP);
42
  Wait(100); // 1s
43
  SetSensor(RSENSOR,SENSOR_LIGHT);   // senzor na caru modry pravy
44
  SetSensor(LSENSOR,SENSOR_LIGHT);   // senzor na caru modry levy
45
  // sensor na prekazku
46
  SetSensor(BUMPER,_SENSOR_CFG(SENSOR_TYPE_LIGHT, SENSOR_MODE_RAW));
47
  SetTxPower(TX_POWER_HI);    // aby daleko videl
48
 
49
  movement=S;
50
  line=S;
51
  dira=0;
52
 
53
  start cara;
54
  start cihla;
55
  SetPower (MLEFT,OUT_FULL);
56
  SetPower (MRIGHT,OUT_FULL);
57
}
58
 
59
task cara()
60
{
61
  Straight();
62
  while(true)
63
  {
64
       if (THRESHOLD < RSENSOR)
65
       {
66
          line=R;
67
          dira=0;
68
          switch(movement)
69
          {
70
            case L:
71
               Right();
72
              break;
73
            default:
74
              Straight();
75
          };
76
       }
77
       else
78
       if (THRESHOLD < LSENSOR)
79
       {
80
          line=L;
81
          dira=0;
82
          switch(movement)
83
          {
84
            case R:
85
               Left();
86
              break;
87
            default:
88
              Straight();
89
          };
90
       }
91
       else
92
       {
93
       if (20 < dira++)
94
       {
95
         PlaySound (SOUND_FAST_UP);
96
         switch(movement)
97
         {
98
            case L:
99
               line=R;
100
               Off(MLEFT);     //couvej
101
               OnRev(MRIGHT);
102
              break;
103
            case R:
104
               line=L;
105
               OnFwd(MLEFT);    // couvej
106
               Off(MRIGHT);
107
             break;
108
         };
109
         Wait(50);
110
         Straight();
111
         Wait(5);
112
         dira=0;
113
       };
114
 
115
       switch(line)
116
       {
117
          case L:
118
               Left();
119
            break;
120
          case R:
121
               Right();
122
            break;
123
         };
124
       }
125
  } // while(true)
126
}
127
 
128
 
129
task cihla() // dalkovy sensor
130
{
131
  int lastlevel;
132
 
133
  lastlevel = 0;
134
  while(true)
135
  {
136
    SendMessage(0);
137
    if(lastlevel > BUMPER)
138
    {
139
      PlaySound(SOUND_DOWN);
140
      stop cara;
141
      Off(MLEFT);           // stop
142
      Off(MRIGHT);
143
      Wait(5);
144
      OnRev(MLEFT);           // do leva
145
      OnRev(MRIGHT);
146
      Wait(30);
147
      Off(MLEFT);           // stop
148
      Off(MRIGHT);
149
      Wait(5);
150
      OnRev(MLEFT);           // rovne
151
      OnFwd(MRIGHT);
152
      Wait(70);
153
      Off(MLEFT);           // stop
154
      Off(MRIGHT);
155
      Wait(5);
156
      OnFwd(MLEFT);           // do prava
157
      OnFwd(MRIGHT);
158
      Wait(25);
159
      Off(MLEFT);           // stop
160
      Off(MRIGHT);
161
      Wait(5);
162
      OnRev(MLEFT);           // rovne
163
      OnFwd(MRIGHT);
164
      Wait(80);
165
      Off(MLEFT);           // stop
166
      Off(MRIGHT);
167
      Wait(5);
168
      OnFwd(MLEFT);           // do prava
169
      OnFwd(MRIGHT);
170
      Wait(25);
171
      Off(MLEFT);           // stop
172
      Off(MRIGHT);
173
      Wait(5);
174
      OnRev(MLEFT);           // rovne
175
      OnFwd(MRIGHT);
176
      Wait(60);
177
      OnRev(MLEFT);           // do leva
178
      OnRev(MRIGHT);
179
      Wait(20);
180
      while(THRESHOLD > RSENSOR); // toc se dokud neni cara
181
      Off(MLEFT);           // stop
182
      Off(MRIGHT);
183
      Wait(30);
184
      line=R;
185
      start cara;
186
    }
187
    lastlevel = BUMPER;
188
    lastlevel -= FTHRESHOLD;
189
  }
190
}
191
 
192