Subversion Repositories svnkaklik

Rev

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

Rev 224 Rev 226
Line 1... Line 1...
1 
//********* Robot Camerus pro IstRobot 2007 ************
 1 
//********* Robot Camerus pro IstRobot 2007 ************
2 
//"$Id: camerus.c 224 2007-04-06 19:56:01Z kakl $"
 2 
//"$Id: camerus.c 226 2007-04-08 08:03:04Z 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 33... Line 33...
33 
#define CASMAX 192
 33 
#define CASMAX 192
34 
#define CASAVR ((CASMAX+CASMIN) / 2)
 34 
#define CASAVR ((CASMAX+CASMIN) / 2)
35 
 
 35 
 
36 
#define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
 36 
#define OFFSETO   0x9F //0x9F         // Vystredeni serva pro objeti prekazky
37 
 
 37 
 
38 
#define THR_H 128
 38 
#define THR 90    // Threshold pro UV cidla na caru
39 
#define THR_L 90
 - 
 
40 
 
 39 
 
41 
#byte INTCON      = 0x0B         // Interrupt configuration register
 40 
#byte INTCON      = 0x0B         // Interrupt configuration register
42 
   #bit GIE       = INTCON.7
 41 
   #bit GIE       = INTCON.7
43 
   #bit PEIE      = INTCON.6
 42 
   #bit PEIE      = INTCON.6
44 
   #bit TMR0IE    = INTCON.5
 43 
   #bit TMR0IE    = INTCON.5
Line 153... Line 152...
153 
   delay_us(angle);
 152 
   delay_us(angle);
154 
   delay_us(angle);
 153 
   delay_us(angle);
155 
   output_low(SERVO);
 154 
   output_low(SERVO);
156 
}
 155 
}
157 
 
 156 
 
- 
 
 157 
//---------------------------- INT --------------------------------
158 
#int_EXT
 158 
#int_EXT
159 
EXT_isr()   // Preruseni od prekazky
 159 
EXT_isr()   // Preruseni od prekazky
160 
{
 160 
{
161 
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
 161 
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vypni
162 
   set_pwm2_duty(0);
 162 
   set_pwm2_duty(0);
Line 170... Line 170...
170 
      output_low(MOT_L); // je odraz -> vpred
 170 
      output_low(MOT_L); // je odraz -> vpred
171 
      output_low(MOT_R);
 171 
      output_low(MOT_R);
172 
      return;
 172 
      return;
173 
   };
 173 
   };
174 
   output_low(IRTX);    // Zapni LED na detekci prekazky
 174 
   output_low(IRTX);    // Zapni LED na detekci prekazky
175 
   delay_ms(5);
 175 
   delay_ms(10);
176 
   if (input(IRRX))     // stale nas signal?
 176 
   if (input(IRRX))     // stale nas signal?
177 
   {
 177 
   {
178 
      output_low(MOT_L); // neni odraz -> vpred
 178 
      output_low(MOT_L); // neni odraz -> vpred
179 
      output_low(MOT_R);
 179 
      output_low(MOT_R);
180 
      return;
 180 
      return;
Line 424... Line 424...
424 
            disp(0x01);
 424 
            disp(0x01);
425 
            while(get_timer1()<=4) // Ujed alespon 12cm
 425 
            while(get_timer1()<=4) // Ujed alespon 12cm
426 
            {
 426 
            {
427 
               set_adc_channel(LMAX);    // Levy UV sensor
 427 
               set_adc_channel(LMAX);    // Levy UV sensor
428 
               delay_us(40);
 428 
               delay_us(40);
429 
               if(read_adc()<THR_L) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
 429 
               if(read_adc()<THR) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
430 
               set_adc_channel(RMAX);    // Pravy UV sensor
 430 
               set_adc_channel(RMAX);    // Pravy UV sensor
431 
               delay_us(40);
 431 
               delay_us(40);
432 
               if(read_adc()<THR_L) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
 432 
               if(read_adc()<THR) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
433 
               cas=CASAVR-CASMIN;    // Cara je rovne
 433 
               cas=CASAVR-CASMIN;    // Cara je rovne
434 
            };
 434 
            };
435 
            stav=rozjezd;
 435 
            stav=rozjezd;
436 
         };
 436 
         };
437 
      }
 437 
      }
Line 442... Line 442...
442 
 
 442 
 
443 
      while(true) // Ve zbytku casu snimku cti krajni UV senzory
 443 
      while(true) // Ve zbytku casu snimku cti krajni UV senzory
444 
      {
 444 
      {
445 
         set_adc_channel(LMAX);    // Levy UV sensor
 445 
         set_adc_channel(LMAX);    // Levy UV sensor
446 
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
 446 
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
447 
         if(read_adc()<THR_L) cas=CASMIN;
 447 
         if(read_adc()<THR) cas=CASMIN;
448 
         set_adc_channel(RMAX);    // Pravy UV sensor
 448 
         set_adc_channel(RMAX);    // Pravy UV sensor
449 
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
 449 
         for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
450 
         if(read_adc()<THR_L) cas=CASMAX;
 450 
         if(read_adc()<THR) cas=CASMAX;
451 
      };
 451 
      };
452 
   }
 452 
   }
453 
}
 453 
}