Subversion Repositories svnkaklik

Rev

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

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