Rev Author Line No. Line
591 kakl 1 // **** Objeti cihly vlevo **** LLLL
2  
3 #define L_TOUCH 1 // Cara vlevo
4 #define R_TOUCH 2 // Cata vpravo
5 #define B_TOUCH 3 // Both
6  
7 int8 n;
8 int8 r1,r2,rr;
9 int8 touch;
10 enum okolo_cihly {pred_carou,na_care,po_care};
11 okolo_cihly ridic; // V jakem jsme stavu objizdeni cihly
12 int8 vzdalenost;
13 int8 visualisation;
14  
15 stav=cihla; // Dalsi prekazku uz nezaznamenavej (je to s velkou pravdepodobnosti cil)
16 odocounter=get_timer1();
17  
18 cihla:
19  
20 rr=RR_CIHLA; //!!! Rozumna rychlost pro objizdeni cihly (bylo by lepsi rychlost zvysovat) a pri detekci pohybu zase snizit
21 disp(0x99);
22 set_pwm1_duty(0); // zabrzdi levym kolem, prave vpred
23 set_pwm2_duty(255);
24 output_high(MOT_L);
25 output_low(MOT_R);
26 while(true) // Na zacatku se vyhni cihle, zatoc co muzes
27 {
28 cas=CASMIN-5; // jeste vic nez hodne do leva
29  
30 if(BUMPER) // Narazili jsme do cihly, musime couvnout!
31 {
32 bum();
33 SaveLog(); // Zapis Black Boxu do EEPROM
34 delay_ms(TUHOS); //!!! Zatuhle prevodovky
35 brzda();
36 goto cihla; // Znovu zacni cihlu objizdet
37 };
38  
39 set_pwm1_duty(0);
40 set_pwm2_duty(255); // !!! mozna prilis maly vykon pro rozjezd pro zatuhlou prevodovku
41 output_high(MOT_L); // leve kolo reverz
42 output_low(MOT_R); // prave kolo vpred
43 if(get_timer1()>(odocounter+5)) // konec zatacky?
44 {
45 disp(0x66);
46 break;
47 }
48 SetServoQ(cas);
49 delay_ms(18);
50 };
51  
52 //------ Objeti cihly v konstantni vzdalenosti ------
53 touch=0; // Indikator detekce cary pri objizdeni
54 ridic=pred_carou;
55 cas=CASAVR-CASMIN; // rovne
56 output_low(MOT_L); // vpred
57 output_low(MOT_R);
58 visualisation=0;
59 while(true)
60 {
61 if(BUMPER) // Narazili jsme do cihly, musime couvnout!
62 {
63 bum();
64 SaveLog(); // Zapis Black Boxu do EEPROM
65 delay_ms(TUHOS); //!!! Zatuhle prevodovky
66 set_pwm1_duty(160); // vpred
67 set_pwm2_duty(160);
68 output_low(MOT_L);
69 output_low(MOT_R);
70 cas=CASMIN;
71 };
72  
73 delta_bearing=bearing-bearing_offset;
74 visualisation=(delta_bearing & 0xF0) | (visualisation & 0x0F);
75 if(IRRX) // hrozi celni srazka s cihlou v prubehu objizdeni
76 {
77 cas=CASMIN;
78 }
79 else
80 {
81 if((vzdalenost!=0)||!input(PROXIMITY)||((delta_bearing>60)&&(delta_bearing<128))) // Udrzovani konstantni vzdalenosti od cihly
82 {
83 if(cas>(CASMIN+30)) cas-=30;
84 }
85 else
86 {
87 if(cas<(CASMAX-30)) cas+=30;
88 };
89 };
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
94  
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);
97  
98 //!!! pro zatuhle prevodovky
99 // r1<<=1; // Rychlost je dvojnasobna
100 // r2<<=1; // Rozsah 2 az 184 pro rr=0
101  
102 set_pwm1_duty(r1); // Nastav rychlost motoru
103 set_pwm2_duty(r2);
104  
105 SetServoQ(cas);
106  
107 i2c_start(); // Sonar Ping
108 i2c_write(SONAR_ADR);
109 i2c_write(0x0);
110 i2c_write(0x52); // mereni v us
111 i2c_stop();
112  
113 for(n=1;n<=90;n++) // 18ms testovani cary do dalsi korekce serva
114 {
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;
121 };
122  
123 i2c_start(); // Odraz ze sonaru
124 i2c_write(SONAR_ADR);
125 i2c_write(0x3);
126 i2c_stop();
127 i2c_start();
128 i2c_write(SONAR_ADR+1);
129 vzdalenost=i2c_read(0);
130 i2c_stop();
131  
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  
141 if(touch==L_TOUCH) visualisation|=0x2;
142 if(touch==R_TOUCH) visualisation|=0x1;
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;
146 disp(visualisation);
147 };
148 disp(0xC3);
149  
150 set_pwm1_duty(0); //!!! pred zatuhlejma prevodovkama tam bylo 20 a 200
151 set_pwm2_duty(255);
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
178 }
179  
180 cara:
181  
182 output_low(MOT_L); // oba motory vpred
183 output_low(MOT_R);