Subversion Repositories svnkaklik

Rev

Details | 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)
256 kakl 16
   odocounter=get_timer1();
235 kakl 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
256 kakl 23
   set_pwm2_duty(255);
227 kakl 24
   output_high(MOT_L);
25
   output_low(MOT_R);
26
   while(true) // Na zacatku se vyhni cihle, zatoc co muzes
226 kakl 27
   {
254 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();
258 kakl 33
         SaveLog();      // Zapis Black Boxu do EEPROM
252 kakl 34
         delay_ms(TUHOS); //!!! Zatuhle prevodovky
239 kakl 35
         brzda();
238 kakl 36
         goto cihla; // Znovu zacni cihlu objizdet
37
      };
38
 
227 kakl 39
      set_pwm1_duty(0);
257 kakl 40
      set_pwm2_duty(200);     // !!! 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 ------
256 kakl 53
   touch=0;    // Indikator detekce cary pri objizdeni
227 kakl 54
   ridic=pred_carou;
228 kakl 55
   cas=CASAVR-CASMIN;   // rovne
227 kakl 56
   output_low(MOT_L);   // vpred
57
   output_low(MOT_R);
247 kakl 58
   visualisation=0;
227 kakl 59
   while(true)
221 kakl 60
   {
235 kakl 61
      if(BUMPER) // Narazili jsme do cihly, musime couvnout!
225 kakl 62
      {
238 kakl 63
         bum();
258 kakl 64
         SaveLog();      // Zapis Black Boxu do EEPROM
252 kakl 65
         delay_ms(TUHOS); //!!! Zatuhle prevodovky
247 kakl 66
         set_pwm1_duty(160);  // vpred
67
         set_pwm2_duty(160);
246 kakl 68
         output_low(MOT_L);
239 kakl 69
         output_low(MOT_R);
249 kakl 70
         cas=CASMIN;
235 kakl 71
      };
236 kakl 72
 
246 kakl 73
      delta_bearing=bearing-bearing_offset;
247 kakl 74
      visualisation=(delta_bearing & 0xF0) | (visualisation & 0x0F);
75
      if(IRRX)  // hrozi celni srazka s cihlou v prubehu objizdeni
235 kakl 76
      {
230 kakl 77
         cas=CASMIN;
227 kakl 78
      }
79
      else
80
      {
247 kakl 81
         if((vzdalenost!=0)||!input(PROXIMITY)||((delta_bearing>60)&&(delta_bearing<128))) // Udrzovani konstantni vzdalenosti od cihly
230 kakl 82
         {
257 kakl 83
            if(cas>(CASMIN+30)) cas-=30;
230 kakl 84
         }
85
         else
86
         {
257 kakl 87
            if(cas<(CASMAX-30)) cas+=30;
230 kakl 88
         };
225 kakl 89
      };
227 kakl 90
      // Elektronicky diferencial
91
      if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace
92
      if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;};   // pro rizeni rychlosti motoru
93
      if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92
229 kakl 94
 
227 kakl 95
      if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN);     // Neco jako nasobeni
96
      if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN);
229 kakl 97
 
252 kakl 98
//!!! pro zatuhle prevodovky
255 kakl 99
//      r1<<=1;     // Rychlost je dvojnasobna
100
//      r2<<=1;     // Rozsah 2 az 184 pro rr=0
252 kakl 101
 
227 kakl 102
      set_pwm1_duty(r1);   // Nastav rychlost motoru
103
      set_pwm2_duty(r2);
229 kakl 104
 
227 kakl 105
      SetServoQ(cas);
229 kakl 106
 
228 kakl 107
      i2c_start();     // Sonar Ping
236 kakl 108
      i2c_write(SONAR_ADR);
229 kakl 109
      i2c_write(0x0);
110
      i2c_write(0x52);  // mereni v us
228 kakl 111
      i2c_stop();
112
 
227 kakl 113
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
225 kakl 114
      {
227 kakl 115
         set_adc_channel(LMAX);
116
         delay_us(100);
117
         if(read_adc()<THR) touch|=L_TOUCH;
118
         set_adc_channel(RMAX);
119
         delay_us(100);
120
         if(read_adc()<THR) touch|=R_TOUCH;
225 kakl 121
      };
228 kakl 122
 
123
      i2c_start();     // Odraz ze sonaru
236 kakl 124
      i2c_write(SONAR_ADR);
229 kakl 125
      i2c_write(0x3);
228 kakl 126
      i2c_stop();
229 kakl 127
      i2c_start();
236 kakl 128
      i2c_write(SONAR_ADR+1);
229 kakl 129
      vzdalenost=i2c_read(0);
228 kakl 130
      i2c_stop();
131
 
246 kakl 132
      i2c_start();     // Cteni kompasu
133
      i2c_write(COMPAS_ADR);
134
      i2c_write(0x1);   // uhel 0-255
135
      i2c_stop();
136
      i2c_start();
137
      i2c_write(COMPAS_ADR+1);
138
      bearing=i2c_read(0);
139
      i2c_stop();
140
 
247 kakl 141
      if(touch==L_TOUCH) visualisation|=0x2;
142
      if(touch==R_TOUCH) visualisation|=0x1;
227 kakl 143
      if((touch==B_TOUCH)&&(ridic==pred_carou)) ridic=na_care;
144
      if((ridic==na_care)&&(touch==0)) break;
145
      if(ridic==na_care) touch=0;
247 kakl 146
      disp(visualisation);
227 kakl 147
   };
148
   disp(0xC3);
229 kakl 149
 
257 kakl 150
   set_pwm1_duty(0);    //!!! pred zatuhlejma prevodovkama tam bylo 20 a 200
151
   set_pwm2_duty(240);
227 kakl 152
   output_high(MOT_L);
153
   output_low(MOT_R);
154
   delay_us(40);
155
   odocounter=get_timer1();    // Poznamenej aktualni stav odometrie
156
   while (true)  // Znovu se musime dotknout cary
157
   {
158
      for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
159
      {
160
         set_adc_channel(LMAX);    // Levy UV sensor
161
         delay_us(100);
162
         if(read_adc()<THR)   // Dotkli jsme se levym senzorem
163
         {
164
            disp(0xE0);
165
            cas=CASAVR-CASMIN; // nastavime, ze cara je rovne
166
            goto cara;
167
         };
168
         set_adc_channel(RMAX);    // Pravy UV sensor
169
         delay_us(100);
170
         if((get_timer1()>=(odocounter+2)) && (read_adc()<THR)) // Pravym senzorem nesmime caru prejet!
171
         {
172
            disp(0x07);
173
            cas=CASMAX; // kdyz prejedem, tak nastavime, ze cara je vpravo
174
            goto cara;
175
         };
176
      }
177
      SetServoQ(CASMIN-5);   // max. max. doleva                 L
225 kakl 178
   }
229 kakl 179
 
226 kakl 180
cara:
229 kakl 181
 
227 kakl 182
   output_low(MOT_L); // oba motory vpred
183
   output_low(MOT_R);