Subversion Repositories svnkaklik

Rev

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