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

 //******** Robot Camerus pro IstRobot 2007 ************
-//"$Id: camerus.c 204 2007-03-21 09:14:22Z kakl $"
+//"$Id: camerus.c 207 2007-03-21 16:09:22Z kakl $"
 //*****************************************************
 #include ".\camerus.h"
 #USE FAST_IO (C)     // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery
 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)
+inline void disp(int8 x)
+{
    int n;
-   x=~x;
    for(n=0;n<=7;n++)
+   {
-      if (x & 1 == 1) output_high(DATA); else output_low(DATA);
+      if (x & 1 == 1) output_low(DATA); else output_high(DATA);
       output_high(CP);
       x>>=1;
       output_low(CP);
+   }
+}
    set_pwm1_duty(0);    // vypni PWM
    set_pwm2_duty(0);
    setup_ccp1(CCP_OFF);
    setup_ccp2(CCP_OFF);
-   for (n=0;n<100;n++)
+   for (n=0;n<200;n++)
+   {
       output_low(MOT_L);
       output_low(MOT_R);
       output_high(MOT_1);
       output_high(MOT_2);
-      delay_us(800);
+      delay_us(400);
       output_high(MOT_L);
       output_high(MOT_R);
       output_low(MOT_1);
       output_low(MOT_2);
-      delay_us(800);
+      delay_us(400);
+   }
    output_low(MOT_L); // smer vpred
    output_low(MOT_R);
    setup_ccp1(CCP_PWM); // RC1               // Zapni PWM pro motory
    setup_ccp2(CCP_PWM); // RC2
 #int_EXT
 EXT_isr()   // Preruseni od prekazky
+{
    set_pwm1_duty(0);    // zabrzdi
    set_pwm2_duty(0);
    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?
    set_pwm1_duty(0);    // reverz (zabrzdi)
    set_pwm2_duty(0);
    output_high(MOT_L);
    output_high(MOT_R);
 */
-   delay_ms(150);
+   delay_ms(200);
    brzda();
-   if(stav==pocihle) stav=jizda;//!!!!while(true); // Zastav na furt, konec drahy
+//!!!   if(stav==cihla) while(true); // Zastav na furt, konec drahy
+//   if(stav==cihla) return; // Po druhe nic neobjizdej
+// Pozor na rozjezd
    if(stav==jizda)   // Objed cihlu
+   {
       #include ".\objizdka_R.c"
+   }
 //---------------------------------- MAIN --------------------------------------
 void main()
+{
-   int8 offset;   // Promena pro ulozeni ovsetu
+   int8 offset;   // Promena pro ulozeni offsetu
    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
    setup_adc(ADC_CLOCK_INTERNAL);
    setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro mereni casu hrany W/B v radce
    setup_timer_1(T1_EXTERNAL);               // Cita pulzy z odometrie z praveho kola
    setup_comparator(NC_NC_NC_NC);
    setup_vref(FALSE);
    set_tris_c(0b11111001);     // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky
-   set_pwm1_duty(0);   // Zastav motory
+   set_pwm1_duty(0);    // Zastav motory
    set_pwm2_duty(0);
-   output_low(MOT_L);
+   output_low(MOT_L);   // Nastav smer vpred
    output_low(MOT_R);
    output_low(IRTX);   // Zapni LED na detekci prekazky
    NightRider(1);    // Aby se poznalo, ze byl RESET
-                     // Musi se pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
+                     // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy
    //... Nastaveni kamery ...
    i2c_start();      // Soft RESET kamery
    i2c_write(0xC0);        // Pro single slave musi mit vsechny zapisy adresu C0h
    i2c_write(0x12);        // Adresa registru COMH
    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
+   cas=CASAVR-CASMIN;   // Inicializace promenych, aby neslo servo za roh
-   stav=start;
+                        // a aby se to rozjelo jeste dneska
-   trasa=0;
+   stav=start;          // Jsme na startu
-   set_timer1(0);         // Vynuluj citac odometrie
+   set_timer1(0);       // Vynuluj citac odometrie
    // ........................... Hlavni smycka ................................
    while(true)
+   {
       int8 pom;
       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
+      {
       if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
 //      r1<<=1;     // Rychlost je dvojnasobna
 //      r2<<=1;     // Rozsah 2 az 184
+/* Nerozumna rychlost po cihle
-      if ((stav==cihla)&&(get_timer1()>(odocounter+5)))
+      if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti
+      {
          rr=rrold;
          stav=pocihle;
       };
+*/
-      if ((stav==jizda)||(stav==cihla)||(stav==pocihle))      // Jizda
+      if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(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
+      if((stav==rozjezd)&&(get_timer1()>10)) // musi ujet alespon 31cm
+      {
          ext_int_edge(H_TO_L);         // Nastav podminky preruseni od cihly
          INT0IF=0;                     // Zruseni predesle udalosti od startera
          enable_interrupts(INT_EXT);
          enable_interrupts(GLOBAL);
+         stav=jizda;
       };
       if(stav==start)         // Snimkuje, toci servem a ceka na start
+      {
          set_adc_channel(MODRA);    // Kroutitko na vystredeni predniho kolecka
             disp(0x80);
             while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc
             set_timer1(0);         // Vynuluj citac odometrie
             set_pwm1_duty(255);    // Rychly rozjezd  !!! Zkontrolovat na oscyloskopu
             set_pwm2_duty(255);
-            disp(0x1);
+            disp(0x01);
+            while(get_timer1()<=4) // Ujed alespon 12cm
+            {
+               set_adc_channel(LMAX);    // Levy UV sensor
-            delay_ms(300);
+               delay_us(40);
+               if(read_adc()<128) {cas=CASMIN; break;};  // Prejeli jsme caru vlevo
+               set_adc_channel(RMAX);    // Pravy UV sensor
+               delay_us(40);
+               if(read_adc()<128) {cas=CASMAX; break;};  // Prejeli jsme caru vpravo
+               cas=CASAVR-CASMIN;    // Cara je rovne
+            };
-            stav=jizda;
+            stav=rozjezd;
          };
+      }
       pom=0x80;    // Zobrazeni pozice cary na displayi
       for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1;

Subversion Repositories svnkaklik

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