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