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