Subversion Repositories svnkaklik

Rev

Rev 194 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 194 Rev 195
Line 1... Line 1...
1 
//******** Robot Camerus pro IstRobot 2007 ************
 1 
//******** Robot Camerus pro IstRobot 2007 ************
2 
//"$Id: camerus.c 194 2007-03-17 14:39:11Z kaklik $"
 2 
//"$Id: camerus.c 195 2007-03-17 15:56:26Z kakl $"
3 
//*****************************************************
 3 
//*****************************************************
4 
 
 4 
 
5 
#include ".\camerus.h"
 5 
#include ".\camerus.h"
6 
 
 6 
 
7 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 7 
#USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
Line 42... Line 42...
42 
   #bit RBIF      = INTCON.0
 42 
   #bit RBIF      = INTCON.0
43 
 
 43 
 
44 
enum stavy {start,rozjezd,jizda,cihla,cil};
 44 
enum stavy {start,rozjezd,jizda,cihla,cil};
45 
stavy stav;    // Kde jsme na trati
 45 
stavy stav;    // Kde jsme na trati
46 
int8 cas;      // Cas hrany bila/cerna v radce
 46 
int8 cas;      // Cas hrany bila/cerna v radce
47 
 
 - 
 
- 
 
 47 
int8 stred;    // vystredeni kolecka
48 
 
 48 
 
49 
inline void brzda()
 49 
inline void brzda()
50 
{
 50 
{
51 
   int8 n,i;
 51 
   int8 n,i;
52 
 
 52 
 
Line 77... Line 77...
77 
{
 77 
{
78 
   int8 n, offset;
 78 
   int8 n, offset;
79 
 
 79 
 
80 
   for(n=0; n<14; n++)
 80 
   for(n=0; n<14; n++)
81 
   {
 81 
   {
82 
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
 - 
 
83 
      Delay_ms(1);
 - 
 
84 
      offset=read_adc();
 - 
 
85 
 
 - 
 
86 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
 82 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
87 
      delay_us(1000);
 83 
      delay_us(1000);
88 
      delay_us(offset);
 84 
      delay_us(stred);
89 
      delay_us(offset);
 85 
      delay_us(stred);
90 
      delay_us(offset);
 86 
      delay_us(stred);
91 
      delay_us(angle);
 87 
      delay_us(angle);
92 
      delay_us(angle);
 88 
      delay_us(angle);
93 
      output_low(SERVO);
 89 
      output_low(SERVO);
94 
      delay_ms(18);
 90 
      delay_ms(18);
95 
   }
 91 
   }
Line 97... Line 93...
97 
 
 93 
 
98 
#int_EXT
 94 
#int_EXT
99 
EXT_isr()   // Preruseni od prekazky
 95 
EXT_isr()   // Preruseni od prekazky
100 
{
 96 
{
101 
   int8 i;
 97 
   int8 i;
102
98 
 
103 
   set_pwm1_duty(0);    // reverz (zabrzdi)
 99 
   set_pwm1_duty(0);    // reverz (zabrzdi)
104 
   set_pwm2_duty(0);
 100 
   set_pwm2_duty(0);
105 
   output_high(MOT_L);  // vzad
 101 
   output_high(MOT_L);  // vzad
106 
   output_high(MOT_R);
 102 
   output_high(MOT_R);
107 
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
 103 
   // Ujistime se, ze prijaty signal je z naseho IR vysilace
Line 120... Line 116...
120 
      output_low(MOT_L); // neni odraz -> vpred
 116 
      output_low(MOT_L); // neni odraz -> vpred
121 
      output_low(MOT_R);
 117 
      output_low(MOT_R);
122 
      return;
 118 
      return;
123 
   };
 119 
   };
124 
 
 120 
 
- 
 
 121 
   set_adc_channel(DALKOMER);   // Prepni A/D prevodnik na detektor cihly
125 
   SetServo((CASAVR-CASMIN));   // rovne
 122 
   SetServo((CASAVR-CASMIN));   // rovne
126 
 
 123 
 
127 
   set_pwm1_duty(140);  // vpred
 124 
   set_pwm1_duty(100);  // vpred
128 
   set_pwm2_duty(140);
 125 
   set_pwm2_duty(100);
129 
   output_low(MOT_L);
 126 
   output_low(MOT_L);
130 
   output_low(MOT_R);
 127 
   output_low(MOT_R);
131 
   i=0;
 128 
   i=0;
132 
   while(true)
 129 
   while(true)
133 
   {
 130 
   {
134 
     while(input(ODO)) if(read_adc()<128) goto brzdi; // Je cihla blizko?
 131 
     while(input(ODO)) if(read_adc()<128) goto brzdi; // Je cihla blizko?
135 
     while(!input(ODO));
 132 
     while(!input(ODO));
136 
     i++;
 133 
     i++;
137 
     if(i==8) return; // nedojeli jsme k cihle, jed dal
 134 
     if(i==8) return; // nedojeli jsme k cihle, jed dal
138 
   }
 135 
   };
139 
brzdi:
136 
brzdi:
140 
   set_pwm1_duty(0);    // reverz (zabrzdi)
 137 
   set_pwm1_duty(0);    // reverz (zabrzdi)
141 
   set_pwm2_duty(0);
 138 
   set_pwm2_duty(0);
142 
   output_high(MOT_L);
 139 
   output_high(MOT_L);
143 
   output_high(MOT_R);
 140 
   output_high(MOT_R);
144 
   delay_ms(100);
 141 
   delay_ms(100);
Line 331... Line 328...
331 
 
 328 
 
332 
   cas=CASAVR-CASMIN;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
 329 
   cas=CASAVR-CASMIN;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
333 
   stav=start;
 330 
   stav=start;
334 
   trasa=0;
 331 
   trasa=0;
335 
 
 332 
 
336 
   // ... Hlavni smycka ...
 333 
   // ......... Hlavni smycka .........
337 
   while(true)
 334 
   while(true)
338 
   {
 335 
   {
339 
      int8 pom;
 336 
      int8 pom;
340 
      int8 n;
 337 
      int8 n;
341 
 
 338 
 
Line 357... Line 354...
357 
 
 354 
 
358 
      if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
 355 
      if((pom<CASMAX) && (pom>CASMIN)) cas=pom;  // Orizni konce radku
359 
      // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
 356 
      // Na konci obrazovaho radku to blbne. Jednak chyba od apertury
360 
      // a vubec to nejak na kraji nefunguje.
 357 
      // a vubec to nejak na kraji nefunguje.
361 
 
 358 
 
362 
      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
 - 
 
363 
      Delay_ms(1);
 - 
 
364 
      offset=read_adc();
 - 
 
365 
 
 - 
 
366 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
 359 
      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
367 
      delay_us(1000);
 360 
      delay_us(1000);
368 
      delay_us(offset);
 361 
      delay_us(stred);
369 
      delay_us(offset);
 362 
      delay_us(stred);
370 
      delay_us(offset);
 363 
      delay_us(stred);
371 
      delay_us(cas);
 364 
      delay_us(cas);
372 
      delay_us(cas);
 365 
      delay_us(cas);
373 
      output_low(SERVO);
 366 
      output_low(SERVO);
374 
 
 367 
 
375 
      // Elektronicky diferencial
 368 
      // Elektronicky diferencial
Line 393... Line 386...
393 
      {
 386 
      {
394 
         set_pwm1_duty(0);       // Zastaveni
 387 
         set_pwm1_duty(0);       // Zastaveni
395 
         set_pwm2_duty(0);
 388 
         set_pwm2_duty(0);
396 
      }
 389 
      }
397 
 
 390 
 
398 
      set_adc_channel(DALKOMER);   // Prepni A/D prevodnik na detektor cihly
 - 
 
399 
      Delay_ms(1);
 - 
 
400 
 
 - 
 
401 
      if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
 391 
      if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
402 
      {
 392 
      {
403 
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
 393 
         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
404 
         INT0IF=0;                     // Zruseni predesle udalosti od startera
 394 
         INT0IF=0;                     // Zruseni predesle udalosti od startera
405 
         enable_interrupts(INT_EXT);
 395 
         enable_interrupts(INT_EXT);
406 
         enable_interrupts(GLOBAL);
 396 
         enable_interrupts(GLOBAL);
407 
      };
 397 
      };
408 
 
 398 
 
409 
      if(stav==start)         // Snimkuje, toci servem a ceka na start
 399 
      if(stav==start)         // Snimkuje, toci servem a ceka na start
410 
      if(read_adc()<128)
 - 
 
411 
      {
 400 
      {
- 
 
 401 
         set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
- 
 
 402 
         Delay_ms(1);
- 
 
 403 
         stred=read_adc();
- 
 
 404 
         set_adc_channel(DALKOMER);   // Prepni A/D prevodnik na detektor cihly
- 
 
 405 
         Delay_ms(1);
- 
 
 406 
         if(read_adc()<128)
- 
 
 407 
         {
412 
         disp(0x80);
 408 
            disp(0x80);
413 
         while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
 409 
            while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
414 
         set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
 410 
            set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
415 
         set_pwm2_duty(255);
 411 
            set_pwm2_duty(255);
416 
         disp(0x1);
 412 
            disp(0x1);
417 
         delay_ms(400);
 413 
            delay_ms(400);
418 
         stav=jizda;
 414 
            stav=jizda;
- 
 
 415 
         };
419 
      }
 416 
      }
420 
 
 417 
 
421 
      pom=0x80;    // Zobrazeni pozice cary na displayi
 418 
      pom=0x80;    // Zobrazeni pozice cary na displayi
422 
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
 419 
      for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
423 
      disp(pom);
 420 
      disp(pom);