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);
}