Subversion Repositories svnkaklik

Rev

Rev 248 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log

Rev Author Line No. Line
193 kakl 1 
// **** Objeti cihly vlevo **** LLLL
2 
 
226 kakl 3 
#define L_TOUCH 1    // Cara vlevo
4 
#define R_TOUCH 2    // Cata vpravo
5 
#define B_TOUCH 3    // Both
6 
 
221 kakl 7 
int8 n;
8 
int8 r1,r2,rr;
226 kakl 9 
int8 touch;
10 
enum okolo_cihly {pred_carou,na_care,po_care};
11 
okolo_cihly ridic;       // V jakem jsme stavu objizdeni cihly
228 kakl 12 
int8 vzdalenost;
247 kakl 13 
int8 visualisation;
193 kakl 14 
 
238 kakl 15 
   stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil)
235 kakl 16 
   touch=0;    // Indikator detekce cary pri objizdeni
17 
 
227 kakl 18 
cihla:
193 kakl 19 
 
238 kakl 20 
   rr=RR_CIHLA;   //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
227 kakl 21 
   disp(0x99);
22 
   set_pwm1_duty(0);    // zabrzdi levym kolem, prave vpred
239 kakl 23 
   set_pwm2_duty(200);
227 kakl 24 
   output_high(MOT_L);
25 
   output_low(MOT_R);
26 
   odocounter=get_timer1();
27 
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
226 kakl 28 
   {
227 kakl 29 
      cas=CASMIN-5;       // jeste vic nez hodne do leva
229 kakl 30 
 
238 kakl 31 
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
32 
      {
33 
         bum();
34 
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
239 kakl 35 
         brzda();
238 kakl 36 
         goto cihla; // Znovu zacni cihlu objizdet
37 
      };
38 
 
227 kakl 39 
      set_pwm1_duty(0);
249 kakl 40 
      set_pwm2_duty(220);     // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku
227 kakl 41 
      output_high(MOT_L);     // leve kolo reverz
42 
      output_low(MOT_R);      // prave kolo vpred
228 kakl 43 
      if(get_timer1()>(odocounter+5))  // konec zatacky?
227 kakl 44 
      {
45 
         disp(0x66);
46 
         break;
47 
      }
48 
      SetServoQ(cas);
49 
      delay_ms(18);
225 kakl 50 
   };
229 kakl 51 
 
227 kakl 52 
   //------ Objeti cihly v konstantni vzdalenosti ------
53 
   ridic=pred_carou;
228 kakl 54 
   cas=CASAVR-CASMIN;   // rovne
227 kakl 55 
   output_low(MOT_L);   // vpred
56 
   output_low(MOT_R);
247 kakl 57 
   visualisation=0;
227 kakl 58 
   while(true)
221 kakl 59 
   {
235 kakl 60 
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
225 kakl 61 
      {
238 kakl 62 
         bum();
235 kakl 63 
         SaveLog(log-1);      // Zapis Black Boxu do EEPROM
247 kakl 64 
         set_pwm1_duty(160);  // vpred
65 
         set_pwm2_duty(160);
246 kakl 66 
         output_low(MOT_L);
239 kakl 67 
         output_low(MOT_R);
249 kakl 68 
         cas=CASMIN;
235 kakl 69 
      };
236 kakl 70 
 
246 kakl 71 
      delta_bearing=bearing-bearing_offset;
247 kakl 72 
      visualisation=(delta_bearing & 0xF0) | (visualisation & 0x0F);
73 
      if(IRRX)  // hrozi celni srazka s cihlou v prubehu objizdeni
235 kakl 74 
      {
230 kakl 75 
         cas=CASMIN;
227 kakl 76 
      }
77 
      else
78 
      {
247 kakl 79 
         if((vzdalenost!=0)||!input(PROXIMITY)||((delta_bearing>60)&&(delta_bearing<128))) // Udrzovani konstantni vzdalenosti od cihly
230 kakl 80 
         {
81 
            if(cas>(CASMIN+20)) cas-=20;
82 
         }
83 
         else
84 
         {
85 
            if(cas<(CASMAX-20)) cas+=20;
86 
         };
225 kakl 87 
      };
227 kakl 88 
      // Elektronicky diferencial
89 
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
90 
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
91 
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
229 kakl 92 
 
227 kakl 93 
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
94 
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
229 kakl 95 
 
227 kakl 96 
      set_pwm1_duty(r1);   // Nastav rychlost motoru
97 
      set_pwm2_duty(r2);
229 kakl 98 
 
227 kakl 99 
      SetServoQ(cas);
229 kakl 100 
 
228 kakl 101 
      i2c_start();     // Sonar Ping
236 kakl 102 
      i2c_write(SONAR_ADR);
229 kakl 103 
      i2c_write(0x0);
104 
      i2c_write(0x52);  // mereni v us
228 kakl 105 
      i2c_stop();
106 
 
227 kakl 107 
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
225 kakl 108 
      {
227 kakl 109 
         set_adc_channel(LMAX);
110 
         delay_us(100);
111 
         if(read_adc()<THR) touch|=L_TOUCH;
112 
         set_adc_channel(RMAX);
113 
         delay_us(100);
114 
         if(read_adc()<THR) touch|=R_TOUCH;
225 kakl 115 
      };
228 kakl 116 
 
117 
      i2c_start();     // Odraz ze sonaru
236 kakl 118 
      i2c_write(SONAR_ADR);
229 kakl 119 
      i2c_write(0x3);
228 kakl 120 
      i2c_stop();
229 kakl 121 
      i2c_start();
236 kakl 122 
      i2c_write(SONAR_ADR+1);
229 kakl 123 
      vzdalenost=i2c_read(0);
228 kakl 124 
      i2c_stop();
125 
 
246 kakl 126 
      i2c_start();     // Cteni kompasu
127 
      i2c_write(COMPAS_ADR);
128 
      i2c_write(0x1);   // uhel 0-255
129 
      i2c_stop();
130 
      i2c_start();
131 
      i2c_write(COMPAS_ADR+1);
132 
      bearing=i2c_read(0);
133 
      i2c_stop();
134 
 
247 kakl 135 
      if(touch==L_TOUCH) visualisation|=0x2;
136 
      if(touch==R_TOUCH) visualisation|=0x1;
227 kakl 137 
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
138 
      if((ridic==na_care)&&(touch==0)) break;
139 
      if(ridic==na_care) touch=0;
247 kakl 140 
      disp(visualisation);
227 kakl 141 
   };
142 
   disp(0xC3);
229 kakl 143 
 
227 kakl 144 
   set_pwm1_duty(20);
145 
   set_pwm2_duty(200);
146 
   output_high(MOT_L);
147 
   output_low(MOT_R);
148 
   delay_us(40);
149 
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
150 
   while (true)  // Znovu se musime dotknout cary
151 
   {
152 
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
153 
      {
154 
         set_adc_channel(LMAX);    // Levy UV sensor
155 
         delay_us(100);
156 
         if(read_adc()<THR)   // Dotkli jsme se levym senzorem
157 
         {
158 
            disp(0xE0);
159 
            cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
160 
            goto cara;
161 
         };
162 
         set_adc_channel(RMAX);    // Pravy UV sensor
163 
         delay_us(100);
164 
         if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
165 
         {
166 
            disp(0x07);
167 
            cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
168 
            goto cara;
169 
         };
170 
      }
171 
      SetServoQ(CASMIN-5);   // max. max. doleva                 L
225 kakl 172 
   }
229 kakl 173 
 
226 kakl 174 
cara:
229 kakl 175 
 
227 kakl 176 
   output_low(MOT_L); // oba motory vpred
177 
   output_low(MOT_R);