Subversion Repositories svnkaklik

Rev

Rev 246 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 246 Rev 247
Line 8... Line 8...
8 
int8 r1,r2,rr;
 8 
int8 r1,r2,rr;
9 
int8 touch;
 9 
int8 touch;
10 
enum okolo_cihly {pred_carou,na_care,po_care};
 10 
enum okolo_cihly {pred_carou,na_care,po_care};
11 
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
 11 
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
12 
int8 vzdalenost;
 12 
int8 vzdalenost;
- 
 
 13 
int8 visualisation;
13 
 
 14 
 
14 
   stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil)
 15 
   stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil)
15 
   touch=0;    // Indikator detekce cary pri objizdeni
 16 
   touch=0;    // Indikator detekce cary pri objizdeni
16 
 
 17 
 
17 
cihla:
 18 
cihla:
Line 51... Line 52...
51 
   //------ Objeti cihly v konstantni vzdalenosti ------
 52 
   //------ Objeti cihly v konstantni vzdalenosti ------
52 
   ridic=pred_carou;
 53 
   ridic=pred_carou;
53 
   cas=CASAVR-CASMIN;   // rovne
 54 
   cas=CASAVR-CASMIN;   // rovne
54 
   output_low(MOT_L);   // vpred
 55 
   output_low(MOT_L);   // vpred
55 
   output_low(MOT_R);
 56 
   output_low(MOT_R);
- 
 
 57 
   visualisation=0;
56 
   while(true)
 58 
   while(true)
57 
   {
 59 
   {
58 
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
 60 
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
59 
      {
 61 
      {
60 
         bum();
 62 
         bum();
61 
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
 63 
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
62 
         set_pwm1_duty(140);  // vpred
 64 
         set_pwm1_duty(160);  // vpred
63 
         set_pwm2_duty(140);
 65 
         set_pwm2_duty(160);
64 
         output_low(MOT_L);
 66 
         output_low(MOT_L);
65 
         output_low(MOT_R);
 67 
         output_low(MOT_R);
66 
      };
 68 
      };
67 
 
 69 
 
68 
      delta_bearing=bearing-bearing_offset;
 70 
      delta_bearing=bearing-bearing_offset;
- 
 
 71 
      visualisation=(delta_bearing & 0xF0) | (visualisation & 0x0F);
69 
      if(IRRX || ((delta_bearing>64)&&(delta_bearing<128))) // hrozi celni srazka s cihlou v prubehu objizdeni
 72 
      if(IRRX)  // hrozi celni srazka s cihlou v prubehu objizdeni
70 
      {
 73 
      {
71 
         cas=CASMIN;
 74 
         cas=CASMIN;
72 
      }
 75 
      }
73 
      else
 76 
      else
74 
      {
 77 
      {
75 
         if((vzdalenost!=0)||!input(PROXIMITY)) // Udrzovani konstantni vzdalenosti od cihly
 78 
         if((vzdalenost!=0)||!input(PROXIMITY)||((delta_bearing>60)&&(delta_bearing<128))) // Udrzovani konstantni vzdalenosti od cihly
76 
         {
 79 
         {
77 
            if(cas>(CASMIN+20)) cas-=20;
 80 
            if(cas>(CASMIN+20)) cas-=20;
78 
         }
 81 
         }
79 
         else
 82 
         else
80 
         {
 83 
         {
Line 126... Line 129...
126 
      i2c_start();
 129 
      i2c_start();
127 
      i2c_write(COMPAS_ADR+1);
 130 
      i2c_write(COMPAS_ADR+1);
128 
      bearing=i2c_read(0);
 131 
      bearing=i2c_read(0);
129 
      i2c_stop();
 132 
      i2c_stop();
130 
 
 133 
 
131 
      if(touch==L_TOUCH) disp(0xC0);
 134 
      if(touch==L_TOUCH) visualisation|=0x2;
132 
      if(touch==R_TOUCH) disp(0x03);
 135 
      if(touch==R_TOUCH) visualisation|=0x1;
133 
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
 136 
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
134 
      if((ridic==na_care)&&(touch==0)) break;
 137 
      if((ridic==na_care)&&(touch==0)) break;
135 
      if(ridic==na_care) touch=0;
 138 
      if(ridic==na_care) touch=0;
- 
 
 139 
      disp(visualisation);
136 
   };
 140 
   };
137 
   disp(0xC3);
 141 
   disp(0xC3);
138 
 
 142 
 
139 
   set_pwm1_duty(20);
 143 
   set_pwm1_duty(20);
140 
   set_pwm2_duty(200);
 144 
   set_pwm2_duty(200);