Rev Author Line No. Line
504 kaklik 1 #include ".\main.h"
2  
3 #define KOLMO1 225 // predni kolecko sroubem dopredu
4 #define KOLMO2 30 // predni kolecko je hlavou sroubu dozadu
5 #define STRED 128 // sredni poloha zataceciho kolecka
6 #define BEAR1 10//10 // 3 stupne zataceni
7 #define BEAR2 25//25
8 #define BEAR3 45//45
9 #define R 100 // Rozumna rychlost
10 #define R17 200 // X nasobek rozumne rychlosti
11 #define PRED_CIHLOU 100 // rychlost pri dalkove detekci cihly
12 //#define L1 1 // cara vlevo
13 #define L2 2 // cara vlevo
14 #define L3 3 // cara vlevo
15 #define S 0 // cara mezi sensory
16 //#define R1 -1 // cara vpravo
17 #define R2 -2 // cara vpravo
18 #define R3 -3 // cara vpravo
19  
20 // servo
21 #define SERVO PIN_B5
22  
23 // kroutitka
24 #define CERVENA 4 // AN4
25 //#define CERNA 5 // AN5
26 //#define ZELENA 6 // AN6
27 #define MODRA 2 // AN2
28  
29 // IR
30 #define IRTX PIN_B2
31 #define CIHLA PIN_A3
32  
33 //motory
34 #define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred
35 #define FL output_low(PIN_A1); output_high(PIN_A0)
36 #define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad
37 #define BL output_low(PIN_A0); output_high(PIN_A1)
38 #define STOPR output_low(PIN_A6);output_low(PIN_A7)
39 #define STOPL output_low(PIN_A0);output_low(PIN_A1)
40  
41 //HID
42 #define LED1 PIN_B1 //oranzova
43 #define LED2 PIN_B2 //zluta
44  
45 #define STROBE PIN_B0
46 //#define SW1 PIN_A2 // Motory On/off
47  
48 unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru
49 signed int8 line = S; // na ktere strane byla detekovana cara
50 //unsigned int8 dira; // pocita dobu po kterou je ztracena cara
51 unsigned int8 uhel; // urcuje aktualni uhel zataceni
52 unsigned int8 speed; // maximalni povolena rychlost
53 unsigned int8 turn; // rychlost toceni
54 unsigned int8 rovinka; // pocitadlo na zjisteni rovinky
55  
56 signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem
57 signed int16 Rmotor; // a pravem motoru
58  
59 // makro pro PWM pro motory
60 #define GO(motor, direction, power) if(get_timer0()<=power) \
61 {direction##motor;} else {stop##motor;}
62 ////////////////////////////////////////////////////////////////////////////////
63 #int_TIMER2
64 TIMER2_isr() // ovladani serva
65 {
66 unsigned int8 n;
67  
68 output_high(SERVO);
69 delay_us(1000);
70 for(n=uhel; n>0; n--) Delay_us(2);
71 output_low(SERVO);
72 }
73  
74 ////////////////////////////////////////////////////////////////////////////////
75 int8 IRcheck() // potvrdi detekci cihly
76 {
77 output_high(IRTX); // vypne vysilac IR
78 delay_ms(10);
79  
80 output_low(STROBE);
81 sensors = spi_read(0); // cteni senzoru
82 sensors=~sensors;
83 output_high(STROBE);
84  
85 if(bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal
86 {
87 output_low(IRTX); // zapne vysilac IR
88 delay_ms(10);
89  
90 output_low(STROBE);
91 sensors = spi_read(0); // cteni senzoru
92 sensors=~sensors;
93 output_high(STROBE);
94  
95 if(!bit_test(sensors,7)) // otestuje, jestli je detekovana cihla
96 {
97 output_high(IRTX); // vypne vysilac IR
98 delay_ms(10);
99  
100 output_low(STROBE);
101 sensors = spi_read(0); // cteni senzoru
102 sensors=~sensors;
103 output_high(STROBE);
104  
105 output_low(IRTX); // zapne vysilac IR
106  
107 if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla
108 }
109 };
110 output_low(IRTX); // zapne vysilac IR
111 return 0; // vrat 0, kdyz je detekovano ruseni
112 }
113 ////////////////////////////////////////////////////////////////////////////////
114 #include ".\objizdka_centrovani_R.c"
115 ////////////////////////////////////////////////////////////////////////////////
116 void main()
117 {
118  
119 unsigned int8 n;
120 unsigned int8 i,j;
121 unsigned int8 last_sensors;
122 unsigned int8 RozumnaRychlost;
123  
124 setup_adc_ports(sAN5|sAN2|sAN4|sAN6|VSS_VDD); // AD pro kroutitka
125 setup_adc(ADC_CLOCK_INTERNAL);
126 setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_16);
127 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
128 setup_timer_1(T1_DISABLED|T1_DIV_BY_8);
129 setup_timer_2(T2_DIV_BY_16,140,16);
130 setup_oscillator(OSC_8MHZ|OSC_INTRC);
131  
132 STOPR; STOPL; // zastav motory
133 Lmotor=0;Rmotor=0;
134  
135 uhel = STRED; // nastav zadni kolecko na stred
136 rovinka = 0;
137  
138 enable_interrupts(INT_TIMER2);
139 enable_interrupts(GLOBAL);
140  
141 output_low(IRTX); // zapni IR vysilac
142  
143 delay_ms(2000); // musime pockat na diagnostiku slave CPU
144  
145 //nastaveni rychlosti
146 set_adc_channel(CERVENA);
147  
148 Delay_ms(1);
149 RozumnaRychlost=R+(read_adc()>>2); // rychlost rovne +63; kroutitko dava 0-63
150 set_adc_channel(MODRA);
151 Delay_ms(1);
152 turn=speed-32+(read_adc()>>2); // rychlost toceni +-32; kroutitko dava 0-63
153  
154 speed=R17;
155  
156 while(true)
157 {
158  
159 GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory PWM podle promenych Lmotor a Rmotor
160  
161 delay_us(2000); // cekani na SLAVE, nez pripravi data od cidel
162  
163 last_sensors=sensors;
164  
165 output_low(STROBE); // vypni zobrazovani na posuvnem registru
166 sensors = spi_read(0); // cteni senzoru
167 sensors=~sensors; // neguj prijata data
168 output_high(STROBE); // zobraz data na posuvnem registru
169  
170 i=0; // havarijni kod
171 for (n=0; n<=6; n++)
172 {
173 if(bit_test(sensors,n)) i++;
174 }
175 if (i>4) // zastavi, kdyz je cerno pod vice nez tremi cidly
176 {
177 BL; BR;
178 delay_ms(300);
179 STOPR; STOPL;
180 While(true);
181 };
182  
183 /*
184 if (!input(CIHLA)) // dalkova detekce cihly
185 {
186 speed=PRED_CIHLOU;
187 }
188 else
189 {
190 speed=RozumnaRychlost;
191 }
192 */
193 if (bit_test(sensors,7)) // detekce cihly
194 {
195 BR;BL;
196 Delay_ms(400);
197 STOPR;STOPL;
198 // if (1==IRcheck()) // kontrola, jestli nebylo rusene cidlo
199 {
200 Delay_ms(100);
201 cikcak();
202 delay_ms(100);
203 objizdka(); // objede cihlu
204 }
205 }
206  
207 if (speed > RozumnaRychlost) speed--; // postupne zpomaleni na Roz. Rychl.
208  
209 if(bit_test(sensors,3)) //...|...//
210 {
211 uhel=STRED;
212 Lmotor=speed;
213 Rmotor=speed;
214 line=S;
215 if (rovinka < 255) rovinka++;
216 continue;
217 }
218  
219 if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
220 {
221 uhel=STRED - BEAR3;
222 Lmotor=0;
223 Rmotor=turn;
224 line=L3;
225 continue;
226 }
227  
228 if(bit_test(sensors,6)) //......|//
229 {
230 uhel=STRED + BEAR3;
231 Rmotor=0;
232 Lmotor=turn;
233 line=R3;
234 continue;
235 }
236  
237 if(bit_test(sensors,1)) //.|.....//
238 {
239 uhel=STRED - BEAR2;
240 Lmotor=speed-70;
241 Rmotor=speed;
242 line=L2;
243 continue;
244 }
245  
246 if(bit_test(sensors,5)) //.....|.//
247 {
248 uhel=STRED + BEAR2;
249 Rmotor=speed-70;
250 Lmotor=speed;
251 line=R2;
252 continue;
253 }
254  
255 if (bit_test(sensors,2)) //..|....//
256 {
257 uhel=STRED - BEAR1;
258 Lmotor=speed-20;
259 Rmotor=speed;
260 line=L2;
261 if (rovinka<255) rovinka++;
262 continue;
263 }
264  
265 if (bit_test(sensors,4)) //....|..//
266 {
267 uhel=STRED + BEAR1;
268 Rmotor=speed-20;
269 Lmotor=speed;
270 line=L2;
271 if (rovinka<255) rovinka++;
272 continue;
273 }
274  
275  
276 if ((L3==line) || (R3==line)) // Brzdeni pri vyjeti z trate
277 {
278 if (rovinka>50)
279 {
280 BL; BR;
281 Delay_ms(100);
282 if (rovinka > 250 || speed > 170) delay_ms(50);
283 };
284 rovinka=0;
285 speed=R17;
286 };
287 }
288 }