WebSVN – svnkaklik – Diff – /roboti/istrobot/camerus/SW/876/camerus.c

 #define MOT_L  PIN_B5      // Smer otaceni leveho motoru; druhy pol je RC2
 #define MOT_R  PIN_B6      // Smer otaceni praveho motoru; druhy pol je RC1
 #define DATA   PIN_B2      // K modulu LEDbar data
 #define CP     PIN_B1      // K modulu LEDbar hodiny
 #define ODO    PIN_A4      // Ze snimace z odometrie z praveho kola
+//#define CIHLA  PIN_B0      // Vstup INT, generuje preruseni pri prekazce
 #define CASMIN 6           // Rozsah radku snimace
 #define CASMAX 192
 #define CASAVR ((CASMAX+CASMIN) / 2)
+#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
+enum stavy {start,rozjezd,jizda,cihla,cil};
+stavy stav;    // Kde jsme na trati
+int8 cas;      // Cas hrany bila/cerna v radce
+void SetServo(int8 angle)
+{
+   int8 n, offset;
+   for(n=0; n<20; n++)
+   {
+      set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
+      Delay_ms(1);
+      offset=read_adc();
+      output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
+      delay_us(1000);
+      delay_us(offset);
+      delay_us(offset);
+      delay_us(offset);
+      delay_us(angle);
+      delay_us(angle);
+      output_low(SERVO);
+      delay_ms(18);
+   }
+}
+#int_EXT
+EXT_isr()   // Preruseni od prekazky
+{
+   set_pwm1_duty(0);    // reverz (zabrzdi)
+   set_pwm2_duty(0);
+   output_high(MOT_L);
+   output_high(MOT_R);
+   delay_ms(400);
+   output_low(MOT_L); // zastav
+   output_low(MOT_R);
+   if (stav==cihla) while(true); // Zastav na furt
+   if(stav==jizda)
+   {
+      int n;
+      SetServo(CASAVR-CASMIN);
+      set_pwm1_duty(40); // pomalu couvej
+      set_pwm2_duty(40);
+      output_high(MOT_L);
+      output_high(MOT_R);
+      n=0;
+      while(true)
+      {
+         while(input(ODO));
+         while(!input(ODO));
+         n++;
+         if(n==6) break;
+      }
+      set_pwm1_duty(255);    // reverz (zabrzdi)
+      set_pwm2_duty(255);
+      output_low(MOT_L);
+      output_low(MOT_R);
+      delay_ms(100);
+      set_pwm1_duty(0);  // Zastav
+      set_pwm2_duty(0);
+      output_low(MOT_L);
+      output_low(MOT_R);
+      delay_ms(1000);
+      SetServo((CASAVR-CASMIN)-20);   // doleva
+      set_pwm1_duty(150);  // vpred
+      set_pwm2_duty(200);
+      output_low(MOT_L);
+      output_low(MOT_R);
+      stav=cihla;
+      n=0;
+      while(true)
+      {
+         while(input(ODO));
+         while(!input(ODO));
+         n++;
+         if(n==10) break;
+      }
+      set_pwm1_duty(0);    // reverz (zabrzdi)
+      set_pwm2_duty(0);
+      output_high(MOT_L);
+      output_high(MOT_R);
+      SetServo((CASAVR-CASMIN));   // rovne
+      set_pwm1_duty(140);  // vpred
+      set_pwm2_duty(140);
+      output_low(MOT_L);
+      output_low(MOT_R);
+      stav=cihla;
+      n=0;
+      while(true)
+      {
+         while(input(ODO));
+         while(!input(ODO));
+         n++;
+         if(n==10) break;
+      }
+      set_pwm1_duty(0);    // reverz (zabrzdi)
+      set_pwm2_duty(0);
+      output_high(MOT_L);
+      output_high(MOT_R);
+      delay_ms(200);
+      set_pwm1_duty(150);  // Zastav
+      set_pwm2_duty(150);
+      output_high(MOT_L);
+      output_high(MOT_R);
+      delay_ms(1000);
+      SetServo(CASMIN);   // max. doleva
+      set_pwm1_duty(0);  // vzad
+      set_pwm2_duty(20);
+      output_low(MOT_L);
+      output_high(MOT_R);
+      stav=cihla;
+      n=0;
+      while(true)
+      {
+         while(input(ODO));
+         while(!input(ODO));
+         n++;
+         if(n==10) break;
+      }
+      set_pwm1_duty(0);    // reverz (zabrzdi)
+      set_pwm2_duty(255);
+      output_low(MOT_L);
+      output_low(MOT_R);
+      delay_ms(100);
+      set_pwm1_duty(0);  // Zastav
+      set_pwm2_duty(0);
+      output_low(MOT_L);
+      output_low(MOT_R);
+      delay_ms(500);
+      SetServo((CASAVR-CASMIN)+5);   // mirne doprava
+      set_pwm1_duty(180);  // vpred
+      set_pwm2_duty(180);
+      output_low(MOT_L);
+      output_low(MOT_R);
+      stav=cihla;
+      n=0;
+      while(true)
+      {
+         while(input(ODO));
+         while(!input(ODO));
+         n++;
+         if(n==10) break;
+      }
+      set_pwm1_duty(0);  // Zastav
+      set_pwm2_duty(0);
+      output_low(MOT_L);
+      output_low(MOT_R);
+      delay_ms(500);
+      cas=CASMIN;  // Cara je vlevo
+      stav=cihla;
+   }
+}
 // Zobrazeni jednoho byte na modulu LEDbar
 void disp(int8 x)
+{
    int n;
       output_low(CP);
+   }
+}
 // Blikani LEDbarem ve stilu Night Rider
-void NightRider(int x)
+void NightRider(int8 x)
+{
    int n,i,j;
    for(j=0;j<x;j++)
+   {
       i=0x01;
       for(n=0;n<7;n++)
+      {
          disp(i);
          rotate_left(&i, 1);
-         delay_ms(50);
+         delay_ms(40);
+      }
       for(n=0;n<7;n++)
+      {
          disp(i);
          rotate_right(&i, 1);
-         delay_ms(50);
+         delay_ms(40);
+      }
+   }
    disp(i);
-   delay_ms(30);
+   delay_ms(40);
    i=0;
    disp(i);
+}
 void main()
+{
-   int8 cas;      // Cas hrany bila/cerna v radce
    int8 offset;   // Promena pro ulozeni ovsetu
    int8 rr;       // Promenna na ulozeni Rozumne rychlost
    int8 r1;       // Rychlost motoru 1
    int8 r2;       // Rychlost motoru 2
-   enum stavy {start,rozjezd,jizda,cihla,cil};
-   stavy stav;    // Kde jsme na trati
    int16 trasa;   // Pocitadlo ujete vzdalenosti
    setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
    setup_adc(ADC_CLOCK_INTERNAL);
    setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
    NightRider(1);    // Musi se dat cas kamere na AGC a AEC
 { // Kalibrace kamery
    int8 t1,t2;
    for(offset=0;offset<=255;offset+=0x04) // Cita porad dokola
+   {
       i2c_start();      // Brightness
       i2c_write(0xC0);
       i2c_write(0x06);
       i2c_write(offset);  // 80h default
       i2c_stop();
       disp(offset);
-      delay_ms(100);
+      delay_ms(50);
       t1=0;
       t2=0;
       while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
       delay_ms(5);
       while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
       set_timer0(0);          // Vynuluj pocitadlo casu
       if(!input(PIX)) continue;
       while(input(PIX));
       t1=get_timer0();    // Precti cas z citace casu hrany
       set_timer0(0);          // Vynuluj pocitadlo casu
       while(!input(PIX));
       t2=get_timer0();
       if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break;
       delay_ms(2);
+   }
+}
 delay_ms(1000);
-   cas=128;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
+   cas=CASAVR-CASMIN;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
    stav=start;
    trasa=0;
    // ... Hlavni smycka ...
    while(true)
       while(!input(HREF));    // Cekej nez se zacnou posilat pixely z radky
       set_timer0(0);          // Vynuluj pocitadlo casu
       while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
+      {
 // !!!!Dodelat rozpoznani cerne cary napric pro zastaveni
-         if(!input(PIX))   // Pokud se 3x za sebou precetla CERNA
+         if(!input(PIX))   // Pokud se Xx za sebou precetla CERNA
          if(!input(PIX))
 //         if(!input(PIX))
+         {
             pom=get_timer0();    // Precti cas z citace casu hrany
             break;
       output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
       delay_us(1000);
       delay_us(offset);
       delay_us(offset);
+      delay_us(offset);
       delay_us(cas);
       delay_us(cas);
       output_low(SERVO);
       set_adc_channel(ZELENA);   // Kroutitko pro vykon motoru
       if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
 //      r1<<=1;     // Rychlost je dvojnasobna
 //      r2<<=1;     // Rozsah 2 az 184
-      if (stav==jizda)      // Jizda
+      if ((stav==jizda)||(stav==cihla))      // Jizda
+      {
          set_pwm1_duty(r1);
          set_pwm2_duty(r2);
          trasa++;
+      }
          set_pwm2_duty(0);
+      }
       set_adc_channel(DALKOMER);   // Prepni A/D prevodnik na detektor cihly
       Delay_ms(1);
-      for(n=0;n<200;n++)      // Detekce prekazky
+      if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
+      {
+         ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
+         INT0IF=0;                     // Zruseni predesle udalosti od startera
+         enable_interrupts(INT_EXT);
-         if(read_adc()<128)
+         enable_interrupts(GLOBAL);
-         {
+      };
-            if(stav==start)
+      if(stav==start)
+      if(read_adc()<128)
-            {
+      {
-              disp(0x55);
+         disp(0x55);
-              while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
+         while(read_adc()<128); // Cekej, dokud starter neda ruku pryc
-              set_pwm1_duty(255);
+         set_pwm1_duty(255);
-              set_pwm2_duty(255);
+         set_pwm2_duty(255);
-              disp(0xAA);
+         disp(0xAA);
-              delay_ms(500);
+         delay_ms(200);
-              stav=jizda;
+         stav=jizda;
-              break;
-            };
-            if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
-            {
-              set_pwm1_duty(0);
-              set_pwm2_duty(0);
-              output_high(MOT_L); // zabrzdi
-              output_high(MOT_R);
-              delay_ms(300);
-              output_low(MOT_L); // zastav
-              output_low(MOT_R);
-              stav=cil;
-              while(true);
-            };
-         }
+      }
       pom=0x80;
       for(n=CASMAX/8; n<cas; n+=CASMAX/8)
+      {
          pom>>=1;
+      }
       disp(pom);
    output_high(PIN_C0);
    delay_ms(1);
    output_low(PIN_C0);
    };

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 360 – Rev 181 → 184