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

 //******** Robot Camerus pro IstRobot 2007 ************
-//"$Id: camerus.c 200 2007-03-18 20:54:40Z kakl $"
+//"$Id: camerus.c 204 2007-03-21 09:14:22Z kakl $"
 //*****************************************************
 #include ".\camerus.h"
 #USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 #define MOT_1     PIN_C1      // PWM vystpy motoru
 #define MOT_2     PIN_C2      //
 #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 / nove je to na RC0 na TIMER1
+                                // Jeden impuls je 31,25mm
 #define IRRX      PIN_B0      // Vstup INT, generuje preruseni pri prekazce
 #define IRTX      PIN_B3      // Modulovani vysilaci IR LED na detekci prekazky
 #define PROXIMITY PIN_C7      // Cidlo kratkeho dosahu na cihlu
 #define CASMIN 6           // Rozsah radku snimace
    #bit RBIE      = INTCON.3
    #bit TMR0IF    = INTCON.2
    #bit INT0IF    = INTCON.1
    #bit RBIF      = INTCON.0
-enum stavy {start,rozjezd,jizda,cihla,cil};
+enum stavy {start,rozjezd,jizda,cihla,pocihle,cil};
 stavy stav;       // Kde jsme na trati
 int8 cas;         // Cas hrany bila/cerna v radce
 int8 stred;       // Vystredeni kolecka
 int16 odocounter;  // Zaznamenani aktualniho stavu pocitadla odometrie
+int8 rr;       // Promenna na ulozeni Rozumne rychlost
+int8 rrold;
 // Zobrazeni jednoho byte na modulu LEDbar
 void disp(int8 x)
+{
    int n;
       output_low(SERVO);
       delay_ms(18);
+   }
+}
+inline void SetServoQ(int8 angle)
+{
+   output_high(SERVO);        // Odvysilani impuzu 1 az 2ms pro servo
+   delay_us(1000);
+   delay_us(stred);
+   delay_us(stred);
+   delay_us(stred);
+   delay_us(angle);
+   delay_us(angle);
+   output_low(SERVO);
+}
 #int_EXT
 EXT_isr()   // Preruseni od prekazky
+{
-   set_pwm1_duty(0);    // reverz (zabrzdi)
+   set_pwm1_duty(0);    // zabrzdi
    set_pwm2_duty(0);
-   output_high(MOT_L);  // vzad
+   output_high(MOT_L);
    output_high(MOT_R);
    // Ujistime se, ze prijaty signal je z naseho IR vysilace
    output_high(IRTX);   // Vypni LED na detekci prekazky
    delay_ms(2);
    if (!input(IRRX))    // stale nas signal?
+   {
       output_low(MOT_L); // neni odraz -> vpred
       output_low(MOT_R);
       return;
    };
+/*
    SetServo((CASAVR-CASMIN));   // rovne
    set_pwm1_duty(140);  // vpred
    set_pwm2_duty(140);
    output_low(MOT_L);
    output_low(MOT_R);
    odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
 //!!! while(true) {disp(MAKE8(get_timer1(),0)); delay_ms(10);};
    while(true)
+   {
       if(!input(PROXIMITY)) break; // Je cihla blizko?
       if(get_timer1()>(odocounter+7)) return; // nedojeli jsme k cihle, jed dal
    };
    set_pwm1_duty(0);    // reverz (zabrzdi)
    set_pwm2_duty(0);
    output_high(MOT_L);
    output_high(MOT_R);
+*/
    delay_ms(150);
    brzda();
-   if (stav==cihla) while(true); // Zastav na furt, konec drahy
+   if(stav==pocihle) stav=jizda;//!!!!while(true); // Zastav na furt, konec drahy
    if(stav==jizda)   // Objed cihlu
+   {
       #include ".\objizdka_R.c"
+   }
 //---------------------------------- MAIN --------------------------------------
 void main()
+{
    int8 offset;   // Promena pro ulozeni ovsetu
-   int8 rr;       // Promenna na ulozeni Rozumne rychlost
    int8 r1;       // Rychlost motoru 1
    int8 r2;       // Rychlost motoru 2
    int16 trasa;   // Pocitadlo ujete vzdalenosti
    setup_adc_ports(ALL_ANALOG);              // Zapnuti A/D prevodniku pro cteni kroutitek
    delay_ms(1000);   // Nech hodnotu chvili na displayi
    set_adc_channel(ZELENA);   // --- Kroutitko pro vykon motoru ---
    delay_ms(1);
    rr=read_adc()>>2; // 0-31  // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!!
+   rrold=rr;
    cas=CASAVR-CASMIN;  // Inicializace promenych, aby neslo servo za roh a aby se to rozjelo jeste dneska
    stav=start;
    trasa=0;
    set_timer1(0);         // Vynuluj citac odometrie
       int8 n;
 next_snap:
       pom=0;
+      disable_interrupts(GLOBAL);  //----------------------- Critical
       while(input(HREF));     // Preskoc 1. radku
       while(!input(HREF));    // Cekej nez se zacnou posilat pixely z 2. radky
       set_timer0(0);          // Vynuluj pocitadlo casu
       while(input(HREF))      // Po dobu vysilani radky cekej na hranu W/B
+      {
       // Elektronicky diferencial
       if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
       if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
       if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
+      enable_interrupts(GLOBAL);    //----------------------- Critical
       if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
       if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
 //      r1<<=1;     // Rychlost je dvojnasobna
 //      r2<<=1;     // Rozsah 2 az 184
+      if ((stav==cihla)&&(get_timer1()>(odocounter+5)))
+      {
+         rr=rrold;
+         stav=pocihle;
+      };
-      if ((stav==jizda)||(stav==cihla))      // Jizda
+      if ((stav==jizda)||(stav==cihla)||(stav==pocihle))      // Jizda
+      {
          set_pwm1_duty(r1);
          set_pwm2_duty(r2);
          trasa++;
+      }
       else
+      {
          set_pwm1_duty(0);       // Zastaveni
          set_pwm2_duty(0);
-      }
+      };
 //!!! trasu zrusit a predelat to na odometrii
       if((stav==jizda)&&(trasa>50)) // musi to alespon 1s jet
+      {
          ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
             disp(0x1);
             delay_ms(300);
             stav=jizda;
          };
+      }
       pom=0x80;    // Zobrazeni pozice cary na displayi
       for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;
       disp(pom);
-      while(true)
+      while(true) // Ve zbytku casu snimku cti krajni UV senzory
+      {
          set_adc_channel(LMAX);    // Levy UV sensor
          for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
          if(read_adc()<128) cas=CASMIN;
          set_adc_channel(RMAX);    // Pravy UV sensor
          for(n=0;n<20;n++) if(input(HREF)) goto next_snap;
          if(read_adc()<128) cas=CASMAX;
       };
+   }
+}

Subversion Repositories svnkaklik

(root)/roboti/istrobot/camerus/SW/876/camerus.c @ 255 – Rev 200 → 204