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 
}