Rev 818 Rev 829
Line 15... Line 15...
15 #define SDIN PIN_D4 // seriovy vstup 15 #define SDIN PIN_D4 // seriovy vstup
16 #define SDOUT input(PIN_B2) // seriovy vystup 16 #define SDOUT input(PIN_B2) // seriovy vystup
17 #define SCLK PIN_D5 // takt 17 #define SCLK PIN_D5 // takt
18   18  
19 // pro komunikaci s OLSA, prvni se posila LSB 19 // pro komunikaci s OLSA, prvni se posila LSB
20 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B 20 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
21 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F 21 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
22 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00 22 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
23 int test[8]={1,0,1,0,1,0,1,0,}; 23 int test[8]={1,0,1,0,1,0,1,0,};
24   24  
25 int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40 25 int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40
26 int mid_offset[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42 26 int mid_offset[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42
27 int right_offset[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44 27 int right_offset[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44
28 int offset[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81 28 int offset[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81
29   29  
30 int left_gain[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41 30 int left_gain[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41
31 int mid_gain[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43 31 int mid_gain[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43
32 int right_gain[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45 32 int right_gain[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45
33 int gain[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5 33 int gain[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5
34   34  
35 int start_int[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08 35 int start_int[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08
36 int stop_int[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10 36 int stop_int[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10
37 int readout[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02 37 int readout[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02
38   38  
39 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50) 39 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
40 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101) 40 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
41 int8 *line_lp=&olsa_lseg; // ukazatel na levou cast radky 41 int8 *line_lp=&olsa_lseg; // ukazatel na levou cast radky
42 int8 *line_rp=&olsa_rseg; // ukazatel na pravou cast radky 42 int8 *line_rp=&olsa_rseg; // ukazatel na pravou cast radky
43   43  
44 int8 pixel; // dec hodnota jednoho pixelu 44 int8 pixel_dec; // dec hodnota jednoho pixelu
45 int1 bpixel[8]={0}; // bin hodnota jednoho pixelu 45 int1 pixel_bin[8]={0}; // bin hodnota jednoho pixelu
46 int8 rpx; // pocet prectenych pixelu 46 int8 pixel_counter; // pocet prectenych pixelu
47 int8 rbit; // pocet prectenych bitu 47 int8 rbit; // pocet prectenych bitu
48 int8 cbit; // pocet prevedenych pixelu 48 int8 cbit; // pocet prevedenych pixelu
49   49  
50 //naraznik 50 //naraznik
51 #define BUMPL input(PIN_D6) 51 #define BUMPL input(PIN_D6)
Line 70... Line 70...
70   70  
71 //PODPROGRAMY 71 //PODPROGRAMY
72 //SENZORY 72 //SENZORY
73 //OLSA01A 73 //OLSA01A
74 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku 74 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
75 { 75 {
76 int8 ct; 76 int8 ct;
77 for(ct=0;ct<=count;ct++) 77 for(ct=0;ct<=count;ct++)
78 { 78 {
79 output_high(SCLK); 79 output_high(SCLK);
80 output_low(SCLK); 80 output_low(SCLK);
81 } 81 }
82 } 82 }
83   83  
84 void olsa_pulse() // vytvori jeden impulz 84 void olsa_pulse() // vytvori jeden impulz
85 { 85 {
86 output_high(SCLK); 86 output_high(SCLK);
87 output_low(SCLK); 87 output_low(SCLK);
88 } 88 }
89   89  
90 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy 90 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
91 { 91 {
92 int *ip; // ukazatel na pole s informaci 92 int *ip; // ukazatel na pole s informaci
93 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN 93 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
94 output_low(SDIN); // start bit 94 output_low(SDIN); // start bit
95 olsa_pulse(); 95 olsa_pulse();
96 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni 96 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
97 { 97 {
98 i=info[ip]; // ziskani hodnoty z pole 98 i=info[ip]; // ziskani hodnoty z pole
99 if(i==1) // vyhodnoceni obsahu informace - nastav 1 99 if(i==1) // vyhodnoceni obsahu informace - nastav 1
100 { 100 {
101 output_high(SDIN); 101 output_high(SDIN);
102 } 102 }
103 else // vyhodnoceni obsahu informace - nastav 0 103 else // vyhodnoceni obsahu informace - nastav 0
104 { 104 {
105 output_low(SDIN); 105 output_low(SDIN);
106 } -  
107 olsa_pulse(); -  
108 } 106 }
-   107 olsa_pulse();
-   108 }
109 output_high(SDIN); // stop bit 109 output_high(SDIN); // stop bit
110 olsa_pulse(); 110 olsa_pulse();
111 } 111 }
112   112  
113 void olsa_reset() // hlavni RESET - provadi se po zapnuti 113 void olsa_reset() // hlavni RESET - provadi se po zapnuti
114 { 114 {
115 output_low(SDIN); 115 output_low(SDIN);
116 output_low(SCLK); 116 output_low(SCLK);
117 olsa_pulses(30); // reset radkoveho senzoru 117 olsa_pulses(30); // reset radkoveho senzoru
118 output_high(SDIN); 118 output_high(SDIN);
119 olsa_pulses(10); // start bit - synchronizace 119 olsa_pulses(10); // start bit - synchronizace
120 olsa_send(main_reset); 120 olsa_send(main_reset);
121 olsa_pulses(5); 121 olsa_pulses(5);
122 olsa_send(set_mode_rg); 122 olsa_send(set_mode_rg);
123 olsa_send(clear_mode_rg); 123 olsa_send(clear_mode_rg);
124 } 124 }
125 125
126 void olsa_setup() // kompletni nastaveni, provadi se po resetu 126 void olsa_setup() // kompletni nastaveni, provadi se po resetu
127 { 127 {
128 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk) 128 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
129 olsa_send(offset); 129 olsa_send(offset);
130 olsa_send(left_gain); 130 olsa_send(left_gain);
131 olsa_send(gain); 131 olsa_send(gain);
132 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk) 132 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
Line 135... Line 135...
135 olsa_send(gain); 135 olsa_send(gain);
136 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk) 136 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
137 olsa_send(offset); 137 olsa_send(offset);
138 olsa_send(right_gain); 138 olsa_send(right_gain);
139 olsa_send(gain); 139 olsa_send(gain);
140 } 140 }
141 141
142 void olsa_integration() // snimani pixelu 142 void olsa_integration() // snimani pixelu
143 { 143 {
144 olsa_send(start_int); // zacatek integrace senzoru 144 olsa_send(start_int); // zacatek integrace senzoru
145 olsa_pulses(22); 145 olsa_pulses(22);
146 olsa_send(stop_int); // konec integrace senzoru 146 olsa_send(stop_int); // konec integrace senzoru
147 olsa_pulses(5); 147 olsa_pulses(5);
148 } -  
149   -  
150 void olsa_bit_save() // ukladani jednotlivych bitu pixelu -  
151 { -  
152 t_bit_save: -  
153 if(SDOUT==0) // zacatek prenosu -  
154 { -  
155 do // prijimej zpravy -  
156 { -  
157 if(SDOUT==1) // zapise do bitu 1 -  
158 { -  
159 bpixel[rbit]=1; -  
160 } -  
161 else // zapise do bitu 0 -  
162 { -  
163 bpixel[rbit]=0; -  
164 } -  
165 rbit++; // zapocita precteni bitu 0 - 7 -  
166 } -  
167 while(rbit==7); -  
168 goto e_bit_save; // skoc na konec prenosu -  
169 } -  
170 else // posli impulz a cekej na prenos -  
171 { -  
172 olsa_pulse(); -  
173 goto t_bit_save; // skoci na zacatek prenosu, pokud SDOUT == 1 -  
174 } -  
175 e_bit_save: -  
176 olsa_pulse(); // posle impulz pro generovani stop bitu -  
177 } -  
178 -  
179 void olsa_convert() // prevede pole prijmutych bitu na pixel -  
180 { -  
181 pixel=0; // vynuluje pixel -  
182 for(cbit=0;cbit<8;cbit++) -  
183 { -  
184 pixel|=bpixel[cbit]<<cbit; // prevadi bity do pixelu -  
185 } -  
186 } -  
187   148 }
188 void olsa_byte_save() // zapis pixelu do pole -  
189 { -  
190 if(rpx<=51) // leva polovina radky -  
191 { -  
192 olsa_lseg[line_lp]=pixel; // zapis do pole -  
193 line_lp++; -  
194 } -  
195 else // prava polovina radky -  
196 { -  
197 olsa_rseg[line_rp]=pixel; // zapis od pole -  
198 line_rp++; -  
199 } -  
200 } -  
201 -  
202 void olsa_evaluate() // vyhodnoceni polohy -  
203 { -  
204 } -  
205 -  
206 void olsa_read() // precist senzor -  
207 { -  
208 rpx=0; // cteny pixel = 0 -  
209 line_lp=0; // ukazatel na levou cast radky = 0 -  
210 line_rp=0; // ukazatel na pravou cast radky = 0 -  
211 olsa_send(readout); // prikaz pro cteni ze senzoru -  
212 do -  
213 { -  
214 olsa_bit_save(); // precte a ulozi bity -  
215 olsa_convert(); // prevede bity do jednoho bytu -  
216 olsa_byte_save(); // zapise byte do pole -  
217 rpx++; -  
218 } -  
219 while(rpx==101); -  
220 olsa_evaluate(); // zjisti pozici cary -  
221 } -  
222   149  
223 //ZACHRANNE SENZORY 150 //ZACHRANNE SENZORY
224 void read_blue_sensors() // cteni nouzovych senzoru 151 void read_blue_sensors() // cteni nouzovych senzoru
225 { 152 {
226 set_adc_channel(LINEL); // cti levy nouzovy senzor 153 set_adc_channel(LINEL); // cti levy nouzovy senzor
227 delay_us(10); 154 delay_us(10);
228 line_l=read_adc(); 155 line_l=read_adc();
229 set_adc_channel(LINER); // cti pravy nouzovy senzor 156 set_adc_channel(LINER); // cti pravy nouzovy senzor
230 delay_us(10); 157 delay_us(10);
231 line_r=read_adc(); 158 line_r=read_adc();
232 } 159 }
233 160
234 //PIPAK 161 //PIPAK
235 void beep(int16 period,int16 length) 162 void beep(int16 period,int16 length)
236 { 163 {
237 int16 bp; //promenna pro nastaveni delky 164 int16 bp; //promenna pro nastaveni delky
238 -  
239 for(bp=length;bp>0;bp--) 165 for(bp=length;bp>0;bp--)
240 { 166 {
241 output_high(SOUND_HI); 167 output_high(SOUND_HI);
242 output_low(SOUND_LO); 168 output_low(SOUND_LO);
243 delay_us(period); 169 delay_us(period);
244 output_high(SOUND_LO); 170 output_high(SOUND_LO);
245 output_low(SOUND_HI); 171 output_low(SOUND_HI);
246 delay_us(period); 172 delay_us(period);
247 } 173 }
248 } 174 }
249 //MOTORY 175 //MOTORY
250 void l_motor_fwd(int8 speedl) // levy motor dopredu 176 void l_motor_fwd(int8 speedl) // levy motor dopredu
251 { 177 {
252 output_high(LMF); 178 output_high(LMF);
253 output_low(LMB); 179 output_low(LMB);
254 set_pwm2_duty(speedl); 180 set_pwm2_duty(speedl);
255 } 181 }
256   182  
257 void l_motor_bwd(int8 speedl) // levy motor dozadu 183 void l_motor_bwd(int8 speedl) // levy motor dozadu
258 { 184 {
259 output_high(LMB); 185 output_high(LMB);
260 output_low(LMF); 186 output_low(LMF);
261 set_pwm2_duty(speedl); 187 set_pwm2_duty(speedl);
262 } 188 }
263   189  
264 void r_motor_fwd(int8 speedr) // pravy motor dopredu 190 void r_motor_fwd(int8 speedr) // pravy motor dopredu
265 { 191 {
266 output_high(RMF); 192 output_high(RMF);
267 output_low(RMB); 193 output_low(RMB);
268 set_pwm1_duty(speedr); 194 set_pwm1_duty(speedr);
269 } 195 }
270   196  
271 void r_motor_bwd(int8 speedr) // pravy motor dozadu 197 void r_motor_bwd(int8 speedr) // pravy motor dozadu
272 { 198 {
273 output_high(RMB); 199 output_high(RMB);
274 output_low(RMF); 200 output_low(RMF);
275 set_pwm1_duty(speedr); 201 set_pwm1_duty(speedr);
276 } 202 }
277   203  
278 void l_motor_off() // levy motor vypnut 204 void l_motor_off() // levy motor vypnut
279 { 205 {
280 output_low(LMF); 206 output_low(LMF);
281 output_low(LMB); 207 output_low(LMB);
282 set_pwm2_duty(0); 208 set_pwm2_duty(0);
283 } 209 }
284 210
285 void r_motor_off() // pravy motor vypnut 211 void r_motor_off() // pravy motor vypnut
286 { 212 {
287 output_low(RMF); 213 output_low(RMF);
288 output_low(RMB); 214 output_low(RMB);
289 set_pwm1_duty(0); 215 set_pwm1_duty(0);
290 } 216 }
291   217  
292 void motor_test() // test motoru 218 void motor_test() // test motoru
293 { 219 {
294 int8 i; 220 int8 i;
295 beep(100,200); 221 beep(100,200);
296 printf("TEST MOTORU\r\n"); 222 printf("TEST MOTORU\r\n");
297 delay_ms(1000); 223 delay_ms(1000);
298 printf("LEVY MOTOR DOPREDU\r\n"); 224 printf("LEVY MOTOR DOPREDU\r\n");
299 delay_ms(1000); 225 delay_ms(1000);
300 for(i=0;i<255;i++) 226 for(i=0;i<255;i++)
301 { 227 {
302 l_motor_fwd(i); 228 l_motor_fwd(i);
303 printf("RYCHLOST: %u\r\n",i); 229 printf("RYCHLOST: %u\r\n",i);
304 delay_ms(5); 230 delay_ms(5);
305 } 231 }
306 for(i=255;i>0;i--) 232 for(i=255;i>0;i--)
307 { 233 {
308 l_motor_fwd(i); 234 l_motor_fwd(i);
309 printf("RYCHLOST: %u\r\n",i); 235 printf("RYCHLOST: %u\r\n",i);
310 delay_ms(5); 236 delay_ms(5);
311 } 237 }
312 printf("LEVY MOTOR DOZADU\r\n"); 238 printf("LEVY MOTOR DOZADU\r\n");
313 delay_ms(1000); 239 delay_ms(1000);
314 for(i=0;i<255;i++) 240 for(i=0;i<255;i++)
315 { 241 {
316 l_motor_bwd(i); 242 l_motor_bwd(i);
317 printf("RYCHLOST: %u\r\n",i); 243 printf("RYCHLOST: %u\r\n",i);
318 delay_ms(5); 244 delay_ms(5);
319 } 245 }
320 for(i=255;i>0;i--) 246 for(i=255;i>0;i--)
321 { 247 {
322 l_motor_bwd(i); 248 l_motor_bwd(i);
323 printf("RYCHLOST: %u\r\n",i); 249 printf("RYCHLOST: %u\r\n",i);
324 delay_ms(5); 250 delay_ms(5);
325 } 251 }
326 printf("PRAVY MOTOR DOPREDU\r\n"); 252 printf("PRAVY MOTOR DOPREDU\r\n");
327 delay_ms(1000); 253 delay_ms(1000);
328 for(i=0;i<255;i++) 254 for(i=0;i<255;i++)
329 { 255 {
330 r_motor_fwd(i); 256 r_motor_fwd(i);
331 printf("RYCHLOST: %u\r\n",i); 257 printf("RYCHLOST: %u\r\n",i);
332 delay_ms(5); 258 delay_ms(5);
333 } 259 }
334 for(i=255;i>0;i--) 260 for(i=255;i>0;i--)
335 { 261 {
336 r_motor_fwd(i); 262 r_motor_fwd(i);
337 printf("RYCHLOST: %u\r\n",i); 263 printf("RYCHLOST: %u\r\n",i);
338 delay_ms(5); 264 delay_ms(5);
339 } 265 }
340 printf("PRAVY MOTOR DOZADU\r\n"); 266 printf("PRAVY MOTOR DOZADU\r\n");
341 delay_ms(1000); 267 delay_ms(1000);
342 for(i=0;i<255;i++) 268 for(i=0;i<255;i++)
343 { 269 {
344 r_motor_bwd(i); 270 r_motor_bwd(i);
345 printf("RYCHLOST: %u\r\n",i); 271 printf("RYCHLOST: %u\r\n",i);
346 delay_ms(5); 272 delay_ms(5);
347 } 273 }
348 for(i=255;i>0;i--) 274 for(i=255;i>0;i--)
349 { 275 {
350 r_motor_bwd(i); 276 r_motor_bwd(i);
351 printf("RYCHLOST: %u\r\n",i); 277 printf("RYCHLOST: %u\r\n",i);
352 delay_ms(5); 278 delay_ms(5);
353 } 279 }
354 l_motor_off(); 280 l_motor_off();
355 r_motor_off(); 281 r_motor_off();
356 printf("KONEC TESTU MOTORU\r\n"); 282 printf("KONEC TESTU MOTORU\r\n");
357 delay_ms(1000); 283 delay_ms(1000);
358 } 284 }
359   285  
360 void diagnostika() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru 286 void diagnostika() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
361 { 287 {
362 read_blue_sensors(); 288 read_blue_sensors();
363 printf("LEVA: %u \t",line_l); 289 printf("LEVA: %u \t",line_l);
Line 374... Line 300...
374 } 300 }
375 } 301 }
376   302  
377 // HLAVNI SMYCKA 303 // HLAVNI SMYCKA
378 void main() 304 void main()
379 { 305 {
380 -  
381 printf("POWER ON \r\n"); 306 printf("POWER ON \r\n");
382 // NASTAVENI > provede se pouze pri zapnuti 307 // NASTAVENI > provede se pouze pri zapnuti
383 setup_adc_ports(sAN0-sAN1-sAN2); 308 setup_adc_ports(sAN0-sAN1-sAN2);
384 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik 309 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
385 setup_spi(SPI_SS_DISABLED); 310 setup_spi(SPI_SS_DISABLED);
Line 394... Line 319...
394 r_motor_off(); // vypne pravy motor 319 r_motor_off(); // vypne pravy motor
395 olsa_reset(); // reset logiky radkoveho senzoru 320 olsa_reset(); // reset logiky radkoveho senzoru
396 olsa_setup(); // nastaveni segmentu radkoveho senzoru (offset a zisk) 321 olsa_setup(); // nastaveni segmentu radkoveho senzoru (offset a zisk)
397 output_high(LED1); // zhasne LED1 322 output_high(LED1); // zhasne LED1
398 output_high(LED2); // zhasne LED2 323 output_high(LED2); // zhasne LED2
-   324 olsa_reset();
-   325 olsa_setup();
399 beep(500,200); // pipni pri startu 326 beep(500,200); // pipni pri startu
400 printf("OK! \r\n"); 327 printf("OK! \r\n");
401 delay_ms(500); 328 delay_ms(500);
402 printf("VYBRAT MOD... \r\n"); 329 printf("VYBRAT MOD... \r\n");
403 -  
404 while(true) 330 while(true)
405 { 331 {
406 olsa_send(main_reset); 332 olsa_integration();
407 } -  
408 } 333 }
-   334 }