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