Subversion Repositories svnkaklik

#include "laserus.h"

#define SERVO PIN_B5    // Vystup pro rizeni serva

#define LASER PIN_B4    // Vstup pro cteni laseru

#define MOT_DIR_L PIN_C0    // Rizeni smeru otaceni motoru

#define MOT_DIR_R PIN_C3

// kroutitka

#define  CERVENA  0  // AN0

#define  CERNA    1  // AN1

//#define  ZELENA   3  // AN3

//#define  MODRA    4  // AN4

#byte INTCON      = 0x0B         // Interrupt configuration register

   #bit GIE       = INTCON.7

   #bit PEIE      = INTCON.6

   #bit TMR0IE    = INTCON.5

   #bit INT0IE    = INTCON.4

   #bit RBIE      = INTCON.3

   #bit TMR0IF    = INTCON.2

   #bit INT0IF    = INTCON.1

   #bit RBIF      = INTCON.0

boolean edge;

int8 uhel, olduhel;

int16 uhel16;

#int_EXT

EXT_isr()

{

   int n, t1, t2, t;

   set_timer0(0);          // Vynulovani casovace pro 2ms

   output_high(SERVO);

   while(get_timer0()<(1000/256));    // Ceka 1ms

   for(n=uhel; n>0; n--) delay_us(3); // Sirka impulzu podle uhlu

   output_low(SERVO);

   while(get_timer0()<(3000/256)); // Ceka do 3ms, nez zacne scanovat

   set_timer0(0);          // Vynulovani casovace pro zjisteni polohy cary

   if(edge)                // Zrcatko prejizdelo tam nebo zpet?

   {

     edge=false;

     ext_int_edge(H_TO_L);      // Pristi inerrupt bude od opacne hrany

     INT0IF=0;                  // Povoleni dalsiho preruseni

     enable_interrupts(INT_EXT);

     enable_interrupts(GLOBAL);

//     do

     {

       while(0==input(LASER));  // Ceka na detekci cary

//   output_high(SERVO);

       t1=get_timer0();         // Poznamena cas nabezne hrany

       while(1==input(LASER));  // Ceka az zkonci cara

//   output_low(SERVO);

       t2=get_timer0();         // Poznamena cas sestupne hrany

       t=t2-t1;

     } while((t<3) || (t>20)); // Cara je detekovana, kdyz trva mezi xx ms

//     if ((t>3) && (t<20)) uhel=(160-(read_adc()>>2))-t1;

     if (abs(olduhel-uhel)<40)

     {

       uhel=(160-(read_adc()>>2))-t1;

       olduhel=uhel;

     };

//     uhel16=uhel;

//     set_pwm1_duty(250-(uhel16<<2));    // Elektronicky diferencial

//     set_pwm2_duty((uhel16));

     set_adc_channel(CERNA);

   }

   else

   {

     edge=true;

     ext_int_edge(L_TO_H);      // Pristi inerrupt bude od opacne hrany

     INT0IF=0;                  // Povoleni dalsiho preruseni

     enable_interrupts(INT_EXT);

     enable_interrupts(GLOBAL);

//     do

     {

       while(0==input(LASER));  // Ceka na detekci cary

       t1=get_timer0();         // Poznamena cas nabezne hrany

       while(1==input(LASER));  // Ceka az zkonci cara

       t2=get_timer0();         // Poznamena cas sestupne hrany

       t=t2-t1;

     } while((t<3) || (t>20)); // Cara je detekovana, kdyz trva mezi xx ms

     if (abs(olduhel-uhel)<40)

     {

//       uhel=((read_adc()>>2)+32)+t1;

//       olduhel=uhel;

     };

     set_adc_channel(CERVENA);

   }

   while(true);

}

void main()

{

   setup_adc_ports(ALL_ANALOG);  // Analogove vstupy pro cteni trimru

   setup_adc(ADC_CLOCK_INTERNAL);

   setup_spi(FALSE);

   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_256); // Casovac pro cteni laseru

   setup_timer_1(T1_DISABLED);

   setup_timer_2(T2_DIV_BY_4,255,1); // Casovac pro PWM cca 900Hz

   setup_ccp1(CCP_PWM); // Nastaveni PWM pro diferencial RC1, RC2

   setup_ccp2(CCP_PWM);

   set_pwm1_duty(90);    // Zastaveni PWM

   set_pwm2_duty(90);

   delay_ms(100);

   set_adc_channel(CERVENA);

   Delay_ms(1);

   ext_int_edge(L_TO_H);

   edge=true;

   uhel=((read_adc()>>2)+32)+30;

   olduhel=uhel;

   enable_interrupts(INT_EXT);

   enable_interrupts(GLOBAL);

   delay_ms(1000);

   output_high(MOT_DIR_L);     // Oba motory vpred

   output_high(MOT_DIR_R);

while(true);

}