Subversion Repositories svnkaklik

Rev

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