Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 kaklik 1
#include ".\main.h"
2
 
3
#define  KOLMO1      225         // predni kolecko sroubem dopredu
4
#define  KOLMO2      30          // predni kolecko je hlavou sroubu dozadu
5
#define  STRED       128      // sredni poloha zataceciho kolecka
6
#define  BEAR1       12       // 3 stupne zataceni
7
#define  BEAR2       34
8
#define  BEAR3       55
9
#define  SPEEDMAX    140      // maximalni rychlost
10
 
11
#define  L           1     // cara vlevo
12
#define  S           2     // casa mezi sensory
13
#define  R           0     // cara vpravo
14
 
15
// servo
16
#define  SERVO PIN_A2
17
 
18
// IR
19
#define IRTX      PIN_B2
20
#define  CIHLA    PIN_A3
21
 
22
//motory
23
#define  FR         output_low(PIN_A7); output_high(PIN_A6)  // Vpred
24
#define  FL         output_low(PIN_A1); output_high(PIN_A0)
25
#define  BR         output_low(PIN_A6); output_high(PIN_A7)  // Vzad
26
#define  BL         output_low(PIN_A0); output_high(PIN_A1)
27
#define  STOPR      output_low(PIN_A6);output_low(PIN_A7)
28
#define  STOPL      output_low(PIN_A0);output_low(PIN_A1)
29
 
30
//HID
31
#define  LED1     PIN_B1      //oranzova
32
#define  LED2     PIN_B2      //zluta
33
 
34
#define  STROBE   PIN_B0
35
//#define  SW1      PIN_A2      // Motory On/off
36
 
37
unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
38
unsigned int8 line;           // na ktere strane byla detekovana cara
39
//unsigned int8 dira;           // pocita dobu po kterou je ztracena cara
40
unsigned int8 uhel;           // urcuje aktualni uhel zataceni
41
//unsigned int8 speed;          // maximalni povolena rychlost
42
unsigned int8 rovinka;        // pocitadlo na zjisteni rovinky
43
 
44
signed int16  Lmotor;         // promene, ktere urcuji velikost vykonu na levem
45
signed int16  Rmotor;         // a pravem motoru
46
 
47
// makro pro PWM pro motory
48
#define GO(motor, direction, power) if(get_timer0()<=power) \
49
{direction##motor;} else {stop##motor;}
50
 
51
////////////////////////////////////////////////////////////////////////////////
52
/*
53
void diagnostika()
54
{
55
   if(input(SW1))STOPR;STOPL;While(TRUE);
56
//   if(LSENSOR==true) output_high(LED2); else output_low(LED2);
57
//   if(RSENSOR==true) output_high(LED1); else output_low(LED1);
58
}
59
*/
60
////////////////////////////////////////////////////////////////////////////////
61
#int_TIMER2
62
TIMER2_isr()      // ovladani serva
63
{
64
   unsigned int8 n;
65
 
66
   output_high(SERVO);
67
   delay_us(1000);
68
   for(n=uhel; n>0; n--) Delay_us(2);
69
   output_low(SERVO);
70
}
71
 
72
////////////////////////////////////////////////////////////////////////////////
73
short int IRcheck()                 // potvrdi detekci cihly
74
{
75
   output_high(IRTX);               // vypne vysilac IR
76
   delay_ms(100);
77
 
78
   output_low(STROBE);
79
   sensors = spi_read(0);         // cteni senzoru
80
   sensors=~sensors;
81
   output_high(STROBE);
82
 
83
   if(true==bit_test(sensors,7))    // otestuje, jestli je stale detekovan IR signal
84
   {
85
      output_low(IRTX);             // zapne vysilac IR
86
      delay_ms(100);
87
 
88
      output_low(STROBE);
89
      sensors = spi_read(0);         // cteni senzoru
90
      sensors=~sensors;
91
      output_high(STROBE);
92
 
93
      if(false==bit_test(sensors,7))      // otestuje, jestli je detekovana cihla
94
      {
95
         output_high(IRTX);            // vypne vysilac IR
96
         delay_ms(100);
97
 
98
         output_low(STROBE);
99
         sensors = spi_read(0);         // cteni senzoru
100
         sensors=~sensors;
101
         output_high(STROBE);
102
 
103
         output_low(IRTX);             // zapne vysilac IR
104
         if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla
105
      }
106
   };
107
   output_low(IRTX);             // zapne vysilac IR
108
   return 0; // vrat 0, kdyz je detekovano ruseni
109
}
110
////////////////////////////////////////////////////////////////////////////////
111
void cikcak()
112
{
113
short int movement;
114
 
115
   if(uhel>STRED)
116
   {
117
      movement = R;
118
      uhel=KOLMO1;
119
      Delay_ms(60);
120
   }
121
 
122
   if(uhel<STRED)
123
   {
124
      movement = L;
125
      uhel=KOLMO2;
126
      Delay_ms(60);
127
   }
128
 
129
sem:
130
   sensors = spi_read(0);         // cteni senzoru
131
   sensors=~sensors;
132
   Delay_ms(5);                  // cekani na SLAVE nez pripravi data od cidel
133
   if(!bit_test(sensors,3) == sensors)
134
   {
135
      if(R == movement)
136
      {
137
         FR;BL;
138
         movement=L;
139
         While(bit_test(sensors,3) == sensors)
140
         {
141
            sensors = spi_read(0);         // cteni senzoru
142
            sensors=~sensors;
143
            Delay_ms(5);
144
         }
145
         STOPL;STOPR;
146
         GoTo sem;
147
      }
148
 
149
      if(L == movement)
150
      {
151
         FL;BR;
152
         movement=R;
153
         While(bit_test(sensors,3) == sensors)
154
         {
155
            sensors = spi_read(0);
156
            sensors =~ sensors;
157
            Delay_ms(5);                  // cekani na SLAVE nez pripravi data od cidel
158
         }
159
 
160
         STOPL;STOPR;
161
         GoTo sem;
162
      }
163
   }
164
}
165
////////////////////////////////////////////////////////////////////////////////
166
void objizdka()
167
{
168
   int8 shure=0;
169
   unsigned int16 n;
170
 
171
   BR;BL;
172
   Delay_ms(400);
173
   STOPR;STOPL;
174
 
175
   uhel=KOLMO1;
176
   Delay_ms(100);
177
   BL;FR;
178
   Delay_ms(200);    // minimalni toceni, kdyby se zastavilo sikmo k cihle
179
 
180
   While(bit_test(sensors,7)) // toc, dokud neni cihla z primeho senzoru
181
   {
182
       sensors = spi_read(0);         // cteni senzoru
183
       sensors=~sensors;
184
       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel
185
   }
186
   STOPL; STOPR;
187
 
188
   for (n=0;n<1500;n++)    // vystred se na hranu cihly
189
   {
190
      if(!input(CIHLA)) {BL; FR;} else {FL; BR;};
191
      delay_ms(1);
192
   }
193
   STOPR;STOPL;
194
 
195
   uhel=STRED;          // dopredu
196
   delay_ms(100);   
197
   FR; FL;
198
   delay_ms(500);
199
   BL;BR;
200
   delay_ms(200);
201
   STOPL;STOPR;
202
 
203
   uhel=STRED+BEAR3;    // doprava
204
   delay_ms(100);   
205
   FL;
206
   delay_ms(200);
207
   uhel=STRED+BEAR2;    // min doprava
208
   FL;FR;
209
   delay_ms(200);
210
   uhel=STRED+BEAR1;    // jeste min doprava
211
   FL;FR;
212
   delay_ms(200);
213
   uhel=STRED;          // rovne
214
   FL;FR;
215
   delay_ms(100);
216
   While((sensors & 0b01111111)!=0) //dokud neni cara
217
   {
218
       sensors = spi_read(0);         // cteni senzoru
219
       sensors=~sensors;
220
       Delay_ms(4);              // cekani na SLAVE nez pripravi data od cidel
221
   }
222
   BL; BR;
223
   delay_ms(400);
224
 
225
   uhel=STRED-BEAR3;    // doleva   
226
}
227
 
228
////////////////////////////////////////////////////////////////////////////////
229
void main()
230
{
231
 
232
   unsigned int16 n;
233
   unsigned int8 i;
234
 
235
   setup_adc_ports(NO_ANALOGS);
236
   setup_adc(ADC_CLOCK_INTERNAL);
237
   setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4);
238
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
239
   setup_timer_1(T1_DISABLED);
240
   setup_timer_2(T2_DIV_BY_16,140,16);
241
   setup_oscillator(OSC_8MHZ|OSC_INTRC);
242
 
243
   STOPR; STOPL;  // zastav motory
244
   Lmotor=0;Rmotor=0;
245
 
246
   uhel=STRED;    // nastav zadni kolecko na stred
247
   rovinka=0;
248
 
249
   enable_interrupts(INT_TIMER2);
250
   enable_interrupts(GLOBAL);
251
 
252
   output_low(IRTX); // zapni IR vysilac
253
 
254
   delay_ms(1000);
255
 
256
   while(true)
257
   {
258
 
259
      GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory
260
 
261
      delay_us(1500);
262
 
263
      output_low(STROBE);
264
      sensors = spi_read(0);         // cteni senzoru
265
      sensors=~sensors;
266
      output_high(STROBE);
267
 
268
      i=0;                    // havarijni kod
269
      for (n=0; n<=6; n++)
270
      {
271
         if(bit_test(sensors,n)) i++;
272
      }
273
      if (i>3) While(true){STOPR; STOPL;}; // zastavi, kdyz je cerno pod vice nez tremi cidly
274
 
275
      if(bit_test(sensors,7))    // detekce dihly
276
      {
277
         objizdka();
278
      }
279
 
280
      if(bit_test(sensors,3)) //...|...//
281
      {
282
         uhel=STRED;
283
         Lmotor=SPEEDMAX;
284
         Rmotor=SPEEDMAX;
285
         line=S;
286
         if (rovinka<255) rovinka++;
287
         continue;
288
      }
289
 
290
      if(bit_test(sensors,0)) //|......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
291
      {
292
         uhel=STRED-BEAR3;
293
         Lmotor=0;
294
         Rmotor=SPEEDMAX;
295
         line=L;
296
         continue;
297
      }
298
 
299
      if(bit_test(sensors,6)) //......|//
300
      {
301
         uhel=STRED+BEAR3;
302
         Rmotor=0;
303
         Lmotor=SPEEDMAX;
304
         line=R;
305
         continue;
306
      }
307
 
308
      if(bit_test(sensors,1)) //.|.....//
309
      {
310
         uhel=STRED-BEAR2;
311
         Lmotor=SPEEDMAX-50;
312
         Rmotor=SPEEDMAX;
313
         line=S;
314
         continue;
315
      }
316
 
317
      if(bit_test(sensors,5)) //.....|.//
318
      {
319
         uhel=STRED+BEAR2;
320
         Rmotor=SPEEDMAX-50;
321
         Lmotor=SPEEDMAX;
322
         line=S;
323
         continue;
324
      }
325
 
326
      if (bit_test(sensors,2)) //..|....//
327
      {
328
         uhel=STRED-BEAR1;
329
         Lmotor=SPEEDMAX;
330
         Rmotor=SPEEDMAX;
331
         line=S;
332
         if (rovinka<255) rovinka++;
333
         continue;
334
      }
335
 
336
      if (bit_test(sensors,4)) //....|..//
337
      {
338
         uhel=STRED+BEAR1;
339
         Rmotor=SPEEDMAX;
340
         Lmotor=SPEEDMAX;
341
         line=S;
342
         if (rovinka<255) rovinka++;
343
         continue;
344
      }
345
 
346
      if ((L==line) || (R==line)) // Brzdeni pri vyjeti z trate
347
      {
348
         if (rovinka>250)
349
         {
350
            BL; BR;
351
            Delay_ms(100);
352
         };
353
         rovinka=0;
354
      };
355
 
356
   }
357
}